def transformPlanToBDIFrame(self, plan): if (self.pose_bdi is None): # print "haven't received POSE_BDI" return # TODO: This transformation should be rewritten using the LOCAL_TO_LOCAL_BDI frame # instead of using FK here t_bodybdi = transformUtils.transformFromPose(self.pose_bdi.pos, self.pose_bdi.orientation) t_bodybdi.PostMultiply() current_pose = self.jointController.q t_bodymain = transformUtils.transformFromPose( current_pose[0:3] , botpy.roll_pitch_yaw_to_quat(current_pose[3:6]) ) t_bodymain.PostMultiply() # iterate and transform self.bdi_plan = plan.decode( plan.encode() ) # decode and encode ensures deepcopy for i, footstep in enumerate(self.bdi_plan.footsteps): step = footstep.pos t_step = transformUtils.frameFromPositionMessage(step) t_body_to_step = vtk.vtkTransform() t_body_to_step.DeepCopy(t_step) t_body_to_step.PostMultiply() t_body_to_step.Concatenate(t_bodymain.GetLinearInverse()) t_stepbdi = vtk.vtkTransform() t_stepbdi.DeepCopy(t_body_to_step) t_stepbdi.PostMultiply() t_stepbdi.Concatenate(t_bodybdi) footstep.pos = transformUtils.positionMessageFromFrame(t_stepbdi) if (self.showBDIPlan is True): self.drawBDIFootstepPlan()
def transformPlanToBDIFrame(self, plan): if (self.pose_bdi is None): # print "haven't received POSE_BDI" return # TODO: This transformation should be rewritten using the LOCAL_TO_LOCAL_BDI frame # instead of using FK here t_bodybdi = transformUtils.transformFromPose(self.pose_bdi.pos, self.pose_bdi.orientation) t_bodybdi.PostMultiply() current_pose = self.jointController.q t_bodymain = transformUtils.transformFromPose( current_pose[0:3] , transformUtils.rollPitchYawToQuaternion(current_pose[3:6]) ) t_bodymain.PostMultiply() # iterate and transform self.bdi_plan = plan.decode( plan.encode() ) # decode and encode ensures deepcopy for i, footstep in enumerate(self.bdi_plan.footsteps): step = footstep.pos t_step = transformUtils.frameFromPositionMessage(step) t_body_to_step = vtk.vtkTransform() t_body_to_step.DeepCopy(t_step) t_body_to_step.PostMultiply() t_body_to_step.Concatenate(t_bodymain.GetLinearInverse()) t_stepbdi = vtk.vtkTransform() t_stepbdi.DeepCopy(t_body_to_step) t_stepbdi.PostMultiply() t_stepbdi.Concatenate(t_bodybdi) footstep.pos = transformUtils.positionMessageFromFrame(t_stepbdi) if (self.showBDIPlan is True): self.drawBDIFootstepPlan()
def assignFrames(self): # foot to box self.footToBox = transformUtils.transformFromPose(np.array([-0.6436723 , 0.18848073, -1.13987699]), np.array([ 0.99576385, 0. , 0. , -0.09194753])) # self.palmToBox = transformUtils.transformFromPose(np.array([-0.13628039, -0.12582009, 0.33638863]), np.array([-0.69866187, 0.07267815, 0.70683338, 0.08352274])) self.palmToBox = transformUtils.transformFromPose(np.array([-0.13516451, -0.12463758, 0.25173153]), np.array([-0.69867721, 0.07265162, 0.70682793, 0.08346358])) pinchToBox = self.getPinchToPalmFrame() pinchToBox.PostMultiply() pinchToBox.Concatenate(self.palmToBox) self.pinchToBox = pinchToBox
def transformFromFootstep(self, footstep): trans = footstep.pos.translation trans = [trans.x, trans.y, trans.z] quat = footstep.pos.rotation quat = [quat.w, quat.x, quat.y, quat.z] return transformUtils.transformFromPose(trans, quat)
def transformGeometry(polyDataList, geom): t = transformUtils.transformFromPose(geom.position, geom.quaternion) polyDataList = [ filterUtils.transformPolyData(polyData, t) for polyData in polyDataList ] return polyDataList
def setPolyData(self, polyData): if polyData.GetNumberOfPoints(): originPose = self.getProperty('Origin') pos, quat = originPose[:3], originPose[3:] t = transformUtils.transformFromPose(pos, quat) polyData = filterUtils.transformPolyData(polyData, t.GetLinearInverse()) PolyDataItem.setPolyData(self, polyData)
def transformFromFootstep(self, footstep): #print 'footstep (trans and quat) :' trans = footstep.pos.translation trans = [trans.x, trans.y, trans.z] #print (trans) quat = footstep.pos.rotation quat = [quat.w, quat.x, quat.y, quat.z] #print (quat) return transformUtils.transformFromPose(trans, quat)
def getPinchToHandFrame(self): pinchToHand = transformUtils.transformFromPose( np.array([-1.22270636e-07, -3.11575498e-01, 0.00000000e+00]), np.array([ 3.26794897e-07, -2.42861455e-17, -1.85832253e-16, 1.00000000e+00 ])) return pinchToHand
def onRobotStatus(self, msg): x = msg.pose.translation.x y = msg.pose.translation.y z = msg.pose.translation.z q1 = msg.pose.rotation.w q2 = msg.pose.rotation.x q3 = msg.pose.rotation.y q4 = msg.pose.rotation.z self.tf_robotStatus = transformUtils.transformFromPose([x,y,z], [q1,q2,q3,q4]) '''
def onICPCorrection(m): t = transformUtils.transformFromPose(m.trans, m.quat) _icpTransforms.append(t) print 'appending icp transform %d' % len(_icpTransforms) if len(_icpTransforms) == 1: storeInitialTransform() for func in _icpCallbacks: func(t)
def getPlanningFrame(self): platformToWorld = self.platform.getChildFrame().transform worldToPlatform = platformToWorld.GetLinearInverse() f = self.robotSystem.footstepsDriver.getFeetMidPoint(self.robotSystem.robotStateModel) footPosition = f.GetPosition() footPosInPlatformFrame = worldToPlatform.TransformPoint(footPosition) planFramePosInPlatformFrame = self.dimensions/2.0 planFramePosInPlatformFrame[1] = footPosInPlatformFrame[1] planFramePosInWorld = platformToWorld.TransformPoint(planFramePosInPlatformFrame) # now we want to find the homogeneous transform for the planning Frame _,quat = transformUtils.poseFromTransform(platformToWorld) self.planToWorld = transformUtils.transformFromPose(planFramePosInWorld,quat)
def testEuler(): ''' Test some euler conversions ''' quat = transformations.random_quaternion() pos = np.random.rand(3) frame = transformUtils.transformFromPose(pos, quat) mat = transformUtils.getNumpyFromTransform(frame) rpy = transformUtils.rollPitchYawFromTransform(frame) rpy2 = transformations.euler_from_matrix(mat) print rpy print rpy2 assert np.allclose(rpy, rpy2)
def getPlanningFrame(self): platformToWorld = self.platform.getChildFrame().transform worldToPlatform = platformToWorld.GetLinearInverse() f = self.robotSystem.footstepsDriver.getFeetMidPoint( self.robotSystem.robotStateModel) footPosition = f.GetPosition() footPosInPlatformFrame = worldToPlatform.TransformPoint(footPosition) planFramePosInPlatformFrame = self.dimensions / 2.0 planFramePosInPlatformFrame[1] = footPosInPlatformFrame[1] planFramePosInWorld = platformToWorld.TransformPoint( planFramePosInPlatformFrame) # now we want to find the homogeneous transform for the planning Frame _, quat = transformUtils.poseFromTransform(platformToWorld) self.planToWorld = transformUtils.transformFromPose( planFramePosInWorld, quat)
def TargetReceived(self, data): self.log( 'Target received (%.3f,%.3f,%.3f), (%.3f,%.3f,%.3f,%.3f)' % \ (data.trans[0], data.trans[1], data.trans[2], \ data.quat[0], data.quat[1], data.quat[2], data.quat[3]) ) if math.isnan(data.trans[0]): self.log('Getting NaN target, stop') return self.TargetMsg = deepcopy(data) targetToWorld = transformUtils.transformFromPose(self.TargetMsg.trans, self.TargetMsg.quat) startPose = self.getPlanningStartPose() handToWorld= self.ikPlanner.getLinkFrameAtPose( 'l_hand_face', startPose) goalFrame = vis.updateFrame(handToWorld, 'OriginalFrame', parent='Pfgrasp', visible=True, scale=0.25) goalFrame2 = vis.updateFrame(targetToWorld, 'PeterFrame', parent='Pfgrasp', visible=True, scale=0.25) handToWorld_XYZ = handToWorld.GetPosition() targetToWorld_XYZ = targetToWorld.GetPosition() dist = sqrt( (handToWorld_XYZ[0]-targetToWorld_XYZ[0])**2 + (handToWorld_XYZ[1]-targetToWorld_XYZ[1])**2 + (handToWorld_XYZ[2]-targetToWorld_XYZ[2])**2 ) self.log( "dist %.3f" % dist ) threshold = float(self.ui.criterionEdit.text) if(dist < threshold): #easygui.msgbox("The correction movement is less than 0.015, you can go grasp it", title="Done") self.log("The correction movement is %.3f less than %.3f, you can go grasp it" % (dist, threshold)) if self.autoMode: self.guardedMoveForwardAndGraspHoldRetreat() else: #print "startPose", startPose #print "targetToWorld", targetToWorld #print "graspingHand", self.graspingHand constraintSet = self.ikPlanner.planEndEffectorGoal(startPose, self.graspingHand, targetToWorld, lockBase=False, lockBack=True) endPose, info = constraintSet.runIk() if info > 10: self.log("in Target received: Bad movement") return graspPlan = constraintSet.runIkTraj() if self.autoMode: self.manipPlanner.commitManipPlan(graspPlan) self.waitForPlanExecution(graspPlan) self.runoneiter()
def testTransform(): ''' test transformFromPose --> getAxesFromTransform is same as quat --> matrix ''' quat = transformations.random_quaternion() pos = np.random.rand(3) frame = transformUtils.transformFromPose(pos, quat) axes = transformUtils.getAxesFromTransform(frame) mat = transformUtils.getNumpyFromTransform(frame) assert np.allclose(mat[:3, :3], np.array(axes).transpose()) mat2 = transformations.quaternion_matrix(quat) mat2[:3, 3] = pos print mat print mat2 assert np.allclose(mat, mat2)
def __init__(self): pose = transformUtils.poseFromTransform(vtk.vtkTransform()) self.pointcloud = ioUtils.readPolyData(ddapp.getDRCBaseDir() + '/software/models/rehearsal_pointcloud.vtp') self.pointcloudPD = vis.showPolyData(self.pointcloud, 'coursemodel', parent=None) segmentation.makeMovable(self.pointcloudPD, transformUtils.transformFromPose(array([0, 0, 0]), array([ 1.0, 0. , 0. , 0.0]))) self.originFrame = self.pointcloudPD.getChildFrame() t = transformUtils.transformFromPose(array([-4.39364111, -0.51507392, -0.73125563]), array([ 0.93821625, 0. , 0. , -0.34604951])) self.valveWalkFrame = vis.updateFrame(t, 'ValveWalk', scale=0.2,visible=True, parent=self.pointcloudPD) t = transformUtils.transformFromPose(array([-3.31840048, 0.36408685, -0.67413123]), array([ 0.93449475, 0. , 0. , -0.35597691])) self.drillPreWalkFrame = vis.updateFrame(t, 'DrillPreWalk', scale=0.2,visible=True, parent=self.pointcloudPD) t = transformUtils.transformFromPose(array([-2.24553758, -0.52990939, -0.73255338]), array([ 0.93697004, 0. , 0. , -0.34940972])) self.drillWalkFrame = vis.updateFrame(t, 'DrillWalk', scale=0.2,visible=True, parent=self.pointcloudPD) t = transformUtils.transformFromPose(array([-2.51306835, -0.92994004, -0.74173541 ]), array([-0.40456572, 0. , 0. , 0.91450893])) self.drillWallWalkFarthestSafeFrame = vis.updateFrame(t, 'DrillWallWalkFarthestSafe', scale=0.2,visible=True, parent=self.pointcloudPD) t = transformUtils.transformFromPose(array([-2.5314524 , -0.27401861, -0.71302976]), array([ 0.98691519, 0. , 0. , -0.16124022])) self.drillWallWalkBackFrame = vis.updateFrame(t, 'DrillWallWalkBack', scale=0.2,visible=True, parent=self.pointcloudPD) t = transformUtils.transformFromPose(array([-1.16122318, 0.04723203, -0.67493468]), array([ 0.93163145, 0. , 0. , -0.36340451])) self.surprisePreWalkFrame = vis.updateFrame(t, 'SurprisePreWalk', scale=0.2,visible=True, parent=self.pointcloudPD) t = transformUtils.transformFromPose(array([-0.5176186 , -1.00151554, -0.70650799]), array([ 0.84226497, 0. , 0. , -0.53906374])) self.surpriseWalkFrame = vis.updateFrame(t, 'SurpriseWalk', scale=0.2,visible=True, parent=self.pointcloudPD) t = transformUtils.transformFromPose(array([-0.69100097, -0.43713269, -0.68495922]), array([ 0.98625075, 0. , 0. , -0.16525575])) self.surpriseWalkBackFrame = vis.updateFrame(t, 'SurpriseWalkBack', scale=0.2,visible=True, parent=self.pointcloudPD) t = transformUtils.transformFromPose(array([ 0.65827322, -0.08028796, -0.77370834]), array([ 0.94399977, 0. , 0. , -0.3299461 ])) self.terrainPreWalkFrame = vis.updateFrame(t, 'TerrainPreWalk', scale=0.2,visible=True, parent=self.pointcloudPD) t = transformUtils.transformFromPose(array([ 5.47126425, -0.09790393, -0.70504679]), array([ 1., 0., 0., 0.])) self.stairsPreWalkFrame = vis.updateFrame(t, 'StairsPreWalk', scale=0.2,visible=True, parent=self.pointcloudPD) self.frameSync = vis.FrameSync() self.frameSync.addFrame(self.originFrame) self.frameSync.addFrame(self.pointcloudPD.getChildFrame(), ignoreIncoming=True) self.frameSync.addFrame(self.valveWalkFrame, ignoreIncoming=True) self.frameSync.addFrame(self.drillPreWalkFrame, ignoreIncoming=True) self.frameSync.addFrame(self.drillWalkFrame, ignoreIncoming=True) self.frameSync.addFrame(self.drillWallWalkFarthestSafeFrame, ignoreIncoming=True) self.frameSync.addFrame(self.drillWallWalkBackFrame, ignoreIncoming=True) self.frameSync.addFrame(self.surprisePreWalkFrame, ignoreIncoming=True) self.frameSync.addFrame(self.surpriseWalkFrame, ignoreIncoming=True) self.frameSync.addFrame(self.surpriseWalkBackFrame, ignoreIncoming=True) self.frameSync.addFrame(self.terrainPreWalkFrame, ignoreIncoming=True) self.frameSync.addFrame(self.stairsPreWalkFrame, ignoreIncoming=True)
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)
def __init__(self): pose = transformUtils.poseFromTransform(vtk.vtkTransform()) self.pointcloud = ioUtils.readPolyData( ddapp.getDRCBaseDir() + '/software/models/rehearsal_pointcloud.vtp') self.pointcloudPD = vis.showPolyData(self.pointcloud, 'coursemodel', parent=None) segmentation.makeMovable( self.pointcloudPD, transformUtils.transformFromPose(array([0, 0, 0]), array([1.0, 0., 0., 0.0]))) self.originFrame = self.pointcloudPD.getChildFrame() t = transformUtils.transformFromPose( array([-4.39364111, -0.51507392, -0.73125563]), array([0.93821625, 0., 0., -0.34604951])) self.valveWalkFrame = vis.updateFrame(t, 'ValveWalk', scale=0.2, visible=True, parent=self.pointcloudPD) t = transformUtils.transformFromPose( array([-3.31840048, 0.36408685, -0.67413123]), array([0.93449475, 0., 0., -0.35597691])) self.drillPreWalkFrame = vis.updateFrame(t, 'DrillPreWalk', scale=0.2, visible=True, parent=self.pointcloudPD) t = transformUtils.transformFromPose( array([-2.24553758, -0.52990939, -0.73255338]), array([0.93697004, 0., 0., -0.34940972])) self.drillWalkFrame = vis.updateFrame(t, 'DrillWalk', scale=0.2, visible=True, parent=self.pointcloudPD) t = transformUtils.transformFromPose( array([-2.51306835, -0.92994004, -0.74173541]), array([-0.40456572, 0., 0., 0.91450893])) self.drillWallWalkFarthestSafeFrame = vis.updateFrame( t, 'DrillWallWalkFarthestSafe', scale=0.2, visible=True, parent=self.pointcloudPD) t = transformUtils.transformFromPose( array([-2.5314524, -0.27401861, -0.71302976]), array([0.98691519, 0., 0., -0.16124022])) self.drillWallWalkBackFrame = vis.updateFrame(t, 'DrillWallWalkBack', scale=0.2, visible=True, parent=self.pointcloudPD) t = transformUtils.transformFromPose( array([-1.16122318, 0.04723203, -0.67493468]), array([0.93163145, 0., 0., -0.36340451])) self.surprisePreWalkFrame = vis.updateFrame(t, 'SurprisePreWalk', scale=0.2, visible=True, parent=self.pointcloudPD) t = transformUtils.transformFromPose( array([-0.5176186, -1.00151554, -0.70650799]), array([0.84226497, 0., 0., -0.53906374])) self.surpriseWalkFrame = vis.updateFrame(t, 'SurpriseWalk', scale=0.2, visible=True, parent=self.pointcloudPD) t = transformUtils.transformFromPose( array([-0.69100097, -0.43713269, -0.68495922]), array([0.98625075, 0., 0., -0.16525575])) self.surpriseWalkBackFrame = vis.updateFrame(t, 'SurpriseWalkBack', scale=0.2, visible=True, parent=self.pointcloudPD) t = transformUtils.transformFromPose( array([0.65827322, -0.08028796, -0.77370834]), array([0.94399977, 0., 0., -0.3299461])) self.terrainPreWalkFrame = vis.updateFrame(t, 'TerrainPreWalk', scale=0.2, visible=True, parent=self.pointcloudPD) t = transformUtils.transformFromPose( array([5.47126425, -0.09790393, -0.70504679]), array([1., 0., 0., 0.])) self.stairsPreWalkFrame = vis.updateFrame(t, 'StairsPreWalk', scale=0.2, visible=True, parent=self.pointcloudPD) self.frameSync = vis.FrameSync() self.frameSync.addFrame(self.originFrame) self.frameSync.addFrame(self.pointcloudPD.getChildFrame(), ignoreIncoming=True) self.frameSync.addFrame(self.valveWalkFrame, ignoreIncoming=True) self.frameSync.addFrame(self.drillPreWalkFrame, ignoreIncoming=True) self.frameSync.addFrame(self.drillWalkFrame, ignoreIncoming=True) self.frameSync.addFrame(self.drillWallWalkFarthestSafeFrame, ignoreIncoming=True) self.frameSync.addFrame(self.drillWallWalkBackFrame, ignoreIncoming=True) self.frameSync.addFrame(self.surprisePreWalkFrame, ignoreIncoming=True) self.frameSync.addFrame(self.surpriseWalkFrame, ignoreIncoming=True) self.frameSync.addFrame(self.surpriseWalkBackFrame, ignoreIncoming=True) self.frameSync.addFrame(self.terrainPreWalkFrame, ignoreIncoming=True) self.frameSync.addFrame(self.stairsPreWalkFrame, ignoreIncoming=True)
def onBDIPose(self, m): t = transformUtils.transformFromPose(m.pos, m.orientation) if self.timer.elapsed() > 1.0: self.transforms.append((self.timer.now(), t)) self.timer.reset()
def __init__(self): self.aprilTagSubsciber = lcmUtils.addSubscriber('APRIL_TAG_TO_CAMERA_LEFT', lcmbotcore.rigid_transform_t, self.onAprilTag) pose = transformUtils.poseFromTransform(vtk.vtkTransform()) desc = dict(classname='MeshAffordanceItem', Name='polaris', Filename='software/models/polaris/polaris_cropped.vtp', pose=pose) self.pointcloudAffordance = segmentation.affordanceManager.newAffordanceFromDescription(desc) self.originFrame = self.pointcloudAffordance.getChildFrame() self.originToAprilTransform = transformUtils.transformFromPose(np.array([-0.038508 , -0.00282131, -0.01000079]), np.array([ 9.99997498e-01, -2.10472556e-03, -1.33815696e-04, 7.46246794e-04])) # offset for . . . who knows why # t = transformUtils.transformFromPose(np.array([ 0.14376024, 0.95920689, 0.36655712]), np.array([ 0.28745842, 0.90741428, -0.28822068, 0.10438304])) t = transformUtils.transformFromPose(np.array([ 0.10873244, 0.93162364, 0.40509084]), np.array([ 0.32997378, 0.88498408, -0.31780588, 0.08318602])) self.leftFootEgressStartFrame = vis.updateFrame(t, 'left foot start', scale=0.2,visible=True, parent=self.pointcloudAffordance) t = transformUtils.transformFromPose(np.array([ 0.265, 0.874, 0.274]), np.array([ 0.35290731, 0.93443693, -0.04181263, 0.02314636])) self.leftFootEgressMidFrame = vis.updateFrame(t, 'left foot mid', scale=0.2,visible=True, parent=self.pointcloudAffordance) t = transformUtils.transformFromPose(np.array([ 0.54339115, 0.89436275, 0.26681047]), np.array([ 0.34635985, 0.93680077, -0.04152008, 0.02674412])) self.leftFootEgressOutsideFrame = vis.updateFrame(t, 'left foot outside', scale=0.2,visible=True, parent=self.pointcloudAffordance) # pose = [np.array([-0.78962299, 0.44284877, -0.29539116]), np.array([ 0.54812954, 0.44571517, -0.46063251, 0.53731713])] #old location # pose = [np.array([-0.78594663, 0.42026626, -0.23248139]), np.array([ 0.54812954, 0.44571517, -0.46063251, 0.53731713])] # updated location pose = [np.array([-0.78594663, 0.42026626, -0.23248139]), np.array([ 0.53047159, 0.46554963, -0.48086192, 0.52022615])] # update orientation desc = dict(classname='CapsuleRingAffordanceItem', Name='Steering Wheel', uuid=newUUID(), pose=pose, Color=[1, 0, 0], Radius=float(0.18), Segments=20) self.steeringWheelAffordance = segmentation.affordanceManager.newAffordanceFromDescription(desc) pose = [np.array([-0.05907324, 0.80460545, 0.45439687]), np.array([ 0.14288327, 0.685944 , -0.703969 , 0.11615873])] desc = dict(classname='BoxAffordanceItem', Name='pedal', Dimensions=[0.12, 0.33, 0.04], pose=pose, Color=[0,1,0]) self.pedalAffordance = segmentation.affordanceManager.newAffordanceFromDescription(desc) # t = transformUtils.transformFromPose(np.array([ 0.04045136, 0.96565326, 0.25810111]), # np.array([ 0.26484648, 0.88360091, -0.37065556, -0.10825996])) # t = transformUtils.transformFromPose(np.array([ -4.34908919e-04, 9.24901627e-01, 2.65614116e-01]), # np.array([ 0.25022251, 0.913271 , -0.32136359, -0.00708626])) t = transformUtils.transformFromPose(np.array([ 0.0384547 , 0.89273742, 0.24140762]), np.array([ 0.26331831, 0.915796 , -0.28055337, 0.11519963])) self.leftFootPedalSwingFrame = vis.updateFrame(t,'left foot pedal swing', scale=0.2, visible=True, parent=self.pointcloudAffordance) t = transformUtils.transformFromPose(np.array([-0.9012598 , -0.05709763, 0.34897024]), np.array([ 0.03879584, 0.98950919, 0.03820214, 0.13381721])) self.leftFootDrivingFrame = vis.updateFrame(t,'left foot driving', scale=0.2, visible=True, parent=self.pointcloudAffordance) # t = transformUtils.transformFromPose(np.array([-0.12702725, 0.92068409, 0.27209386]), # np.array([ 0.2062255 , 0.92155886, -0.30781119, 0.11598529])) # t = transformUtils.transformFromPose(np.array([-0.14940375, 0.90347275, 0.23812658]), # np.array([ 0.27150909, 0.91398724, -0.28877386, 0.0867167 ])) # t = transformUtils.transformFromPose(np.array([-0.17331227, 0.87879312, 0.25645152]), # np.array([ 0.26344489, 0.91567196, -0.28089824, 0.11505581])) # self.leftFootDrivingKneeInFrame = vis.updateFrame(t,'left foot driving knee in', scale=0.2, visible=True, parent=self.pointcloudAffordance) t = transformUtils.transformFromPose(np.array([-0.12702725, 0.92068409, 0.27209386]), np.array([ 0.2062255 , 0.92155886, -0.30781119, 0.11598529])) self.leftFootDrivingKneeInFrame = vis.updateFrame(t,'left foot driving knee in', scale=0.2, visible=True, parent=self.pointcloudAffordance) t = transformUtils.transformFromPose(np.array([-0.13194951, 0.89871423, 0.24956246]), np.array([ 0.21589082, 0.91727326, -0.30088849, 0.14651633])) self.leftFootOnPedal = vis.updateFrame(t,'left foot on pedal', scale=0.2, visible=True, parent=self.pointcloudAffordance) t = transformUtils.transformFromPose(np.array([ 0.17712239, 0.87619935, 0.27001509]), np.array([ 0.33484372, 0.88280787, -0.31946488, 0.08044963])) self.leftFootUpFrame = vis.updateFrame(t,'left foot up frame', scale=0.2, visible=True, parent=self.pointcloudAffordance) t = transformUtils.transformFromPose(np.array([ 0.47214939, -0.04856998, 0.01375837]), np.array([ 6.10521653e-03, 4.18621358e-04, 4.65520611e-01, 8.85015882e-01])) self.rightHandGrabFrame = vis.updateFrame(t,'right hand grab bar', scale=0.2, visible=True, parent=self.pointcloudAffordance) self.frameSync = vis.FrameSync() self.frameSync.addFrame(self.originFrame) self.frameSync.addFrame(self.pointcloudAffordance.getChildFrame(), ignoreIncoming=True) self.frameSync.addFrame(self.leftFootEgressStartFrame, ignoreIncoming=True) self.frameSync.addFrame(self.leftFootEgressMidFrame, ignoreIncoming=True) self.frameSync.addFrame(self.leftFootEgressOutsideFrame, ignoreIncoming=True) self.frameSync.addFrame(self.steeringWheelAffordance.getChildFrame(), ignoreIncoming=True) self.frameSync.addFrame(self.pedalAffordance.getChildFrame(), ignoreIncoming=True) self.frameSync.addFrame(self.leftFootPedalSwingFrame, ignoreIncoming=True) self.frameSync.addFrame(self.leftFootDrivingFrame, ignoreIncoming=True) self.frameSync.addFrame(self.leftFootDrivingKneeInFrame, ignoreIncoming=True) self.frameSync.addFrame(self.leftFootOnPedal, ignoreIncoming=True) self.frameSync.addFrame(self.leftFootUpFrame, ignoreIncoming=True) self.frameSync.addFrame(self.rightHandGrabFrame, ignoreIncoming=True)
def setTransform(self, pos, quat): trans = transformUtils.transformFromPose(pos, quat) self.transform.SetMatrix(trans.GetMatrix()) self.transform.Modified()
def onFootstepStatus(self, msg): #print "got message" import ipab #print msg.actual_foot_position_in_world[0], msg.actual_foot_position_in_world[1], msg.actual_foot_position_in_world[2] #print msg.actual_foot_orientation_in_world[0], msg.actual_foot_orientation_in_world[1], msg.actual_foot_orientation_in_world[2], msg.actual_foot_orientation_in_world[3] #print msg.LEFT #print msg.RIGHT x = msg.actual_foot_position_in_world[0] y = msg.actual_foot_position_in_world[1] z = msg.actual_foot_position_in_world[2] q1 = msg.actual_foot_orientation_in_world[0] q2 = msg.actual_foot_orientation_in_world[1] q3 = msg.actual_foot_orientation_in_world[2] q4 = msg.actual_foot_orientation_in_world[3] if msg.status == 1: tf_footStatus = transformUtils.transformFromPose([x,y,z], [q1,q2,q3,q4]) self.transforms_series[:] = [] self.transforms_series.append(tf_footStatus) self.transforms_series.append(self.tf_robotStatus.GetInverse()) tf_foot_robot = transformUtils.concatenateTransforms(self.transforms_series) self.footstep_index = self.footstep_index + 1 # Check what foot is in contact #print 'self.footstep_index' #print self.footstep_index [robot_pos, robot_ori] = transformUtils.poseFromTransform(self.tf_robotStatus) [current_pos, current_ori] = transformUtils.poseFromTransform(tf_foot_robot) if (current_pos[1] > 0): current_left = True else: current_left = False #print 'current:' #print (current_pos) #print 'robot:' #print (robot_pos) #print 'Left?' #print current_left # I want to take the first status for the LEFT foot if self.new_status and current_left: # left foot in contact (reached left single support) self.footStatus.append(Footstep(tf_footStatus, 0)) self.new_first_double_supp = True self.new_status = False # right foot expected pose (from planning) if self.footstep_index+1 < len(self.planned_footsteps): if (self.planned_footsteps[self.footstep_index+1].is_right_foot): self.footStatus.append(self.planned_footsteps[self.footstep_index+1]) else: self.footStatus.append(self.planned_footsteps[self.footstep_index]) else: self.footStatus.append(self.footStatus_right[len(self.footStatus_right)-1]) elif not current_left: # right foot in contact self.footStatus_right.append(Footstep(tf_footStatus, 1)) else: self.new_status = True if (len(self.footStatus) > 0 and self.new_first_double_supp): t1 = self.footStatus[len(self.footStatus)-2].transform t2 = self.footStatus[len(self.footStatus)-1].transform #print 't1 and t2:' #print (transformUtils.poseFromTransform(t1)) #print (transformUtils.poseFromTransform(t2)) #print 'list lenght:' #print (len(self.footStatus)) self.new_first_double_supp = False if self.footStatus[len(self.footStatus)-2].is_right_foot: t1, t2 = t2, t1 pose = self.getNextDoubleSupportPose(t1, t2) self.displayExpectedPose(pose) standingFootName = self.ikPlanner.rightFootLink if self.footStatus[len(self.footStatus)-2].is_right_foot else self.ikPlanner.leftFootLink self.makeReplanRequest(standingFootName, removeFirstLeftStep = False, nextDoubleSupportPose=pose)
def getPinchToHandFrame(self): pinchToHand = transformUtils.transformFromPose(np.array([ -1.22270636e-07, -3.11575498e-01, 0.00000000e+00]), np.array([ 3.26794897e-07, -2.42861455e-17, -1.85832253e-16, 1.00000000e+00])) return pinchToHand
def TargetReceived(self, data): self.log( 'Target received (%.3f,%.3f,%.3f), (%.3f,%.3f,%.3f,%.3f)' % \ (data.trans[0], data.trans[1], data.trans[2], \ data.quat[0], data.quat[1], data.quat[2], data.quat[3]) ) if math.isnan(data.trans[0]): self.log('Getting NaN target, stop') return self.TargetMsg = deepcopy(data) targetToWorld = transformUtils.transformFromPose( self.TargetMsg.trans, self.TargetMsg.quat) startPose = self.getPlanningStartPose() handToWorld = self.ikPlanner.getLinkFrameAtPose( 'l_hand_face', startPose) goalFrame = vis.updateFrame(handToWorld, 'OriginalFrame', parent='Pfgrasp', visible=True, scale=0.25) goalFrame2 = vis.updateFrame(targetToWorld, 'PeterFrame', parent='Pfgrasp', visible=True, scale=0.25) handToWorld_XYZ = handToWorld.GetPosition() targetToWorld_XYZ = targetToWorld.GetPosition() dist = sqrt((handToWorld_XYZ[0] - targetToWorld_XYZ[0])**2 + (handToWorld_XYZ[1] - targetToWorld_XYZ[1])**2 + (handToWorld_XYZ[2] - targetToWorld_XYZ[2])**2) self.log("dist %.3f" % dist) threshold = float(self.ui.criterionEdit.text) if (dist < threshold): #easygui.msgbox("The correction movement is less than 0.015, you can go grasp it", title="Done") self.log( "The correction movement is %.3f less than %.3f, you can go grasp it" % (dist, threshold)) if self.autoMode: self.guardedMoveForwardAndGraspHoldRetreat() else: #print "startPose", startPose #print "targetToWorld", targetToWorld #print "graspingHand", self.graspingHand constraintSet = self.ikPlanner.planEndEffectorGoal( startPose, self.graspingHand, targetToWorld, lockBase=False, lockBack=True) endPose, info = constraintSet.runIk() if info > 10: self.log("in Target received: Bad movement") return graspPlan = constraintSet.runIkTraj() if self.autoMode: self.manipPlanner.commitManipPlan(graspPlan) self.waitForPlanExecution(graspPlan) self.runoneiter()
def repositionFromDescription(self, desc): position, quat = desc['pose'] t = transformUtils.transformFromPose(position, quat) self.getChildFrame().copyFrame(t)
def transformGeometry(polyDataList, geom): t = transformUtils.transformFromPose(geom.position, geom.quaternion) polyDataList = [filterUtils.transformPolyData(polyData, t) for polyData in polyDataList] return polyDataList