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)
