def drawFootstepPlan(self, msg, folder, left_color=None, right_color=None, alpha=1.0):
        for step in folder.children():
            om.removeFromObjectModel(step)
        allTransforms = []
        volFolder = getWalkingVolumesFolder()
        map(om.removeFromObjectModel, volFolder.children())
        slicesFolder = getTerrainSlicesFolder()
        map(om.removeFromObjectModel, slicesFolder.children())


        for i, footstep in enumerate(msg.footsteps):
            trans = footstep.pos.translation
            trans = [trans.x, trans.y, trans.z]
            quat = footstep.pos.rotation
            quat = [quat.w, quat.x, quat.y, quat.z]

            footstepTransform = transformUtils.transformFromPose(trans, quat)

            allTransforms.append(footstepTransform)


            if i < 2:
                continue

            if footstep.is_right_foot:
                mesh = getRightFootMesh()
                if (right_color is None):
                    color = getRightFootColor()
                else:
                    color = right_color
            else:
                mesh = getLeftFootMesh()
                if (left_color is None):
                    color = getLeftFootColor()
                else:
                    color = left_color

            # add gradual shading to steps to indicate destination
            frac = float(i)/ float(msg.num_steps-1)
            this_color = [0,0,0]
            this_color[0] = 0.25*color[0] + 0.75*frac*color[0]
            this_color[1] = 0.25*color[1] + 0.75*frac*color[1]
            this_color[2] = 0.25*color[2] + 0.75*frac*color[2]


            if self.show_contact_slices:
                self.drawContactVolumes(footstepTransform, color)

            contact_pts_left, contact_pts_right = FootstepsDriver.getContactPts()
            if footstep.is_right_foot:
                sole_offset = np.mean(contact_pts_right, axis=0)
            else:
                sole_offset = np.mean(contact_pts_left, axis=0)

            t_sole_prev = frameFromPositionMessage(msg.footsteps[i-2].pos)
            t_sole_prev.PreMultiply()
            t_sole_prev.Translate(sole_offset)
            t_sole = transformUtils.copyFrame(footstepTransform)
            t_sole.Translate(sole_offset)
            yaw = np.arctan2(t_sole.GetPosition()[1] - t_sole_prev.GetPosition()[1],
                             t_sole_prev.GetPosition()[0] - t_sole.GetPosition()[0])
            T_terrain_to_world = transformUtils.frameFromPositionAndRPY([t_sole_prev.GetPosition()[0], t_sole_prev.GetPosition()[1], 0],
                                                                        [0, 0, math.degrees(yaw)])
            path_dist = np.array(footstep.terrain_path_dist)
            height = np.array(footstep.terrain_height)
            # if np.any(height >= trans[2]):
            terrain_pts_in_local = np.vstack((path_dist, np.zeros(len(footstep.terrain_path_dist)), height))
            d = DebugData()
            for j in range(terrain_pts_in_local.shape[1]-1):
                d.addLine(terrain_pts_in_local[:,j], terrain_pts_in_local[:,j+1], radius=0.01)
            obj = vis.showPolyData(d.getPolyData(), 'terrain slice', parent=slicesFolder, visible=slicesFolder.getProperty('Visible'), color=[.8,.8,.3])
            obj.actor.SetUserTransform(T_terrain_to_world)

            renderInfeasibility = False
            if renderInfeasibility and footstep.infeasibility > 1e-6:
                d = DebugData()
                start = allTransforms[i-1].GetPosition()
                end = footstepTransform.GetPosition()
                d.addArrow(start, end, 0.02, 0.005,
                           startHead=True,
                           endHead=True)
                vis.showPolyData(d.getPolyData(), 'infeasibility %d -> %d' % (i-2, i-1), parent=folder, color=[1, 0.2, 0.2])

            stepName = 'step %d' % (i-1)

            obj = vis.showPolyData(mesh, stepName, color=this_color, alpha=alpha, parent=folder)
            obj.setIcon(om.Icons.Feet)
            frameObj = vis.showFrame(footstepTransform, stepName + ' frame', parent=obj, scale=0.3, visible=False)
            obj.actor.SetUserTransform(footstepTransform)
            obj.addProperty('Support Contact Groups', footstep.params.support_contact_groups, attributes=om.PropertyAttributes(enumNames=['Whole Foot', 'Front 2/3', 'Back 2/3']))
            obj.properties.setPropertyIndex('Support Contact Groups', 0)
            obj.footstep_index = i
            obj.footstep_property_callback = obj.properties.connectPropertyChanged(functools.partial(self.onFootstepPropertyChanged, obj))

            self.drawContactPts(obj, footstep, color=this_color)
dataset/ETHPy150Open RobotLocomotion/director/src/python/director/footstepsdriver.py/FootstepsDriver.drawFootstepPlan/swapped operands of `-`