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)
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)
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.affordanceFromDescription(desc)
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')
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)