def frame(self):
point = [self.position.x, self.position.y, self.position.z]
quaternion = [
self.orientation.w, self.orientation.x, self.orientation.y,
self.orientation.z
]
return Frame.from_quaternion(quaternion, point=point)
def parse_feedback(self, result):
"""Parses the result as a :class:`compas.geometry.Frame` and :class:`ExternalAxes`.
Return
------
:class:`compas.geometry.Frame`, :class:`ExternalAxes`
Current frame and external axes of the robot.
"""
# read pos
x = result['float_values'][0]
y = result['float_values'][1]
z = result['float_values'][2]
pos = [x, y, z]
# read orient
orient_q1 = result['float_values'][3]
orient_q2 = result['float_values'][4]
orient_q3 = result['float_values'][5]
orient_q4 = result['float_values'][6]
orientation = [orient_q1, orient_q2, orient_q3, orient_q4]
# read gantry joints
external_axes = [
result['float_values'][i] for i in range(7, 13)
if not is_rapid_none(result['float_values'][i])
]
# write result
# As compas frame
result = Frame.from_quaternion(orientation, point=pos)
# End
return result, ExternalAxes(*external_axes)
def frame_from_pose(pose):
"""Returns a frame from a PyBullet pose.
Parameters
----------
pose : tuple
Returns
-------
:class:`compas.geometry.Frame`
"""
point, (x, y, z, w) = pose
return Frame.from_quaternion([w, x, y, z], point=point)
def frame_from_pose(pose, scale=1.0):
"""Returns a frame from a PyBullet pose.
Parameters
----------
point, quaternion : tuple
Returns
-------
:class:`compas.geometry.Frame`
"""
point, (x, y, z, w) = pose
return Frame.from_quaternion([w, x, y, z],
point=[v * scale for v in point])
def tcp_frame(self):
"""TCP represented as a :class:`compas.geometry.Frame`."""
return Frame.from_quaternion(self.tcp_quaternion, point=self.tcp_coord)
def tool_frame_pose_quaternion(self, pose_quaternion):
self.tool_frame = Frame.from_quaternion(pose_quaternion[3:],
point=pose_quaternion[:3])
