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 = 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 = positionMessageFromFrame(t_stepbdi) if (self.showBDIPlan is True): self.drawBDIFootstepPlan()
def moveToPoint(self, x, y, z): pose = geometry_msgs.msg.Pose() pose.position.x = x pose.position.y = y pose.position.z = z quat = transformUtils.rollPitchYawToQuaternion([0, -3.14 / 2, 0]) pose.orientation.x = quat[0] pose.orientation.y = quat[1] pose.orientation.z = quat[2] pose.orientation.w = quat[3] poseStamped = geometry_msgs.msg.PoseStamped() poseStamped.pose = pose poseStamped.header.frame_id = "base" response = self.robotService.runIK(poseStamped) if not response.success: rospy.loginfo( "ik was not successful, returning without moving robot") return rospy.loginfo("ik was successful, moving to joint position") self.robotService.moveToJointPosition(response.joint_state.position, self.maxJointDegreesPerSecond)
def setBoxGraspTarget(position, rpy, dimensions): rot_quat = transformUtils.rollPitchYawToQuaternion( [math.radians(x) for x in rpy]) t = transformUtils.transformFromPose(position, rot_quat) om.removeFromObjectModel(om.findObjectByName('box')) obj = makeBox() obj.setProperty('Dimensions', dimensions) obj.getChildFrame().copyFrame(t) obj.setProperty('Surface Mode', 'Wireframe') obj.setProperty('Color', [1, 0, 0])
def getPoseLCMFromXYZRPY(xyz, rpy): wxyz = transformUtils.rollPitchYawToQuaternion(rpy) trans = bot_core.vector_3d_t() trans.x, trans.y, trans.z = xyz quat = bot_core.quaternion_t() quat.w, quat.x, quat.y, quat.z = wxyz pose = bot_core.position_3d_t() pose.translation = trans pose.rotation = quat return pose
def to_xyzquat(pose): pos = pose[:3] rpy = pose[3:] quat = transformUtils.rollPitchYawToQuaternion(rpy) return np.hstack([pos, quat[1], quat[2], quat[3], quat[0]])