def getEnv(self, key, iter=0, first=False):
val = self.base.getModel(key)
print key, val
obj = None
obj_list = []
parent_tf = tf.identity_matrix()
if self.base.typeOf(key) == self.base.ROBOTS.ident:
obj = self._getRobot(key)
obj_list.append(obj)
parent_tf = obj.GetTransform()
elif self.base.typeOf(key) == self.base.OBJECTS.ident:
obj = self._getObject(key)
obj_list.append(obj)
parent_tf = obj.GetTransform()
elif self.base.typeOf(key) == self.base.LOCATIONS.ident:
obj = self._getLocation(key)
obj_list.append(obj)
parent_tf = obj.GetTransform()
elif self.base.typeOf(key) == self.base.SENSORS.ident:
obj = self._getSensor(key)
obj_list.append(obj)
parent_tf = obj.GetTransform()
else:
if val.has_key("translation"):
translation = map(lambda x: float(x), val["translation"].split(" "))
parent_tf = tf.concatenate_matrices(parent_tf, tf.compose_matrix(translate = translation))
if val.has_key("quat"):
quat = map(lambda x: float(x), val["quat"].split(" "))
rot = rave.axisAngleFromQuat(quat)
m2 = tf.compose_matrix(angles = rot)
parent_tf = tf.concatenate_matrices(parent_tf, m2)
if first:
parent_tf = tf.identity_matrix()
if obj != None:
obj.SetTransform(parent_tf)
if iter==0:
return obj_list
# search for ancestors
child_expr = pycassa.create_index_expression('base', key)
clild_clause = pycassa.create_index_clause([child_expr])
for child_key, _ in self.base.col.get_indexed_slices(clild_clause):
child_obj = self.getEnv(child_key, iter-1)
for obj in child_obj:
if type(obj) != type(None):
obj.SetTransform(tf.concatenate_matrices(parent_tf, obj.GetTransform()))
obj_list.append(obj)
return obj_list
def get_transform_to_world(self):
mat_w_2_mocap = transformations.identity_matrix()
exist_mat_w_2_mocap = False
mat_mocap_2_link = transformations.identity_matrix()
exists_mat_mocap_2_link = False
mat_link_2_rgb = transformations.identity_matrix()
exists_mat_link_2_rgb = False
mat_optical = transformations.identity_matrix()
exists_mat_optical = False
with open(self.filename) as csv_file:
csv_reader = csv.reader(csv_file, delimiter=',')
for row in csv_reader:
if (not exist_mat_w_2_mocap) and (row[3] == "world" and row[4] == "kinect2_mocap"):
exist_mat_w_2_mocap = True
mat_w_2_mocap = matrix_from_row(row)
if (not exists_mat_mocap_2_link) and (row[3] == "/kinect2_mocap" and row[4] == "/kinect2_link"):
exists_mat_mocap_2_link = True
mat_mocap_2_link = matrix_from_row(row)
if (not exists_mat_link_2_rgb) and (row[3] == "kinect2_link" and row[4] == "kinect2_rgb_optical_frame"):
exists_mat_link_2_rgb = True
mat_link_2_rgb = matrix_from_row(row)
if (not exists_mat_optical) and (row[3] == "kinect2_rgb_optical_frame" and row[4] == "kinect2_ir_optical_frame"):
exists_mat_optical = True
mat_optical = matrix_from_row(row)
if exist_mat_w_2_mocap and exists_mat_mocap_2_link and exists_mat_link_2_rgb and exists_mat_optical:
break
else:
print ("Warning! No transformation in world found")
# print mat_w_2_mocap, mat_mocap_2_link, mat_link_2_rgb
# return mat_mocap_2_link.dot(transformations.inverse_matrix(mat_link_2_rgb.dot(mat_w_2_mocap)))
return mat_w_2_mocap.dot(mat_mocap_2_link.dot(mat_link_2_rgb.dot(mat_optical)))
def test_inverse_homogenous_transform_non_zero_translation(self):
"""
tests whether inverse_homogeneous_transform performs the inversion correctly for T with non-zero translation vector.
"""
T = tr.identity_matrix()
T[0:3, 3] = np.array([1, 1, 1])
T_inv_expected = tr.identity_matrix()
T_inv_expected[0:3, 3] = np.array([-1, -1, -1])
T_inv = inverse_homogeneous_transform(T)
np.testing.assert_almost_equal(T_inv, T_inv_expected)
def fill_from_Orient(orient_data):
'''Fill messages with information from 'orientation' MTData block.
'''
self.pub_imu = True
if 'quaternion' in orient_data:
w, x, y, z = orient_data['quaternion']
elif 'roll' in orient_data:
x, y, z, w = quaternion_from_euler(
radians(orient_data['roll']),
radians(orient_data['pitch']), radians(orient_data['yaw']))
elif 'matrix' in orient_data:
m = identity_matrix()
m[:3, :3] = orient_data['matrix']
x, y, z, w = quaternion_from_matrix(m)
w, x, y, z = convert_quat((w, x, y, z), o['frame'])
self.imu_msg.orientation.x = x
self.imu_msg.orientation.y = y
self.imu_msg.orientation.z = z
self.imu_msg.orientation.w = w
self.imu_msg.orientation_covariance = self.orientation_covariance
self.pub_euler = True
if 'roll' in orient_data:
self.euler_msg.yaw = orient_data['yaw']
self.euler_msg.pitch = orient_data['pitch']
self.euler_msg.roll = orient_data['roll']
def rvec_and_tvec_to_matrix(rvec, tvec):
"""Rodrigues rotation and translation vector to 4x4 matrix"""
t_matrix = tft.translation_matrix(tvec)
R, _ = cv2.Rodrigues(rvec)
r_matrix = tft.identity_matrix()
r_matrix[:3, :3] = R
return np.dot(t_matrix, r_matrix)
def __init__(self, program_file=PROGRAM_FILE):
# TODO: Either implement behavior that fixes programs when markers change
# or only let this callback run once
self._markers_sub = rospy.Subscriber(SUB_NAME,
Marker,
callback=self._markers_callback)
self._curr_markers = None
self._tf_listener = tf.TransformListener()
self._arm = fetch_api.Arm()
self._gripper = fetch_api.Gripper()
self._torso = fetch_api.Torso()
self._controller_client = actionlib.SimpleActionClient(
'/query_controller_states', QueryControllerStatesAction)
self._program_file = program_file
self._programs = self._read_in_programs()
self._joint_reader = JointStateReader()
mat = tft.identity_matrix()
mat[:, 0] = np.array([0, 0, -1, 0])
mat[:, 2] = np.array([1, 0, 0, 0])
o = tft.quaternion_from_matrix(mat)
self._constraint_pose = Pose(orientation=Quaternion(*o))
oc = OrientationConstraint()
oc.header.frame_id = 'base_link'
oc.link_name = 'gripper_link'
oc.orientation = self._constraint_pose.orientation
oc.weight = 1.0
oc.absolute_z_axis_tolerance = 1.0
oc.absolute_x_axis_tolerance = 1.0
oc.absolute_y_axis_tolerance = 1.0
self._constraint = None
def fill_from_Orientation_Data(o): '''Fill messages with information from 'Orientation Data' MTData2 block.''' self.pub_imu = True try: x, y, z, w = o['Q1'], o['Q2'], o['Q3'], o['Q0'] except KeyError: pass try: # FIXME check that Euler angles are in radians x, y, z, w = quaternion_from_euler(radians(o['Roll']), radians(o['Pitch']), radians(o['Yaw'])) except KeyError: pass try: a, b, c, d, e, f, g, h, i = o['a'], o['b'], o['c'], o['d'],\ o['e'], o['f'], o['g'], o['h'], o['i'] m = identity_matrix() m[:3,:3] = ((a, b, c), (d, e, f), (g, h, i)) x, y, z, w = quaternion_from_matrix(m) except KeyError: pass self.imu_msg.orientation.x = x self.imu_msg.orientation.y = y self.imu_msg.orientation.z = z self.imu_msg.orientation.w = w self.imu_msg.orientation_covariance = (radians(1.), 0., 0., 0., radians(1.), 0., 0., 0., radians(9.))
def coord_transform(self, v__fin, fin, fout):
''' transform vector v which is represented in frame fin into its representation in frame fout
Args:
- v__fin: 3D vector represented in fin coordinates
- fin: string describing input coordinate frame (bd, bu, fc, dc, lned, lenu)
- fout: string describing output coordinate frame (bd, bu, fc, dc, lned, lenu)
Returns
- v__fout: vector v represent in fout coordinates
'''
# trivial transform, checking input shape
if fin==fout:
v4__fin = list(v__fin)+[0.0]
R = tft.identity_matrix()
v4__fout = np.dot(R, v4__fin)
v__fout = np.array(v4__fout[0:3])
return v__fout
# check for existence of rotation matrix
R_str = 'R_{}2{}'.format(fin, fout)
try:
R_i2o = getattr(self, R_str)
except AttributeError:
err = 'No static transform exists from {} to {}.'.format(fin, fout)
err += ' Are you sure these frames are not moving relative to each other?'
raise AttributeError(err)
# perform transform
v4__fin = list(v__fin) + [0.0]
v4__fout = np.dot(R_i2o, v4__fin)
v__fout = np.array(v4__fout[0:3])
return v__fout
def fill_from_Orientation_Data(o):
'''Fill messages with information from 'Orientation Data' MTData2 block.'''
global pub_imu, imu_msg
pub_imu = True
try:
x, y, z, w = o['Q1'], o['Q2'], o['Q3'], o['Q0']
except KeyError:
pass
try:
# FIXME check that Euler angles are in radians
x, y, z, w = quaternion_from_euler(radians(o['Roll']),
radians(o['Pitch']),
radians(o['Yaw']))
except KeyError:
pass
try:
a, b, c, d, e, f, g, h, i = o['a'], o['b'], o['c'], o['d'],\
o['e'], o['f'], o['g'], o['h'], o['i']
m = identity_matrix()
m[:3, :3] = ((a, b, c), (d, e, f), (g, h, i))
x, y, z, w = quaternion_from_matrix(m)
except KeyError:
pass
imu_msg.orientation.x = x
imu_msg.orientation.y = y
imu_msg.orientation.z = z
imu_msg.orientation.w = w
imu_msg.orientation_covariance = (radians(1.), 0., 0., 0.,
radians(1.), 0., 0., 0.,
radians(9.))
def getSkeletonTransformation(user_id, tf, tf_name, world_name, last_updated):
"""
Return the rototranslation matrix between tf_name and world_name
and the time stamp
@param user_id id of the human
@param tf
@param tf_name name of the element in the tf tree
@param world_name name of the world in the tf tree
@param last_update time stamp of the last time the tf tree was updated
"""
tf_name_full = getFullTfName(user_id, tf_name)
if tf.frameExists(tf_name_full) and tf.frameExists(world_name):
try:
time = tf.getLatestCommonTime(tf_name_full, world_name)
pos, quat = tf.lookupTransform(world_name, tf_name_full, time)
except:
return last_updated, None
last_updated = time.secs if last_updated <= time.secs else last_updated
r = transformations.quaternion_matrix(quat)
skel_trans = transformations.identity_matrix()
skel_trans[0:4,0:4] = r
skel_trans[0:3, 3] = pos
return last_updated, skel_trans
else:
return last_updated, None
def fill_from_Orientation_Data(o):
'''Fill messages with information from 'Orientation Data' MTData2
block.'''
self.pub_imu = True
try:
x, y, z, w = o['Q1'], o['Q2'], o['Q3'], o['Q0']
except KeyError:
pass
try:
x, y, z, w = quaternion_from_euler(radians(o['Roll']),
radians(o['Pitch']),
radians(o['Yaw']))
except KeyError:
pass
try:
a, b, c, d, e, f, g, h, i = o['a'], o['b'], o['c'], o['d'],\
o['e'], o['f'], o['g'], o['h'], o['i']
m = identity_matrix()
m[:3, :3] = ((a, b, c), (d, e, f), (g, h, i))
x, y, z, w = quaternion_from_matrix(m)
except KeyError:
pass
w, x, y, z = convert_quat((w, x, y, z), o['frame'])
self.imu_msg.orientation.x = x
self.imu_msg.orientation.y = y
self.imu_msg.orientation.z = z
self.imu_msg.orientation.w = w
self.imu_msg.orientation_covariance = self.orientation_covariance
def test_inverse_homogenous_transform_trivial(self):
"""
tests whether inverse_homogeneous_transform yields T = inv(T) when T is the identity matrix.
"""
T = tr.identity_matrix()
T_inv = inverse_homogeneous_transform(T)
np.testing.assert_almost_equal(T, T_inv)
def root_transform(self):
multi_matrix = identity_matrix()
for node in self.path_to_root:
matrix = fromStringToMatrix(node.transform)
multi_matrix = dot(matrix, multi_matrix)
root_transform = fromMatrixToTransform(multi_matrix)
return root_transform
def __init__(self):
self.subscriber = rospy.Subscriber("input_odom",
Odometry,
self.callback,
queue_size=1)
self.publisher = rospy.Publisher("output_odom", Odometry, queue_size=1)
self.T_NED_V = TR.euler_matrix(pi, 0, 0, 'rxyz')
self.T_XAV_UAV = TR.euler_matrix(pi, 0, 0, 'rxyz')
self.T_XAV_V = TR.identity_matrix() # the output transform
def __init__(self):
self.node_name = 'pose2d_to_path_node'
# Store the frames used to link the robot and vicon paths
self.Tv_w = tr.identity_matrix()
self.Tw_r1 = tr.identity_matrix()
self.adjVicon = True
self.adjRobot = True
# setup publishers and subscribers
self.sub_vicon = rospy.Subscriber("~vicon_pose", PoseStamped, self.viconPoseCallback)
self.sub_pose = rospy.Subscriber("~pose", Pose2DStamped, self.poseCallback)
self.sub_weights = rospy.Subscriber("~weights", KinematicsWeights, self.weightsCallback)
self.pub_path = rospy.Publisher("~path", Path, queue_size=1)
# Setup the path message
self.msg_path = Path()
rospy.loginfo("[%s] has started!", self.node_name)
def fill_from_Orient(orient_data):
'''Fill messages with information from 'orientation' MTData block.
'''
self.pub_imu = True
if 'quaternion' in orient_data:
w, x, y, z = orient_data['quaternion']
elif 'roll' in orient_data:
x, y, z, w = quaternion_from_euler(
radians(orient_data['roll']),
radians(orient_data['pitch']), radians(orient_data['yaw']))
elif 'matrix' in orient_data:
m = identity_matrix()
m[:3, :3] = orient_data['matrix']
x, y, z, w = quaternion_from_matrix(m)
self.imu_msg.orientation.x = x
self.imu_msg.orientation.y = y
self.imu_msg.orientation.z = z
self.imu_msg.orientation.w = w
self.imu_msg.orientation_covariance = (radians(1.), 0., 0., 0.,
radians(1.), 0., 0., 0.,
radians(9.))
self.view_msg.pose.position.x = 0
self.view_msg.pose.position.y = 0
self.view_msg.pose.position.z = 0
self.view_msg.pose.orientation.x = x
self.view_msg.pose.orientation.y = y
self.view_msg.pose.orientation.z = z
self.view_msg.pose.orientation.w = w
y2 = y**2
t0 = -2 * (y2 + z**2) + 1
t1 = 2 * (x * y - w) * z
t2 = -2 * (x * z + w) * y
t3 = 2 * (y * z - w) * x
t4 = -2 * (x**2 + y2) + 1
if t2 > 1:
t2 = 1
if t2 < -1:
t2 = -1
pitch = math.asin(t2)
roll = math.atan2(t3, t4)
yaw = 180 * (math.atan2(t1, t0) - math.pi / 2) / math.pi + 90
'''0.02495820 Sevilla'''
'''-0.0083775 Toulouse'''
#s = 'Roll: ' + str(roll) + ' , Pitch: ' + str(pitch) + ' , Yaw: ' + str(yaw)
#print s
ang = math.acos(math.fabs(math.cos(pitch) * math.cos(roll)))
#s = 'ANG = ' + str(ang)
#print s
self.ang_msg.data = ang
self.ang_msg2.data = pitch
self.ang_msg3.data = roll
def cartesian_to_ik(self, cartesian, single_sol=False): # Cartesian coordinates of [roll,pitch,yaw,X,Y,Z] to ik solutions # Returns all possible IK solutions within joint limits if single is False # Returns best IK solution within joint limit if single is True self.single_sol = single_sol roll,pitch,yaw,X,Y,Z = cartesian matrix_from_euler = TF.euler_matrix(roll,pitch,yaw) matrix_from_translation = TF.translation_matrix([X,Y,Z]) - TF.identity_matrix() matrix_from_cartesian = matrix_from_euler + matrix_from_translation return self.__get_ik_from_matrix(matrix_from_cartesian)
def __init__(self):
self.node_name = 'pose2d_to_path_node'
# Store the frames used to link the robot and vicon paths
self.Tv_w = tr.identity_matrix()
self.Tw_r1 = tr.identity_matrix()
self.adjVicon = True
self.adjRobot = True
# setup publishers and subscribers
self.sub_vicon = rospy.Subscriber("~vicon_pose", PoseStamped,
self.viconPoseCallback)
self.sub_pose = rospy.Subscriber("~pose", Pose2DStamped,
self.poseCallback)
self.sub_weights = rospy.Subscriber("~weights", KinematicsWeights,
self.weightsCallback)
self.pub_path = rospy.Publisher("~path", Path, queue_size=1)
# Setup the path message
self.msg_path = Path()
rospy.loginfo("[%s] has started!", self.node_name)
def test_inverse_homogenous_transform_translation_rotation(self):
"""
tests whether inverse_homogeneous_transform performs the inversion correctly for
T with non-zero translation vector and a non-identity rotation matrix.
"""
# Spesifically tests matrix inversion used in the function.
a_T_b = tr.identity_matrix()
a_T_b[0:3, 0:3] = np.array(
[[0, -1, 0], [1, 0, 0], [0, 0, 1]]
) # rotation around z axis by 90 degrees. frame B is rotated with respect to az
a_T_b[0:3, 3] = np.array(
[1, 1, 0]
) # translation by 1 unit in positive x. translation from from frame A to frame B expressed in frame A.
b_T_a_exp = tr.identity_matrix()
b_T_a_exp[0:3,
3] = np.matmul(np.array([[0, 1, 0], [-1, 0, 0], [0, 0, 1]]),
-np.array([1, 1, 0]))
b_T_a_exp[0:3, 0:3] = np.array([[0, 1, 0], [-1, 0, 0],
[0, 0, 1]]) # R_inv * -t
T_inv = inverse_homogeneous_transform(a_T_b)
np.testing.assert_almost_equal(T_inv, b_T_a_exp)
def inverse_homogeneous_transform(T):
""" Inverts a homogeneous transformation matrix using the formula 2.92 of [1]. """
t = T[0:3, 3]
R = T[0:3, 0:3]
T_inv = tr.identity_matrix()
R_inv = np.transpose(R)
t_inv = np.matmul(R_inv, -t)
T_inv[0:3, 0:3] = R_inv
T_inv[0:3, 3] = t_inv
return T_inv
def _new_frame_callback(self, message):
if self._frame_num % self._skip_frames == 0:
data = point_cloud_to_array(message)[self._base_markers,:,0]
#Remove markers which are not visible
visible = np.where(np.logical_not(np.isnan(data[:,0])))[0]
data = data[visible, :]
#Compute the centroid of the observed points
centroid = np.mean(data, axis=0)
#Compute the translation and rotation components
translation = centroid.squeeze()
rotation = convert.quaternion_from_matrix(convert.identity_matrix())
#Publish the transform
self.publish_transform(translation, rotation)
#Compute the transformation matrix
homog = convert.identity_matrix()
homog[0:3,3] = translation
#Transform points into rigid body frame
homog_data = np.vstack((data.T, np.ones((1, data.shape[0]))))
desired = la.inv(homog).dot(homog_data)
print(desired)
#Save the marker arrangement if all markers are visible
if desired.shape[1] == len(self._base_markers):
try:
save_rigid_body_config(args.data_file, 'test_body', self._base_markers, desired[0:3,:])
except IOError:
print('Error: Unable to save rigid body config')
rospy.signal_shutdown('Captured all markers in a single frame')
self._frame_num += 1
def __init__(self):
self.subscriber = rospy.Subscriber("vicon",
PoseStamped,
self.callback,
queue_size=1)
self.reference_pub = rospy.Publisher("reference",
Odometry,
queue_size=1)
self.ext_att_pub = rospy.Publisher("external_attitude",
Quaternion,
queue_size=1)
self.T_V_NED = TR.euler_matrix(pi, 0, 0, 'rxyz')
# self.T_XAV_V = TR.identity_matrix() # from subscriber callback
self.T_UAV_XAV = TR.euler_matrix(pi, 0, 0, 'rxyz')
self.T_UAV_NED = TR.identity_matrix() # the output transform
def forward(self, qs, start=0, end=5):
""" FK problem """
h = tt.identity_matrix()
for i in range(start, end):
rz = tt.rotation_matrix(self.theta[i] - qs[i], (0, 0, 1))
tz = tt.translation_matrix((0, 0, self.d[i]))
tx = tt.translation_matrix((self.a[i], 0, 0))
rx = tt.rotation_matrix(self.alpha[i], (1, 0, 0))
a = tt.concatenate_matrices(rz, tz, tx, rx)
h = tt.concatenate_matrices(h, a)
out = "%f\t%f\t%f\t%f + %f" % (self.a[i], self.alpha[i],
self.d[i], qs[i], self.theta[i])
rospy.logdebug(out)
xyz = h[:3, 3]
qtn = tt.quaternion_from_matrix(h)
rpy = tt.euler_from_matrix(h[:3, :3], axes='sxyz')
return xyz, qtn, rpy, h
def quat_from_orient(orient): '''Build a quaternion from orientation data.''' try: w, x, y, z = orient['quaternion'] return (x, y, z, w) except KeyError: pass try: return quaternion_from_euler(pi*orient['roll']/180., pi*orient['pitch']/180, pi*orient['yaw']/180.) except KeyError: pass try: m = identity_matrix() m[:3,:3] = orient['matrix'] return quaternion_from_matrix(m) except KeyError: pass
def quat_from_orient(orient): '''Build a quaternion from orientation data.''' try: w, x, y, z = orient['quaternion'] return (x, y, z, w) except KeyError: pass try: return quaternion_from_euler(pi*orient['roll']/180., pi*orient['pitch']/180, pi*orient['yaw']/180.) except KeyError: pass try: m = identity_matrix() m[:3,:3] = orient['matrix'] return quaternion_from_matrix(m) except KeyError: pass
def fill_from_Orient(orient_data):
'''Fill messages with information from 'orientation' MTData block.'''
self.pub_imu = True
if orient_data.has_key('quaternion'):
w, x, y, z = orient_data['quaternion']
elif orient_data.has_key('roll'):
# FIXME check that Euler angles are in radians
x, y, z, w = quaternion_from_euler(radians(orient_data['roll']),
radians(orient_data['pitch']), radians(orient_data['yaw']))
elif orient_data.has_key('matrix'):
m = identity_matrix()
m[:3,:3] = orient_data['matrix']
x, y, z, w = quaternion_from_matrix(m)
self.imu_msg.orientation.x = x
self.imu_msg.orientation.y = y
self.imu_msg.orientation.z = z
self.imu_msg.orientation.w = w
self.imu_msg.orientation_covariance = (radians(1.), 0., 0., 0.,
radians(1.), 0., 0., 0., radians(9.))
def fill_from_Orient(orient_data):
'''Fill messages with information from 'orientation' MTData block.'''
self.pub_imu = True
if orient_data.has_key('quaternion'):
w, x, y, z = orient_data['quaternion']
elif orient_data.has_key('roll'):
# FIXME check that Euler angles are in radians
x, y, z, w = quaternion_from_euler(radians(orient_data['roll']),
radians(orient_data['pitch']), radians(orient_data['yaw']))
elif orient_data.has_key('matrix'):
m = identity_matrix()
m[:3,:3] = orient_data['matrix']
x, y, z, w = quaternion_from_matrix(m)
self.imu_msg.orientation.x = x
self.imu_msg.orientation.y = y
self.imu_msg.orientation.z = z
self.imu_msg.orientation.w = w
self.imu_msg.orientation_covariance = (radians(1.), 0., 0., 0.,
radians(1.), 0., 0., 0., radians(9.))
def fill_from_Orient(orient_data):
'''Fill messages with information from 'orientation' MTData block.
'''
self.pub_imu = True
if 'quaternion' in orient_data:
w, x, y, z = orient_data['quaternion']
elif 'roll' in orient_data:
x, y, z, w = quaternion_from_euler(
radians(orient_data['roll']), radians(orient_data['pitch']),
radians(orient_data['yaw']))
elif 'matrix' in orient_data:
m = identity_matrix()
m[:3, :3] = orient_data['matrix']
x, y, z, w = quaternion_from_matrix(m)
w, x, y, z = convert_quat((w, x, y, z), o['frame'])
self.imu_msg.orientation.x = x
self.imu_msg.orientation.y = y
self.imu_msg.orientation.z = z
self.imu_msg.orientation.w = w
self.imu_msg.orientation_covariance = self.orientation_covariance
def fill_from_Orient(orient_data):
'''Fill messages with information from 'orientation' MTData block.
'''
self.pub_imu = True
if 'quaternion' in orient_data:
w, x, y, z = orient_data['quaternion']
elif 'roll' in orient_data:
x, y, z, w = quaternion_from_euler(
radians(orient_data['roll']),
radians(orient_data['pitch']), radians(orient_data['yaw']))
elif 'matrix' in orient_data:
m = identity_matrix()
m[:3, :3] = orient_data['matrix']
x, y, z, w = quaternion_from_matrix(m)
self.imu_msg.orientation.x = x
self.imu_msg.orientation.y = y
self.imu_msg.orientation.z = z
self.imu_msg.orientation.w = w
self.imu_msg.orientation_covariance = (radians(1.), 0., 0., 0.,
radians(1.), 0., 0., 0.,
radians(9.))
def forward_DH(self, q=None, start=1, end=None):
if q is None:
q = self.q
if end is None:
end = len(self.q)
q[4] = 0.4
H = transforms.identity_matrix()
for i in range(start, end):
Rz = transforms.rotation_matrix(self.DH[i][3] + q[i], (0, 0, 1))
Tz = transforms.translation_matrix((0, 0, self.DH[i][2]))
Tx = transforms.translation_matrix((self.DH[i][1], 0, 0))
Rx = transforms.rotation_matrix(self.DH[i][0], (1, 0, 0))
A = transforms.concatenate_matrices(Rz, Tz, Tx, Rx)
print(A)
H = transforms.concatenate_matrices(H, A)
#out = "%f\t%f\t%f\t%f + %f" % (self.DH[i][1], self.DH[i][0],
#self.DH[i][2], self.q[i], self.DH[i][3])
#rospy.logdebug(out)
xyz = H[:3, 3]
qtn = transforms.quaternion_from_matrix(H)
rpy = transforms.euler_from_matrix(H[:3, :3], axes='sxyz')
print(H)
# check #################
br = TransformBroadcaster()
ls = tf.TransformListener()
try:
ls.waitForTransform("tool0", "iiwa_base_link", rospy.Time(0),
rospy.Duration(4.0))
(pos, rot) = ls.lookupTransform('iiwa_base_link', 'tool0',
rospy.Time())
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
print("can't get iiwa_base_link <- tool0 transform")
return -1
print(pos)
##########################
return xyz, qtn, rpy, H
def start(self):
mat = tft.identity_matrix()
mat[:, 0] = np.array([0, 0, -1, 0])
mat[:, 2] = np.array([1, 0, 0, 0])
o = tft.quaternion_from_matrix(mat)
self._constraint_pose = Pose(orientation=Quaternion(*o))
gripper_im = create_gripper_interactive_marker(self._constraint_pose,
pregrasp=False,
rotation_enabled=False)
self._im_server.insert(gripper_im, feedback_cb=self.handle_feedback)
self._im_server.applyChanges()
oc = OrientationConstraint()
oc.header.frame_id = 'base_link'
oc.link_name = 'gripper_link'
oc.orientation = self._constraint_pose.orientation
oc.weight = 1.0
oc.absolute_z_axis_tolerance = 0.1
oc.absolute_x_axis_tolerance = 0.1
oc.absolute_y_axis_tolerance = 0.1
self.contraint = oc
def marker_inf_cb(self, data):
print "in marker callback"
q = Quaternion()
q = data.pose.orientation
marker_orientation = np.array([q.x, q.y, q.z, q.w])
euler_angles = tr.euler_from_quaternion(marker_orientation, 'rxyz')
vec3 = Vector3()
vec3.x = data.pose.position.x
vec3.y = data.pose.position.y
vec3.z = data.pose.position.z
marker_pose = np.array([
vec3.x, vec3.y, vec3.z, euler_angles[0], euler_angles[1],
euler_angles[2]
])
for i in range(6):
if np.isnan(marker_pose[i]):
return
cam_object_T = self.set_cam_object_T(marker_pose)
cam_object_R = tr.identity_matrix()
cam_object_R[0:3, 0:3] = cam_object_T[0:3, 0:3]
cam_object_q = tr.quaternion_from_matrix(cam_object_R)
self.br.sendTransform(
(cam_object_T[0, 3], cam_object_T[1, 3], cam_object_T[2, 3]),
cam_object_q, rospy.Time.now(), "object_in_cam_frame",
"camera_rgb_optical_frame")
# publishing the object marker to rviz for object_in_cam
det_marker = Marker()
det_marker.header.frame_id = "camera_rgb_optical_frame"
det_marker.header.stamp = rospy.Time.now()
det_marker.type = Marker.SPHERE
det_marker.pose.position.x = cam_object_T[0, 3]
det_marker.pose.position.y = cam_object_T[1, 3]
det_marker.pose.position.z = cam_object_T[2, 3]
det_marker.pose.orientation.x = 0
det_marker.pose.orientation.y = 0
det_marker.pose.orientation.z = 0
det_marker.pose.orientation.w = 1
det_marker.scale.x = 0.05
det_marker.scale.y = 0.05
det_marker.scale.z = 0.05
det_marker.color.a = 1.0
det_marker.color.r = 1.0
det_marker.color.g = 0.0
det_marker.color.b = 0.0
self.marker_object_in_cam_pub.publish(det_marker)
base_object_T = np.dot(np.dot(self.base_end_T, self.end_cam_T),
cam_object_T)
# print base_object_T
final_err = self.error_computation(base_object_T, self.base_target_T)
# target thresholds:
trans_th = [0.05, 0.05, 0.05]
rot_th = [15 * np.pi / 180, 15 * np.pi / 180, 15 * np.pi / 180]
print "timer: ", self.timer
if self.timer > 5:
print "goal reached ............................"
self.gripper_final_action()
if np.abs(final_err[0]) < trans_th[0] and np.abs(
final_err[1]) < trans_th[1] and np.abs(
final_err[2]) < trans_th[2]:
if np.abs(final_err[3]) < rot_th[0] and np.abs(
final_err[4]) < rot_th[1] and np.abs(
final_err[5]) < rot_th[2]:
self.timer += 1
return
else:
self.timer = 0
print "error :", final_err
else:
self.timer = 0
print "error :", final_err
# use of error_gains:
error_gains = np.reshape(np.array([1, 1, 1, 1, 1, 1]), (6, 1))
final_err2 = np.multiply(final_err, error_gains)
# error value type 2
end_object_T = np.dot(self.end_cam_T, cam_object_T)
end_target_T = np.dot(self.end_grip_T, self.grip_target_T)
object_target_T = np.dot(end_object_T, np.linalg.inv(end_target_T))
# print "error style 2: ", object_target_T
# publishing the object marker to rviz
det_marker = Marker()
det_marker.header.frame_id = "iiwa_base_link"
det_marker.header.stamp = rospy.Time.now()
det_marker.type = Marker.SPHERE
det_marker.pose.position.x = base_object_T[0, 3]
det_marker.pose.position.y = base_object_T[1, 3]
det_marker.pose.position.z = base_object_T[2, 3]
det_marker.pose.orientation.x = 0
det_marker.pose.orientation.y = 0
det_marker.pose.orientation.z = 0
det_marker.pose.orientation.w = 1
det_marker.scale.x = 0.05
det_marker.scale.y = 0.05
det_marker.scale.z = 0.05
det_marker.color.a = 1.0
det_marker.color.r = 0.0
det_marker.color.g = 0.0
det_marker.color.b = 1.0
self.marker_object_in_base_pub.publish(det_marker)
# print "final error is : ", final_err
# print "base_target_T: ", base_object_T
# computation of jacobian of the manipulator
J = self.kdl_kin.jacobian(self.end_eff_current_pose)
if self.used_method == "DLSM":
# use of damped_least_squares as matrix inverse
J_inverse = np.dot(
J.T,
np.linalg.inv(
np.dot(J, J.T) + ((self.landa**2) * np.identity(6))))
elif self.used_method == "JPM":
# use of matrix pseudo_inverse as matrix inverse
J_inverse = np.linalg.pinv(J)
elif self.used_method == "JTM":
# use of matrix transpose as matrix inverse
J_inverse = np.transpose(J)
final_vel = np.dot(J_inverse, final_err2)
# the dereived final speed command for the manipulator
speed_cmd = list(np.array(final_vel).reshape(-1, ))
# avoid huge joint velocities:
joint_vel_th = [0.7, 0.7, 0.7, 0.7, 0.7, 0.7, 0.7]
if speed_cmd[0] > joint_vel_th[0]:
print "huge velocity for the first joint!"
return
elif speed_cmd[1] > joint_vel_th[1]:
print "huge velocity for the second joint!"
return
elif speed_cmd[2] > joint_vel_th[2]:
print "huge velocity for the third joint!"
return
elif speed_cmd[3] > joint_vel_th[3]:
print "huge velocity for the fourth joint!"
return
elif speed_cmd[4] > joint_vel_th[4]:
print "huge velocity for the fifth joint!"
return
elif speed_cmd[5] > joint_vel_th[5]:
print "huge velocity for the sixth joint!"
return
elif speed_cmd[6] > joint_vel_th[6]:
print "huge velocity for the seventh joint!"
return
# block to handle speed publisher using pose publisher
intergration_time_step = 0.5
pose_change = [speed * intergration_time_step for speed in speed_cmd]
final_pose = [
a + b for a, b in zip(self.end_eff_current_pose, pose_change)
]
# actionlib commands
self.trajectory.points[0].positions = final_pose
self.trajectory.points[0].velocities = [0.0] * len(self.joint_names)
self.trajectory.points[0].accelerations = [0.0] * len(self.joint_names)
self.trajectory.points[0].time_from_start = rospy.Duration(1.0)
# print "pose_change", pose_change
# print "The manipulator is moving..."
goal = FollowJointTrajectoryGoal()
goal.trajectory = self.trajectory
goal.goal_time_tolerance = rospy.Duration(0.0)
self.manipulator_client.send_goal(goal)
action_result = self.manipulator_client.wait_for_result()
if action_result is True:
# publishing the path of the object in "red" color
final_object_position = base_object_T[0:3, 3]
final_object_R = tr.identity_matrix()
final_object_R[0:3, 0:3] = base_object_T[0:3, 0:3]
final_object_quaternion = tr.quaternion_from_matrix(final_object_R)
self.object_path.markers.append(Marker())
self.object_path.markers[-1].header.frame_id = "iiwa_base_link"
self.object_path.markers[-1].header.stamp = rospy.Time.now()
self.object_path.markers[-1].id = len(self.object_path.markers)
self.object_path.markers[-1].type = Marker.SPHERE
self.object_path.markers[
-1].pose.position.x = final_object_position[0]
self.object_path.markers[
-1].pose.position.y = final_object_position[1]
self.object_path.markers[
-1].pose.position.z = final_object_position[2]
self.object_path.markers[
-1].pose.orientation.x = final_object_quaternion[0]
self.object_path.markers[
-1].pose.orientation.y = final_object_quaternion[1]
self.object_path.markers[
-1].pose.orientation.z = final_object_quaternion[2]
self.object_path.markers[
-1].pose.orientation.w = final_object_quaternion[3]
self.object_path.markers[-1].scale.x = 0.01
self.object_path.markers[-1].scale.y = 0.01
self.object_path.markers[-1].scale.z = 0.01
self.object_path.markers[-1].color.a = 1.0
self.object_path.markers[-1].color.r = 1.0
self.object_path.markers[-1].color.g = 0.0
self.object_path.markers[-1].color.b = 0.0
self.marker_obj_path_pub.publish(self.object_path)
# publishing path of the end effector in "green" color
final_ee_position = self.base_end_T[0:3, 3]
final_ee_R = tr.identity_matrix()
final_ee_R[0:3, 0:3] = self.base_end_T[0:3, 0:3]
final_ee_quaternion = tr.quaternion_from_matrix(final_ee_R)
self.ee_path.markers.append(Marker())
self.ee_path.markers[-1].header.frame_id = "iiwa_base_link"
self.ee_path.markers[-1].header.stamp = rospy.Time.now()
self.ee_path.markers[-1].id = len(self.ee_path.markers)
self.ee_path.markers[-1].type = Marker.SPHERE
self.ee_path.markers[-1].pose.position.x = final_ee_position[0]
self.ee_path.markers[-1].pose.position.y = final_ee_position[1]
self.ee_path.markers[-1].pose.position.z = final_ee_position[2]
self.ee_path.markers[-1].pose.orientation.x = final_ee_quaternion[
0]
self.ee_path.markers[-1].pose.orientation.y = final_ee_quaternion[
1]
self.ee_path.markers[-1].pose.orientation.z = final_ee_quaternion[
2]
self.ee_path.markers[-1].pose.orientation.w = final_ee_quaternion[
3]
self.ee_path.markers[-1].scale.x = 0.01
self.ee_path.markers[-1].scale.y = 0.01
self.ee_path.markers[-1].scale.z = 0.01
self.ee_path.markers[-1].color.a = 1.0
self.ee_path.markers[-1].color.r = 0.0
self.ee_path.markers[-1].color.g = 1.0
self.ee_path.markers[-1].color.b = 0.0
self.marker_eff_path_pub.publish(self.ee_path)
def get_current_pose(self):
frame_described_in = str(self.frameline.text())
left = ('Left' == str(self.arm_box.currentText()))
right = False
if not left:
right = True
arm_tip_frame = LinearMoveTool.RIGHT_TIP
else:
arm_tip_frame = LinearMoveTool.LEFT_TIP
self.tf_listener.waitForTransform(frame_described_in, arm_tip_frame, rospy.Time(), rospy.Duration(2.))
p_arm = tfu.tf_as_matrix(self.tf_listener.lookupTransform(frame_described_in, arm_tip_frame, rospy.Time(0))) * tr.identity_matrix()
trans, rotation = tr.translation_from_matrix(p_arm), tr.quaternion_from_matrix(p_arm)
for value, vr in zip(trans, [self.xline, self.yline, self.zline]):
vr.setText(str(value))
for value, vr in zip(tr.euler_from_quaternion(rotation), [self.phi_line, self.theta_line, self.psi_line]):
vr.setText(str(np.degrees(value)))
# test if the marker is tracked or not, skip
print 'vicon pos %.4f %.4f %.4f' % tuple(vmpos), 'robot pos %.4f %.4f %.4f' % tuple(vmpos_robot)
if norm(vmpos) < 1e-9:
print 'vicon pos is bad, not using this data'
continue
xs = xs + [vmpos[0]]
ys = ys + [vmpos[1]]
zs = zs + [vmpos[2]]
xt = xt + [vmpos_robot[0]]
yt = yt + [vmpos_robot[1]]
zt = zt + [vmpos_robot[2]]
print 'data length', len(xs)
plt.scatter(xs, ys, c='r', marker='o')
plt.scatter(xt, yt, c='b', marker='o')
plt.show()
viconpts = np.vstack([xs, ys, zs]).T
robotpts = np.vstack([xt, yt, zt]).T
(R,t,rmse) = rigid_transform_3D(viconpts, robotpts) # then you'll get vicon frame wrt robot frame
Rh = tfm.identity_matrix()
Rh[np.ix_([0,1,2],[0,1,2])] = R
quat = tfm.quaternion_from_matrix(Rh)
print 'vicon_T_robot:', " ".join('%.8e' % x for x in (t.tolist() + quat.tolist()))
print 'rmse:', rmse
def space_coordinates(self, joints):
T = tfs.identity_matrix()
for idx in range(0, self.dof):
T = tfs.concatenate_matrices(T, self.transformations[idx](joints[idx]))
return self.space_from_matrix(T)
def error_computation(self, base_object, base_target):
position_err = base_object[0:3, 3] - base_target[0:3, 3]
# computation of orientation_err using the skew symmetric matrix
# the order for the result quaternion from tr package is x, y, z, w
# no need to change the order in goal_q and ee_q
cos = np.cos
sin = np.sin
base_object_R = tr.identity_matrix()
base_object_R[0:3, 0:3] = base_object[0:3, 0:3]
modification_matrix1 = np.array(
[[cos(4 * np.pi / 6), 0,
sin(4 * np.pi / 6), 0], [0, 1, 0, 0],
[-sin(4 * np.pi / 6), 0,
cos(4 * np.pi / 6), 0], [0, 0, 0, 1]])
modification_matrix2 = np.array([[1, 0, 0, 0],
[0, cos(np.pi), -sin(np.pi), 0],
[0, sin(np.pi),
cos(np.pi), 0], [0, 0, 0, 1]])
modified_base_object_R = np.dot(
np.dot(base_object_R, modification_matrix1), modification_matrix2)
base_object_quaternion = tr.quaternion_from_matrix(
modified_base_object_R)
goal_q = [
base_object_quaternion[0], base_object_quaternion[1],
base_object_quaternion[2], base_object_quaternion[3]
]
### object tf wrt base before modification in orientation
real_base_object_quaternion = tr.quaternion_from_matrix(base_object_R)
object_real_q = [
real_base_object_quaternion[0], real_base_object_quaternion[1],
real_base_object_quaternion[2], real_base_object_quaternion[3]
]
self.br.sendTransform(
(base_object[0, 3], base_object[1, 3], base_object[2, 3]),
object_real_q, rospy.Time.now(), "actual_object_tf",
"iiwa_base_link")
### object tf wrt base after modification in orientation
self.br.sendTransform(
(base_object[0, 3], base_object[1, 3], base_object[2, 3]), goal_q,
rospy.Time.now(), "modified_object_tf", "iiwa_base_link")
# computation of orientation error using skey_symmetric matrix
base_target_R = tr.identity_matrix()
base_target_R[0:3, 0:3] = base_target[0:3, 0:3]
base_target_quaternion = tr.quaternion_from_matrix(base_target_R)
ee_q = [
base_target_quaternion[0], base_target_quaternion[1],
base_target_quaternion[2], base_target_quaternion[3]
]
skew_mat = np.array([[0.0, -goal_q[2], goal_q[1]],
[goal_q[2], 0.0, -goal_q[0]],
[-goal_q[1], goal_q[0], 0.0]])
orientation_err = np.empty((3, 1))
for i in range(3):
orientation_err[i] = (ee_q[3] * goal_q[i]) - (
goal_q[3] * ee_q[i]) - (skew_mat[i, 0] * ee_q[0] +
skew_mat[i, 1] * ee_q[1] +
skew_mat[i, 2] * ee_q[2])
# print "orientation error: ", orientation_err
final_err = np.empty((6, 1))
final_err[0] = position_err[0]
final_err[1] = position_err[1]
final_err[2] = position_err[2]
# final_err[0] = 0
# final_err[1] = 0
# final_err[2] = 0
final_err[3] = orientation_err[0]
final_err[4] = orientation_err[1]
final_err[5] = orientation_err[2]
# final_err[3] = 0
# final_err[4] = 0
# final_err[5] = 0
return final_err
roslib.load_manifest('ptp_arm_action')
import rospy
import actionlib
import ptp_arm_action.msg as ptp
import numpy as np
from object_manipulator.convert_functions import *
import tf.transformations as tr
rospy.init_node('ptp_client')
client = actionlib.SimpleActionClient('right_ptp', ptp.LinearMovementAction)
client.wait_for_server()
#Construct goal
goal = ptp.LinearMovementGoal()
goal.relative = True
goal.trans_vel = 0.02
goal.rot_vel = 0.02
motion = tr.identity_matrix()
motion[0, 3] = .1
print motion
goal.goal = stamp_pose(mat_to_pose(motion), 'base_link')
print 'goal is\n', goal
client.send_goal(goal)
client.wait_for_result()
r = client.get_result()
print r
#print r.__class__, r
print 'done!'
#rospy.spin()
def __init__(self):
self.br = tf2_ros.TransformBroadcaster()
self.mat44_map_odom = tft.identity_matrix()
t_ = threading.Thread(target=self.tf_publish)
t_.start()
