def make_pose_rel(parent: Pose, child: Pose) -> Pose: """ :param parent: e.g. scene object :param child: e.g. action point :return: relative pose """ p = Pose() p.position = make_position_rel(parent.position, child.position).rotated(parent.orientation, True) p.orientation = make_orientation_rel(parent.orientation, child.orientation) return p
def make_pose_abs(parent: Pose, child: Pose) -> Pose: """ :param parent: e.g. scene object :param child: e.g. action point :return: absolute pose """ p = Pose() p.position = child.position.rotated(parent.orientation) p.position = make_position_abs(parent.position, p.position) p.orientation = make_orientation_abs(parent.orientation, child.orientation) return p
def get_end_effector_pose(self) -> Pose: if self.simulator: return self.forward_kinematics(self.robot_joints()) try: pos = self._dobot.get_pose() # in mm except DobotApiException as e: raise DobotException("Failed to get pose.") from e p = Pose() p.position.x = pos.position.x / 1000.0 p.position.y = pos.position.y / 1000.0 p.position.z = pos.position.z / 1000.0 p.orientation = self.ROTATE_EEF * Orientation.from_rotation_vector( z=math.radians(pos.position.r)) self._handle_pose_out(p) return tr.make_pose_abs(self.pose, p)