Python DebugData.addArrow方法代码示例

# 需要导入模块: from ddapp.debugVis import DebugData [as 别名]
# 或者: from ddapp.debugVis.DebugData import addArrow [as 别名]
def newMesh():

    d = DebugData()
    d.addArrow((0,0,0), (0,0,0.3))
    pd = d.getPolyData()
    meshId = affordanceitems.MeshAffordanceItem.getMeshManager().add(pd)

    desc = dict(classname='MeshAffordanceItem', Name='test mesh', Filename=meshId, uuid=newUUID(), pose=((0.5,0.0,1.0), (1,0,0,0)))
    return affordanceFromDescription(desc)

# 需要导入模块: from ddapp.debugVis import DebugData [as 别名]
# 或者: from ddapp.debugVis.DebugData import addArrow [as 别名]
    def onSpawnMesh(self):

        d = DebugData()
        d.addArrow((0,0,0), (0,0,0.3))
        pd = d.getPolyData()
        meshId = affordanceitems.MeshAffordanceItem.getMeshManager().add(pd)

        pose = transformUtils.poseFromTransform(self.getSpawnFrame())
        desc = dict(classname='MeshAffordanceItem', Name='mesh', Filename=meshId, uuid=newUUID(), pose=pose)
        return self.affordanceManager.newAffordanceFromDescription(desc)

# 需要导入模块: from ddapp.debugVis import DebugData [as 别名]
# 或者: from ddapp.debugVis.DebugData import addArrow [as 别名]
 def doFTDraw(self, unusedrobotstate):
     frames = []
     fts = []
     vis_names = []
     if (hasattr(self.robotStateJointController, 'lastRobotStateMessage') and 
         self.robotStateJointController.lastRobotStateMessage):
         if self.draw_right:
             rft = np.array(self.robotStateJointController.lastRobotStateMessage.force_torque.r_hand_force +
               self.robotStateJointController.lastRobotStateMessage.force_torque.r_hand_torque)
             rft[0] = -1.*rft[0] # right FT looks to be rotated 180deg around the z axis
             rft[1] = -1.*rft[1]
             rft[3] = -1.*rft[3]
             rft[4] = -1.*rft[4]
             rft -= self.ft_right_bias
             fts.append(rft)
             frames.append(self.robotStateModel.getLinkFrame('r_hand_force_torque'))
             vis_names.append('ft_right')
         if self.draw_left:
             lft = np.array(self.robotStateJointController.lastRobotStateMessage.force_torque.l_hand_force +
               self.robotStateJointController.lastRobotStateMessage.force_torque.l_hand_torque)
             lft -= self.ft_left_bias
             fts.append(lft)
             frames.append(self.robotStateModel.getLinkFrame('l_hand_force_torque'))
             vis_names.append('ft_left')
         
     for i in range(0, len(frames)):
         frame = frames[i]
         ft = fts[i]
         offset = [0., 0., 0.]
         p1 = frame.TransformPoint(offset)
         scale = 1.0/25.0 # todo add slider for this?
         scalet = 1.0 / 2.5 
         p2f = frame.TransformPoint(offset + ft[0:3]*scale)
         p2t = frame.TransformPoint(offset + ft[3:6])
         normalt = (np.array(p2t) - np.array(p1))
         normalt = normalt /  np.linalg.norm(normalt)
         d = DebugData()
         # force
         if np.linalg.norm(np.array(p2f) - np.array(p1)) < 0.1:
             d.addLine(p1, p2f, color=[1.0, 0.0, 0.0])
         else:
             d.addArrow(p1, p2f, color=[1.,0.,0.])
         # torque
         if self.draw_torque:
             d.addCircle(p1, normalt, scalet*np.linalg.norm(ft[3:6]))
         # frame (largely for debug)
         vis.updateFrame(frame, vis_names[i]+'frame', view=self.view, parent='wristft', visible=False, scale=0.2)
         vis.updatePolyData(d.getPolyData(), vis_names[i], view=self.view, parent='wristft')

# 需要导入模块: from ddapp.debugVis import DebugData [as 别名]
# 或者: from ddapp.debugVis.DebugData import addArrow [as 别名]
    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 = transformUtils.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.GetPosition()[0] - t_sole_prev.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)