Python diff_poses Examples

Example #1

File: mico_bs_controller.py Project: mu-777/predictive_display_for_mobile_manipulator

 def _ee_state_err(self):
     pose_ref = self._mani_ref.ee_pose if self._is_initialized_mani_ref else self._robot_state_cur.ee_pose
     pose_err = diff_poses(self._robot_state_cur.ee_pose, pose_ref)
     return np.matrix(rosmsg2nparray(pose_err)).T.round(decimals=self._decimals)

Example #2

File: mico_controller.py Project: mu-777/predictive_display_for_mobile_manipulator

 def update_input(self):
     self.arm_base_tf = update_tf(self._tf_buffer, self.arm_base_tf)
     # TODO: diff_poses may be wrong
     ee_pose_from_arm_base = diff_poses(self.ref_pose_from_origin_frame, self.arm_base_tf)
     return PoseStamped(header=Header(stamp=rospy.Time.now()),
                        pose=ee_pose_from_arm_base)