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)
