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