def receiveRigidBodyFrame(id, position, rotation): global tcp_to_target_T, global_to_arm_T, tcp_to_arm_T, counter trace("Received frame for rigid body", id, position, rotation) if (id == 1): trans_matrix = math3d.Transform() quaternion = math3d.UnitQuaternion(rotation[3], rotation[0], rotation[1], rotation[2]) trans_matrix.set_pos(position) trans_matrix.set_orient(quaternion.orientation) global_to_arm_T = trans_matrix.get_inverse() trace("arm_to_global_T", trans_matrix.matrix) trace("global_to_arm_T", global_to_arm_T.matrix) elif (id == 3): # frame counter, exec each 10 frame # if(counter < 2): # counter += 1 # return # counter = 0 trans_matrix = math3d.Transform() quaternion = math3d.UnitQuaternion(rotation[3], rotation[0], rotation[1], rotation[2]) trans_matrix.set_pos(position) trans_matrix.set_orient(quaternion.orientation) obj_to_global_T = trans_matrix temp_matrix = global_to_arm_T.matrix * obj_to_global_T.matrix * tcp_to_target_T.matrix tcp_to_arm_T = math3d.Transform() tcp_to_arm_T.set_pos(temp_matrix[:3, 3].A1) orient = math3d.Orientation(temp_matrix[:3, :3].A1) tcp_to_arm_T.set_orient(orient) trace("obj_to_global_T", obj_to_global_T.matrix)
def convert_to_transform(data): quaternion = math3d.UnitQuaternion(data['rot3'], data['rot0'], data['rot1'], data['rot2']) transform = math3d.Transform() transform.set_pos((data['pos0'], data['pos1'], data['pos2'])) transform.set_orient(quaternion.orientation) return transform
def get_QR_pose(pose): global QR_pose pos = [pose.pose.position.x,pose.pose.position.y,pose.pose.position.z] orient = m3d.UnitQuaternion(pose.pose.orientation.w, m3d.Vector(pose.pose.orientation.x, pose.pose.orientation.y, pose.pose.orientation.z)) QR_pose = m3d.Transform(orient,pos).pose_vector
def __convert_to_transform(self, pose_stamped): quaternion = math3d.UnitQuaternion(pose_stamped.pose.orientation.w, pose_stamped.pose.orientation.x, pose_stamped.pose.orientation.y, pose_stamped.pose.orientation.z) transform = math3d.Transform() transform.set_pos( (pose_stamped.pose.position.x, pose_stamped.pose.position.y, pose_stamped.pose.position.z)) transform.set_orient(quaternion.orientation) return transform
def receiveRigidBodyPackageFrame(frameNumber, rigidBodyList): global target_to_object_T, global_to_tcp_T, target_to_tcp_T, recv4 # trace( "Received frame for rigid body ", frameNumber) for rigidBody in rigidBodyList: if (rigidBody['name'] == 'ur_facebow_fixed_big_ball'): recv4 = True trans_matrix = math3d.Transform() quaternion = math3d.UnitQuaternion(rigidBody['rot'][3], rigidBody['rot'][0], rigidBody['rot'][1], rigidBody['rot'][2]) trans_matrix.set_pos(rigidBody['pos']) trans_matrix.set_orient(quaternion.orientation) global_to_tcp_T = trans_matrix.get_inverse() # trace("tcp_to_global_T", trans_matrix.matrix) # trace("global_to_tcp_T", global_to_tcp_T.matrix) elif (rigidBody['name'] == 'cal_design_facebow_small'): if (recv4 == True): trans_matrix = math3d.Transform() quaternion = math3d.UnitQuaternion(rigidBody['rot'][3], rigidBody['rot'][0], rigidBody['rot'][1], rigidBody['rot'][2]) trans_matrix.set_pos(rigidBody['pos']) trans_matrix.set_orient(quaternion.orientation) object_to_global_T = trans_matrix # trace("A", global_to_tcp_T) # trace("B", global_to_object_T.matrix) # trace("C", target_to_object_T.matrix) temp_matrix = global_to_tcp_T.matrix * object_to_global_T.matrix * target_to_object_T.matrix target_to_tcp_T = math3d.Transform() target_to_tcp_T.set_pos(temp_matrix[:3, 3].A1) # target_to_tcp_T.pos.x = target_to_tcp_T.pos.x * -1.0; # target_to_tcp_T.pos.y = 0 # target_to_tcp_T.pos.z = 0 orient = math3d.Orientation(temp_matrix[:3, :3].A1) target_to_tcp_T.set_orient(orient)
def GetTransform(tf_msg_): quat_ = m3d.UnitQuaternion(tf_msg_.transform.rotation.w, tf_msg_.transform.rotation.x, tf_msg_.transform.rotation.y, tf_msg_.transform.rotation.z) orient_ = quat_.get_orientation() pos_ = m3d.Vector(tf_msg_.transform.translation.x, tf_msg_.transform.translation.y, tf_msg_.transform.translation.z) return m3d.Transform(orient_, pos_)
def receiveRigidBodyFrame(id, position, rotation): global target_to_object_T, global_to_tcp_T, target_to_tcp_T, counter, recv4, mqtt_client trace("Received frame for rigid body", id, position, rotation) if (id == 1): trans_matrix = math3d.Transform() quaternion = math3d.UnitQuaternion(rotation[3], rotation[0], rotation[1], rotation[2]) trans_matrix.set_pos(position) trans_matrix.set_orient(quaternion.orientation) json_value = json.dumps({ 'type': 'tcp', 'matrix': trans_matrix.array.flatten().tolist() }) mqtt_client.publish('iqr/cs-ras/optitrack_data', json_value) elif (id == 3): trans_matrix = math3d.Transform() quaternion = math3d.UnitQuaternion(rotation[3], rotation[0], rotation[1], rotation[2]) trans_matrix.set_pos(position) trans_matrix.set_orient(quaternion.orientation) json_value = json.dumps({ 'type': 'pointer', 'matrix': trans_matrix.array.flatten().tolist() }) mqtt_client.publish('iqr/cs-ras/optitrack_data', json_value) elif (id == 4): trans_matrix = math3d.Transform() quaternion = math3d.UnitQuaternion(rotation[3], rotation[0], rotation[1], rotation[2]) trans_matrix.set_pos(position) trans_matrix.set_orient(quaternion.orientation) json_value = json.dumps({ 'type': 'fork', 'matrix': trans_matrix.array.flatten().tolist() }) mqtt_client.publish('iqr/cs-ras/optitrack_data', json_value)
def receiveRigidBodyPackageFrame(frameNumber, rigidBodyList): mutex.acquire() rigid_body_dict.clear() for rigidBody in rigidBodyList: if rigidBody['tracking'] == 1: trans_matrix = math3d.Transform() quaternion = math3d.UnitQuaternion(rigidBody['rot'][3], rigidBody['rot'][0], rigidBody['rot'][1], rigidBody['rot'][2]) trans_matrix.set_pos(rigidBody['pos']) trans_matrix.set_orient(quaternion.orientation) rigid_body_dict[rigidBody['name']] = trans_matrix mutex.release()
def kdl_frame_to_m3d_transform(frame): """Converts a kdl frame to a math3d transformation. Arguments: frame -- The frame """ if isinstance(frame, kdl.Frame): quat = frame.M.GetQuaternion() pos = frame.p m3d_quat = m3d.UnitQuaternion(quat[3], quat[0], quat[1], quat[2], norm_warn=False) m3d_vector = m3d.Vector(pos[0], pos[1], pos[2]) transform = m3d.Transform(m3d_quat.orientation, m3d_vector) return transform else: raise Exception("Conversion not defined for type %s" % type(frame))
def get_axis_angle(self): """Return an (axis,angle) pair representing the equivalent orientation.""" return m3d.UnitQuaternion(self).axis_angle
def get_quaternion(self): """Return a quaternion representing this orientation.""" return m3d.UnitQuaternion(self)