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])