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 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 getPoseLCMFromXYZRPY(xyz, rpy):
wxyz = transformUtils.rollPitchYawToQuaternion(rpy)
trans = lcmdrc.vector_3d_t()
trans.x, trans.y, trans.z = xyz
quat = lcmdrc.quaternion_t()
quat.w, quat.x, quat.y, quat.z = wxyz
pose = lcmdrc.position_3d_t()
pose.translation = trans
pose.rotation = quat
return pose
def getPoseLCMFromXYZRPY(xyz, rpy):
wxyz = transformUtils.rollPitchYawToQuaternion(rpy)
trans = lcmdrc.vector_3d_t()
trans.x, trans.y, trans.z = xyz
quat = lcmdrc.quaternion_t()
quat.w, quat.x, quat.y, quat.z = wxyz
pose = lcmdrc.position_3d_t()
pose.translation = trans
pose.rotation = quat
return pose
