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 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]])
