def publish(self):
if self.last_pub and (rospy.Time.now() - self.last_pub) < self.interval:
return
(Tpose, Ttf, frames, stamp) = self.get_Tpose()
frame_id = frames[0]
if Tpose is None:
return
self.last_pub = rospy.Time.now()
Tpose_p = Tpose[0:3, 3]
Tpose_q = tft.quaternion_from_matrix(Tpose)
if self.options.pose:
pub_msg = PoseStamped()
pub_msg.header.stamp = stamp
pub_msg.header.frame_id = frame_id
pub_msg.pose.position = Point(*(Tpose_p.tolist()))
pub_msg.pose.orientation = Quaternion(*(Tpose_q.tolist()))
self.pub.publish(pub_msg)
if self.options.tf:
from_frame = frames[1]
to_frame = frames[2]
tf_p = Ttf[0:3, 3]
tf_q = tft.quaternion_from_matrix(Ttf)
if self.options.tf_always_new:
stamp = rospy.Time.now()
self.br.sendTransform(tf_p, tf_q, stamp, to_frame, from_frame)
def interpolate_cartesian(start_pose, end_pose, num_steps):
diff_pos = end_pose[:3,3] - start_pose[:3,3]
start_quat = tf_trans.quaternion_from_matrix(start_pose)
end_quat = tf_trans.quaternion_from_matrix(end_pose)
mat_list = []
for fraction in np.linspace(0, 1, num_steps):
cur_quat = tf_trans.quaternion_slerp(start_quat, end_quat, fraction)
cur_mat = np.mat(tf_trans.quaternion_matrix(cur_quat))
cur_mat[:3,3] = start_pose[:3,3] + fraction * diff_pos
mat_list.append(cur_mat)
return mat_list
def quaternion_dist(B_a, B_b):
quat_a = tf_trans.quaternion_from_matrix(B_a)
quat_b = tf_trans.quaternion_from_matrix(B_b)
quat_a_norm = quat_a / np.linalg.norm(quat_a)
quat_b_norm = quat_b / np.linalg.norm(quat_b)
dot = np.dot(quat_a_norm, quat_b_norm)
if dot > 0.99999:
dist = 0
else:
dist = np.arccos(dot)
return dist
def pack_marker(pos, _xaxis): marker = Marker() marker.header.frame_id = "/map" marker.type = marker.ARROW marker.action = marker.ADD marker.scale.x = 3.0 marker.scale.y = 0.2 marker.scale.z = 0.2 # get some likely scale marker.color.a = 1.0 # calculate q from xaxis xaxis = np.array(_xaxis) / np.linalg.norm(_xaxis) yaxis = np.cross(xaxis, [0, 1, 0]) zaxis = np.cross(xaxis, yaxis) R = np.array([list(xaxis), yaxis, zaxis]).T T = np.hstack( (R, np.zeros((3,1))) ) T = np.vstack( (T, np.array([0, 0, 0, 1]).reshape((1,4))) ) q = transformations.quaternion_from_matrix(T) marker.pose.orientation.x = q[0] marker.pose.orientation.y = q[1] marker.pose.orientation.z = q[2] marker.pose.orientation.w = q[3] marker.pose.position.x = pos[0] marker.pose.position.y = pos[1] marker.pose.position.z = pos[2] return marker
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 numpy_matrix_to_quaternion(np_matrix):
'''Given a 3x3 rotation matrix, return its geometry_msgs Quaternion'''
assert np_matrix.shape == (3, 3), "Must submit 3x3 rotation matrix"
pose_mat = np.eye(4)
pose_mat[:3, :3] = np_matrix
np_quaternion = transformations.quaternion_from_matrix(pose_mat)
return geometry_msgs.Quaternion(*np_quaternion)
def pack_pose(time, child, parent, matrix=None, trans=None, quat=None):
if matrix is not None and (trans is None and quat is None):
trans = tfs.translation_from_matrix(matrix)
quat = tfs.quaternion_from_matrix(matrix)
elif matrix is None and trans is not None and quat is not None:
matrix = None # nothing
else:
print 'invalid use'
t = TransformStamped()
t.header.frame_id = parent
t.header.stamp = time
t.child_frame_id = child
t.transform.translation.x = trans[0]
t.transform.translation.y = trans[1]
t.transform.translation.z = trans[2]
quat = numpy.array(quat)
quat = quat / numpy.linalg.norm(quat, ord=2)
t.transform.rotation.x = quat[0]
t.transform.rotation.y = quat[1]
t.transform.rotation.z = quat[2]
t.transform.rotation.w = quat[3]
return t
def matrix_to_pose(matrix):
# type: (np.ndarray) -> Pose
quat_array = quaternion_from_matrix(matrix)
orientation = array_to_quaternion(quat_array)
position = array_to_point(matrix[0:3, 3])
return Pose(position=position, orientation=orientation)
def interpolate_ep(self, ep_a, ep_b, t_vals):
pos_a, rot_a = ep_a
pos_b, rot_b = ep_b
num_samps = len(t_vals)
pos_waypoints = np.array(pos_a) + np.array(np.tile(pos_b - pos_a, (1, num_samps))) * np.array(t_vals)
pos_waypoints = [np.mat(pos).T for pos in pos_waypoints.T]
rot_homo_a, rot_homo_b = np.eye(4), np.eye(4)
rot_homo_a[:3,:3] = rot_a
rot_homo_b[:3,:3] = rot_b
quat_a = tf_trans.quaternion_from_matrix(rot_homo_a)
quat_b = tf_trans.quaternion_from_matrix(rot_homo_b)
rot_waypoints = []
for t in t_vals:
cur_quat = tf_trans.quaternion_slerp(quat_a, quat_b, t)
rot_waypoints.append(np.mat(tf_trans.quaternion_matrix(cur_quat))[:3,:3])
return zip(pos_waypoints, rot_waypoints)
def calibrate3d(self):
A = np.matrix(self.A)
B = np.matrix(self.B)
ret_R, ret_t = rigid_transform_3D(A, B)
new_col = ret_t.reshape(3, 1)
tmp = np.append(ret_R, new_col, axis=1)
aug=np.array([[0.0,0.0,0.0,1.0]])
translation = np.squeeze(np.asarray(ret_t))
T = np.append(tmp,aug,axis=0)
quaternion = quaternion_from_matrix(T)
# Send the transform to ROS
self.tf_broadcaster.sendTransform(ret_t ,quaternion , rospy.Time.now(), self.kinect.link_frame,self.base_frame)
## Compute inverse of transformation
invR = ret_R.T
invT = -invR * ret_t
B_in_A = np.empty(B.shape)
for i in xrange(len(B)):
p = invR*B[i].T + invT
B_in_A[i] = p.T
## Compute the standard deviation
err = A-B_in_A
std = np.std(err,axis=0)
self.static_transform = '<node pkg="tf" type="static_transform_publisher" name="'+self.transform_name+'" args="'\
+' '.join(map(str, translation))+' '+' '.join(map(str, quaternion))+' '+self.base_frame+' '+self.kinect.link_frame+' 100" />'
self.depth_pt_pub.publish(self.get_prepared_pointcloud(A,self.kinect.link_frame))
self.world_pt_pub.publish(self.get_prepared_pointcloud(B,self.base_frame))
def TransformToPose( G ):
t = array( G )[0:3,3]
q = tr.quaternion_from_matrix( G )
orientation = geometry_msgs.msg.Quaternion( q[0], q[1], q[2], q[3] )
position = geometry_msgs.msg.Point( t[0], t[1], t[2] )
pose = geometry_msgs.msg.Pose( position, orientation )
return pose
def publish_state(self, t):
state_msg = TransformStamped()
t_ned_imu = tft.translation_matrix(self.model.get_position())
R_ned_imu = tft.quaternion_matrix(self.model.get_orientation())
T_ned_imu = tft.concatenate_matrices(t_ned_imu, R_ned_imu)
#rospy.loginfo("T_ned_imu = \n" + str(T_ned_imu))
if self.T_imu_vicon is None:
# grab the static transform from imu to vicon frame from param server:
frames = rospy.get_param("frames", None)
ypr = frames['vicon_to_imu']['rotation']
q_vicon_imu = tft.quaternion_from_euler(*[radians(x) for x in ypr], axes='rzyx') # xyzw
R_vicon_imu = tft.quaternion_matrix(q_vicon_imu)
t_vicon_imu = tft.translation_matrix(frames['vicon_to_imu']['translation'])
# rospy.loginfo(str(R_vicon_imu))
# rospy.loginfo(str(t_vicon_imu))
self.T_vicon_imu = tft.concatenate_matrices(t_vicon_imu, R_vicon_imu)
self.T_imu_vicon = tft.inverse_matrix(self.T_vicon_imu)
self.T_enu_ned = tft.euler_matrix(radians(90), 0, radians(180), 'rzyx')
rospy.loginfo(str(self.T_enu_ned))
rospy.loginfo(str(self.T_imu_vicon))
#rospy.loginfo(str(T_vicon_imu))
# we have /ned -> /imu, need to output /enu -> /vicon:
T_enu_vicon = tft.concatenate_matrices(self.T_enu_ned, T_ned_imu, self.T_imu_vicon )
state_msg.header.stamp = t
state_msg.header.frame_id = "/enu"
state_msg.child_frame_id = "/simflyer1/flyer_vicon"
stt = state_msg.transform.translation
stt.x, stt.y, stt.z = T_enu_vicon[:3,3]
stro = state_msg.transform.rotation
stro.x, stro.y, stro.z, stro.w = tft.quaternion_from_matrix(T_enu_vicon)
self.pub_state.publish(state_msg)
def fromMatrix(m):
"""
:param m: 4x4 array
:type m: :func:`numpy.array`
:return: New PoseMath object
Return a PoseMath object initialized from 4x4 matrix m
.. doctest::
>>> import numpy
>>> import tf_conversions.posemath as pm
>>> print PoseMath.fromMatrix(numpy.identity(4))
position:
x: 0.0
y: 0.0
z: 0.0
orientation:
x: 0.0
y: 0.0
z: 0.0
w: 1.0
"""
(x, y, z) = (m[0, 3], m[1, 3], m[2, 3])
q = transformations.quaternion_from_matrix(m)
return PoseMath(Pose(Point(x, y, z), Quaternion(*q)))
def _makePreGraspPose(self, boxMat, axis):
if axis==0: #x axis
alpha = 0
gamma = 0
else: #y axis
alpha = pi/2
gamma = -pi/2
ik_request = PositionIKRequest()
ik_request.ik_link_name = self.toolLinkName
with self._joint_state_lock:
ik_request.ik_seed_state.joint_state = copy.deepcopy(self._joint_states)
ik_request.pose_stamped = PoseStamped()
ik_request.pose_stamped.header.stamp = rospy.Time.now()
ik_request.pose_stamped.header.frame_id = self.frameID
beta = pi/2
while beta < 3*pi/2:
rotationMatrix = transformations.euler_matrix(alpha, beta, gamma, 'rzyz')
distance = self.preGraspDistance + self.gripperFingerLength
preGraspMat = transformations.translation_matrix([0,0,-distance])
fullMat = transformations.concatenate_matrices(boxMat, rotationMatrix, preGraspMat)
p = transformations.translation_from_matrix(fullMat)
q = transformations.quaternion_from_matrix(fullMat)
print "trying {} radians".format(beta)
try:
self._ik_server(ik_request, Constraints(), rospy.Duration(5.0))
except rospy.service.ServiceException:
beta += 0.1
else:
if ik_resp.error_code.val > 0:
return beta
else:
#print ik_resp.error_code.val
beta += 0.01
rospy.logerr('No way to pick this up. All IK solutions failed.')
return 7 * pi / 6
def set_orientation(self, orientation):
orientation = np.array(orientation)
if orientation.shape == (4, 4):
# We're getting a homogeneous rotation matrix - not a quaternion
orientation = transformations.quaternion_from_matrix(orientation)
return PoseEditor2(self.nav, [self.position, orientation])
def transformQuaternion(self, target_frame, ps):
"""
:param target_frame: the tf target frame, a string
:param ps: the geometry_msgs.msg.QuaternionStamped message
:return: new geometry_msgs.msg.QuaternionStamped message, in frame target_frame
:raises: any of the exceptions that :meth:`~tf.Transformer.lookupTransform` can raise
Transforms a geometry_msgs QuaternionStamped message to frame target_frame, returns a new QuaternionStamped message.
"""
# mat44 is frame-to-frame transform as a 4x4
mat44 = self.asMatrix(target_frame, ps.header)
# pose44 is the given quat as a 4x4
pose44 = xyzw_to_mat44(ps.quaternion)
# txpose is the new pose in target_frame as a 4x4
txpose = numpy.dot(mat44, pose44)
# quat is orientation of txpose
quat = tuple(transformations.quaternion_from_matrix(txpose))
# assemble return value QuaternionStamped
r = geometry_msgs.msg.QuaternionStamped()
r.header.stamp = ps.header.stamp
r.header.frame_id = target_frame
r.quaternion = geometry_msgs.msg.Quaternion(*quat)
return r
def transformPose(self, target_frame, ps):
"""
:param target_frame: the tf target frame, a string
:param ps: the geometry_msgs.msg.PoseStamped message
:return: new geometry_msgs.msg.PoseStamped message, in frame target_frame
:raises: any of the exceptions that :meth:`~tf.Transformer.lookupTransform` can raise
Transforms a geometry_msgs PoseStamped message to frame target_frame, returns a new PoseStamped message.
"""
# mat44 is frame-to-frame transform as a 4x4
mat44 = self.asMatrix(target_frame, ps.header)
# pose44 is the given pose as a 4x4
pose44 = numpy.dot(xyz_to_mat44(ps.pose.position), xyzw_to_mat44(ps.pose.orientation))
# txpose is the new pose in target_frame as a 4x4
txpose = numpy.dot(mat44, pose44)
# xyz and quat are txpose's position and orientation
xyz = tuple(transformations.translation_from_matrix(txpose))[:3]
quat = tuple(transformations.quaternion_from_matrix(txpose))
# assemble return value PoseStamped
r = geometry_msgs.msg.PoseStamped()
r.header.stamp = ps.header.stamp
r.header.frame_id = target_frame
r.pose = geometry_msgs.msg.Pose(geometry_msgs.msg.Point(*xyz), geometry_msgs.msg.Quaternion(*quat))
return r
def computeTransformation(world, slam, camera):
# Here we do not care about the slamMworld transformation
# timing as it is constant.
tWorld = listener.getLatestCommonTime(world, camera)
tMap = listener.getLatestCommonTime(slam, camera)
t = min(tWorld, tMap)
# Current pose given by the SLAM.
(slamMcam_T, slamMcam_Q) = listener.lookupTransform(camera, slam, t)
slamMcam = np.matrix(
transformer.fromTranslationRotation(slamMcam_T, slamMcam_Q))
# Estimation of the camera position given by control.
#FIXME: order is weird but it works.
(worldMcam_T, worldMcam_Q) = listener.lookupTransform(world, camera, t)
worldMcam = np.matrix(
transformer.fromTranslationRotation(worldMcam_T, worldMcam_Q))
(slamMworld_T, slamMworld_Q) = listener.lookupTransform(slam, world, t)
slamMworld = np.matrix(
transformer.fromTranslationRotation(slamMworld_T, slamMworld_Q))
slamMcamEstimated = slamMworld * worldMcam
# Inverse frames.
camMslam = np.linalg.inv(slamMcam)
camMslamEstimated = np.linalg.inv(slamMcamEstimated)
# Split.
camMslam_T = translation_from_matrix(camMslam)
camMslam_Q = quaternion_from_matrix(camMslam)
camMslam_E = euler_from_matrix(camMslam)
camMslamEstimated_T = translation_from_matrix(camMslamEstimated)
camMslamEstimated_Q = quaternion_from_matrix(camMslamEstimated)
camMslamEstimated_E = euler_from_matrix(camMslamEstimated)
# Compute correction.
camMslamCorrected_T = [
camMslam_T[0], camMslamEstimated_T[1], camMslam_T[2]
]
camMslamCorrected_E = [
camMslamEstimated_E[0], camMslam_E[1], camMslamEstimated_E[2]
]
camMslamCorrected_Q = quaternion_from_euler(*camMslamCorrected_E)
return (camMslamCorrected_T, camMslamCorrected_Q, t)
def find_controlled_tool_pose(self, target_pose):
# find current tool location
t_B_c = self.get_transform("torso_lift_link", self.tool_frame)
# find error in position
err_pos = target_pose[:3, 3] - t_B_c[:3, 3]
# find error in angle
err_ang = util.quaternion_dist(target_pose, t_B_c)
# find control values
u_x = self.x_pid.update_state(err_pos[0, 0])
u_y = self.y_pid.update_state(err_pos[1, 0])
u_z = self.z_pid.update_state(err_pos[2, 0]) + self.grav_comp
u_pos = np.mat([u_x, u_y, u_z]).T
u_a = self.a_pid.update_state(err_ang)
# quat_diff = (np.array(tf_trans.quaternion_from_matrix(target_pose)) -
# np.array(tf_trans.quaternion_from_matrix(t_B_c)))
# u_qx = self.qx_pid.update_state(quat_diff[0])
# u_qy = self.qy_pid.update_state(quat_diff[1])
# u_qz = self.qz_pid.update_state(quat_diff[2])
# u_qw = self.qw_pid.update_state(quat_diff[3])
# u_quat = np.array([u_qx, u_qy, u_qz, u_qw])
# u_quat = u_quat / np.linalg.norm(u_quat)
# find commanded frame of tool
# rotation
u_a = np.clip(u_a, 0, 1)
ei_q_slerp = tf_trans.quaternion_slerp(
tf_trans.quaternion_from_matrix(t_B_c),
tf_trans.quaternion_from_matrix(target_pose), u_a)
new_quat = ei_q_slerp
# new_quat = tf_trans.quaternion_multiply(tf_trans.quaternion_from_matrix(t_B_c),
# u_quat)
t_B_new = np.mat(tf_trans.quaternion_matrix(new_quat))
# position
u_pos_clipped = np.clip(u_pos, -self.u_pos_max, self.u_pos_max)
t_B_new[:3, 3] = t_B_c[:3, 3] + u_pos_clipped
# publish informative topics
self.goal_pub.publish(
util.pose_mat_to_stamped_msg("torso_lift_link", target_pose))
self.cur_pub.publish(
util.pose_mat_to_stamped_msg("torso_lift_link", t_B_c))
self.cmd_pub.publish(
util.pose_mat_to_stamped_msg("torso_lift_link", t_B_new))
return t_B_new, err_pos, err_ang
def computeTransformation(world, slam, camera):
# Here we do not care about the slamMworld transformation
# timing as it is constant.
tWorld = listener.getLatestCommonTime(world, camera)
tMap = listener.getLatestCommonTime(slam, camera)
t = min(tWorld, tMap)
# Current pose given by the SLAM.
(slamMcam_T, slamMcam_Q) = listener.lookupTransform(camera, slam, t)
slamMcam = np.matrix(transformer.fromTranslationRotation(slamMcam_T,
slamMcam_Q))
# Estimation of the camera position given by control.
#FIXME: order is weird but it works.
(worldMcam_T, worldMcam_Q) = listener.lookupTransform(world, camera, t)
worldMcam = np.matrix(transformer.fromTranslationRotation(worldMcam_T,
worldMcam_Q))
(slamMworld_T, slamMworld_Q) = listener.lookupTransform(slam, world, t)
slamMworld = np.matrix(transformer.fromTranslationRotation(slamMworld_T,
slamMworld_Q))
slamMcamEstimated = slamMworld * worldMcam
# Inverse frames.
camMslam = np.linalg.inv(slamMcam)
camMslamEstimated = np.linalg.inv(slamMcamEstimated)
# Split.
camMslam_T = translation_from_matrix(camMslam)
camMslam_Q = quaternion_from_matrix(camMslam)
camMslam_E = euler_from_matrix(camMslam)
camMslamEstimated_T = translation_from_matrix(camMslamEstimated)
camMslamEstimated_Q = quaternion_from_matrix(camMslamEstimated)
camMslamEstimated_E = euler_from_matrix(camMslamEstimated)
# Compute correction.
camMslamCorrected_T = [camMslam_T[0],
camMslamEstimated_T[1],
camMslam_T[2]]
camMslamCorrected_E = [camMslamEstimated_E[0],
camMslam_E[1],
camMslamEstimated_E[2]]
camMslamCorrected_Q = quaternion_from_euler(*camMslamCorrected_E)
return (camMslamCorrected_T, camMslamCorrected_Q, t)
def _unpack_recv_data(self, recv_data):
name = recv_data[0]
q = quaternion_from_matrix(self._matrix_from_list(recv_data[1:]))
return name, Pose(position=Vector3(x=float(recv_data[13])*self.mm2m,
y=float(recv_data[14])*self.mm2m,
z=float(recv_data[15])*self.mm2m),
orientation=Quaternion(x=q[0], y=q[1],
z=q[2], w=q[3]))
def test_quaternion_from_matrix(self, q):
# FIXME
matrix = quaternion_matrix(q)
q2 = quaternion_from_matrix(matrix)
# q1 = speed_up_and_execute(spw.quaternion_from_matrix, [matrix])
q1_2 = np.array([x.evalf(real=True) for x in spw.quaternion_from_matrix(matrix)]).astype(float)
# self.assertTrue(np.isclose(q1, q2).all() or np.isclose(q1, -q2).all(), msg='{} != {} | {}'.format(q, q1, q1_2))
self.assertTrue(np.isclose(q1_2, q2).all() or np.isclose(q1_2, -q2).all(), msg='{} != {}'.format(q, q1_2))
def quaternion(self):
"""The quaternion corresponding to this rotation"""
if self._quaternion is None:
self._quaternion = tft.quaternion_from_matrix(self.to_tf())
if self._quaternion[3] < 0:
self._quaternion = -self._quaternion
self._quaternion.flags.writeable = False
return self._quaternion
def mat2pose(m):
"""
Converts numpy matrix (4x4) into pose np.array([[x],[y],[qw],[qx],[qy],[qz]])
"""
q = np.expand_dims(quaternion_from_matrix(m), axis=1)
p = np.expand_dims(m[:3, 3], axis=1)
pose = np.vstack((p, q))
return pose
def transform_to_pose(matrix):
pose = Pose()
trans_vector = tft.translation_from_matrix(matrix)
pose.position = Point(trans_vector[0], trans_vector[1], trans_vector[2])
quartern = tft.quaternion_from_matrix(matrix)
pose.orientation = Quaternion(quartern[0], quartern[1], quartern[2],
quartern[3])
return pose
def G2qt( G ):
"""
Get 3x3 matrix and 3x1 from 4x4
"""
q = tr.quaternion_from_matrix( G )
t = G[0:3,3]
return q,t
def matrixToQuaternion(matrix):
"""Concert 3x3 rotation matrix into a quaternion"""
(R11, R12, R13, R21, R22, R23, R31, R32, R33) = matrix
# Build 4x4 matrix
M = [[R11, R21, R31, 0], [R12, R22, R32, 0], [R13, R23, R33, 0], [0, 0, 0, 1]]
A = numpy.array(M)
[w, x, y, z] = quaternion_from_matrix(A)
return [w, x, y, z]
def setterPatternRotation(data, value, collection_key):
assert len(value) == 3, "value must be a list with length 3."
matrix = utilities.rodriguesToMatrix(value)
hmatrix = np.identity(4).astype(np.float)
hmatrix[0:3, 0:3] = matrix
quat = transformations.quaternion_from_matrix(hmatrix)
data['patterns']['collections'][collection_key]['quat'] = quat
def fwd_kin(self):
self.robot = URDF.from_parameter_server()
self.kin = KDLKinematics(self.robot, BASE, EE_FRAME)
g = self.kin.forward(self.q)
p = np.array(g[0:3,-1]).ravel()
self.pose.pose.position = Point(*p.ravel())
self.pose.pose.orientation = Quaternion(*tr.quaternion_from_matrix(g))
return
def adjustRobotPose(self, pose):
Tr2_w = self.poseToTransform(pose)
Tadj = np.dot(np.dot(self.Tv_w, self.Tw_r1), Tr2_w)
pose.position.x, pose.position.y, pose.position.z = Tadj[0:3, 3]
q = tr.quaternion_from_matrix(Tadj)
for i, attr in enumerate(['x', 'y', 'z', 'w']):
pose.orientation.__setattr__(attr, q[i])
return pose
def kdl_to_quaternion(rotation_matrix):
return Quaternion(*quaternion_from_matrix([[
rotation_matrix[0, 0], rotation_matrix[0, 1], rotation_matrix[0, 2], 0
], [
rotation_matrix[1, 0], rotation_matrix[1, 1], rotation_matrix[1, 2], 0
], [
rotation_matrix[2, 0], rotation_matrix[2, 1], rotation_matrix[2, 2], 0
], [0, 0, 0, 1]]))
def adjustRobotPose(self, pose):
Tr2_w = self.poseToTransform(pose)
Tadj = np.dot(np.dot(self.Tv_w, self.Tw_r1),Tr2_w)
pose.position.x, pose.position.y, pose.position.z = Tadj[0:3,3]
q = tr.quaternion_from_matrix(Tadj)
for i,attr in enumerate(['x', 'y', 'z', 'w']):
pose.orientation.__setattr__(attr, q[i])
return pose
def flipOrbToNdt (orbPose):
qOrb = [orbPose.qx, orbPose.qy, orbPose.qz, orbPose.qw]
orbFlip = trafo.concatenate_matrices(
trafo.quaternion_matrix(qOrb),
trafo.rotation_matrix(np.pi/2, (1,0,0)),
trafo.rotation_matrix(np.pi/2, (0,0,1))
)
return trafo.quaternion_from_matrix(orbFlip)
def quaternion(self): """The quaternion corresponding to this rotation""" if self._quaternion is None: self._quaternion = tft.quaternion_from_matrix(self.to_tf()) if self._quaternion[3] < 0: self._quaternion = -self._quaternion self._quaternion.flags.writeable = False return self._quaternion
def flipOrbToNDT (orbPose):
qOrb = [orbPose.qx, orbPose.qy, orbPose.qz, orbPose.qw]
orbFlip = trafo.concatenate_matrices(
trafo.quaternion_matrix(qOrb),
trafo.rotation_matrix(np.pi/2, (1,0,0)),
trafo.rotation_matrix(np.pi/2, (0,0,1))
)
return trafo.quaternion_from_matrix(orbFlip)
def transformTo(tf_xyzquat, pose):
T_mat = tfm.concatenate_matrices(tfm.translation_matrix(tf_xyzquat[0:3]),
tfm.quaternion_matrix(tf_xyzquat[3:7]))
pose_mat = tfm.concatenate_matrices(tfm.translation_matrix(pose[0:3]),
tfm.quaternion_matrix(pose[3:7]))
new_pose_mat = np.dot(T_mat, pose_mat)
return tfm.translation_from_matrix(new_pose_mat).tolist(
) + tfm.quaternion_from_matrix(new_pose_mat).tolist()
def plan_place(self, desired_pose):
places = list()
ps = PoseStamped()
# ps.pose = self.current_grasping_target.object.primitive_poses[0]
ps.header.frame_id = self.current_grasping_target.object.header.frame_id
grasp_to_pick_matrix = self.computeGraspToPickMatrix()
place_to_base_matrix = self.computePlaceToBaseMatrix(desired_pose)
grasp2_to_place_matrix = place_to_base_matrix * grasp_to_pick_matrix
position_vector = grasp2_to_place_matrix[0:-1, 3]
quaternion = quaternion_from_matrix(grasp2_to_place_matrix)
position_array = position_vector.getA1()
l = PlaceLocation()
direction_components = self.pick_result.grasp.pre_grasp_approach.direction.vector
l.place_pose.header.frame_id = ps.header.frame_id
l.place_pose.pose.position.x = position_array[0]
l.place_pose.pose.position.y = position_array[1]
l.place_pose.pose.position.z = position_array[2]
l.place_pose.pose.orientation.x = quaternion[0]
l.place_pose.pose.orientation.y = quaternion[1]
l.place_pose.pose.orientation.z = quaternion[2]
l.place_pose.pose.orientation.w = quaternion[3]
# copy the posture, approach and retreat from the grasp used
approach = copy.deepcopy(self.pick_result.grasp.pre_grasp_approach)
approach.desired_distance /= 2.0
l.post_place_posture = self.pick_result.grasp.pre_grasp_posture
l.pre_place_approach = approach
l.post_place_retreat = self.pick_result.grasp.post_grasp_retreat
places.append(copy.deepcopy(l))
# create another several places, rotate each by 90 degrees in roll direction
l.place_pose.pose = rotate_pose_msg_by_euler_angles(l.place_pose.pose, 1.57, 0, 0)
places.append(copy.deepcopy(l))
l.place_pose.pose = rotate_pose_msg_by_euler_angles(l.place_pose.pose, 1.57, 0, 0)
places.append(copy.deepcopy(l))
l.place_pose.pose = rotate_pose_msg_by_euler_angles(l.place_pose.pose, 1.57, 0, 0)
places.append(copy.deepcopy(l))
print "DEBUG: Places: ", places[0]
success, place_result = self.pickplace.place_with_retry(
self.current_grasping_target.object.name,
places,
scene=self.scene,
allow_gripper_support_collision=False,
planner_id="PRMkConfigDefault",
)
if success:
rospy.loginfo("Planning succeeded.")
trajectory_approved = get_user_approval()
if trajectory_approved:
return place_result.trajectory_stages
else:
rospy.loginfo("Plan rejected. Replanning...")
else:
rospy.logwarn("Planning failed. Replanning...")
desired_pose[2] += 0.05
return self.plan_place(desired_pose)
def odometry_callback(self, msg):
world_T_t265 = None
odom_T_t265 = None
#self.listener.waitForTransform(world_frame, t265_world_frame, rospy.Time(0), rospy.Duration(1))
#self.listener.waitForTransform(odom_frame, t265_odom_frame, rospy.Time(0), rospy.Duration(1))
# See the tf.transformations library documentation: http://docs.ros.org/en/jade/api/tf/html/python/transformations.html
try:
#(trans, rot) = self.listener.lookupTransform(self.world_frame, self.t265_world_frame, rospy.Time(0))
T = self.tfBuffer.lookup_transform(self.world_frame,
self.t265_world_frame,
rospy.Time())
trans = (T.transform.translation.x, T.transform.translation.y,
T.transform.translation.z)
rot = (T.transform.rotation.x, T.transform.rotation.y,
T.transform.rotation.z, T.transform.rotation.w)
world_T_t265 = tf.concatenate_matrices(
tf.translation_matrix(trans), tf.quaternion_matrix(rot))
#t265_T_world = tf.inverse_matrix(world_T_t265)
except (LookupException, ConnectivityException,
ExtrapolationException):
rospy.logwarn("OdomGlue: Could not lookup world to T265 transform")
return
try:
#(trans, rot) = self.listener.lookupTransform(self.odom_frame, self.t265_odom_frame, rospy.Time(0))
T = self.tfBuffer.lookup_transform(self.odom_frame,
self.t265_odom_frame,
rospy.Time())
trans = (T.transform.translation.x, T.transform.translation.y,
T.transform.translation.z)
rot = (T.transform.rotation.x, T.transform.rotation.y,
T.transform.rotation.z, T.transform.rotation.w)
odom_T_t265 = tf.concatenate_matrices(tf.translation_matrix(trans),
tf.quaternion_matrix(rot))
t265_T_odom = tf.inverse_matrix(odom_T_t265)
except (LookupException, ConnectivityException,
ExtrapolationException):
rospy.logwarn("OdomGlue: Could not lookup odom to T265 transform")
return
world_T_odom = tf.concatenate_matrices(world_T_t265, t265_T_odom)
translation = (0.0, 0.0, 0.0)
rotation = (0.0, 0.0, 0.0, 1.0)
try:
self.broadcaster.sendTransform(
tf.translation_from_matrix(world_T_odom),
tf.quaternion_from_matrix(world_T_odom), rospy.Time.now(),
self.odom_frame, self.world_frame)
except:
pass
def cb_initial(self,msg):
trans = [msg.pose.pose.position.x, msg.pose.pose.position.y, msg.pose.pose.position.z]
rot = [msg.pose.pose.orientation.x, msg.pose.pose.orientation.y, msg.pose.pose.orientation.z, msg.pose.pose.orientation.w]
print ("Re-Initializing pose to: ", trans, rot)
# Calculate BASE_LINK to MAP transform
transform = t.concatenate_matrices(t.translation_matrix(trans), t.quaternion_matrix(rot))
inversed_transform = t.inverse_matrix(transform)
inv_trans = t.translation_from_matrix(inversed_transform)
inv_rot = t.quaternion_from_matrix(inversed_transform)
base_to_map_pose = PoseStamped()
base_to_map_pose.header.stamp = msg.header.stamp
base_to_map_pose.header.frame_id = "base_link"
base_to_map_pose.pose.position.x, base_to_map_pose.pose.position.y, base_to_map_pose.pose.position.z = inv_trans
base_to_map_pose.pose.orientation.x, base_to_map_pose.pose.orientation.y, base_to_map_pose.pose.orientation.z, base_to_map_pose.pose.orientation.w = inv_rot
self.listener.waitForTransform("odom", "base_link",msg.header.stamp, rospy.Duration(0.5))
map_odom = self.listener.transformPose('odom',base_to_map_pose)
# Calculate MAP to ODOM transform
trans_odom = [map_odom.pose.position.x, map_odom.pose.position.y, map_odom.pose.position.z]
rot_odom = [map_odom.pose.orientation.x, map_odom.pose.orientation.y, map_odom.pose.orientation.z, map_odom.pose.orientation.w]
transform_odom = t.concatenate_matrices(t.translation_matrix(trans_odom), t.quaternion_matrix(rot_odom))
inversed_transform_odom = t.inverse_matrix(transform_odom)
inv_trans_odom = t.translation_from_matrix(inversed_transform_odom)
inv_rot_odom = t.quaternion_from_matrix(inversed_transform_odom)
map_to_odom = TransformStamped()
map_to_odom.header.stamp = rospy.Time.now()
map_to_odom.header.frame_id = 'map'
map_to_odom.child_frame_id = 'odom'
map_to_odom.transform.translation.x, map_to_odom.transform.translation.y, map_to_odom.transform.translation.z = inv_trans_odom
map_to_odom.transform.rotation.x, map_to_odom.transform.rotation.y, map_to_odom.transform.rotation.z, map_to_odom.transform.rotation.w = inv_rot_odom
br = tf2_ros.StaticTransformBroadcaster()
br.sendTransform(map_to_odom)
def send_reset_to_rovio(self):
rospy.wait_for_service('rovio/reset_to_pose')
try:
rovio_reset_srv = rospy.ServiceProxy('rovio/reset_to_pose',
SrvResetToPose)
# compute pose from local ENU (East-North-Up frame) to
# IMU frame of the ViSensor (== C frame, according to MSF)
T_Enu_C = tf.concatenate_matrices(self._T_Enu_I, self._T_I_C)
q_Enu_C = tf.quaternion_from_matrix(T_Enu_C)
# set new Sensor IMU position and orientation respect to World frame
# (which is now aligned to local ENU)
self._pose_world_imu_msg.position.x = T_Enu_C[0, 3]
self._pose_world_imu_msg.position.y = T_Enu_C[1, 3]
self._pose_world_imu_msg.position.z = T_Enu_C[2, 3]
self._pose_world_imu_msg.orientation.w = q_Enu_C[3]
self._pose_world_imu_msg.orientation.x = q_Enu_C[0]
self._pose_world_imu_msg.orientation.y = q_Enu_C[1]
self._pose_world_imu_msg.orientation.z = q_Enu_C[2]
rovio_reset_srv(self._pose_world_imu_msg)
if self._verbose:
q_Enu_I = tf.quaternion_from_matrix(self._T_Enu_I)
(yaw, pitch, roll) = tf.euler_from_quaternion(q_Enu_I, 'rzyx')
rospy.loginfo(rospy.get_name() +
": body frame of MAV assumed with " +
str(math.degrees(roll)) + " (deg) roll, " +
str(math.degrees(pitch)) + " (deg) pitch, " +
str(math.degrees(yaw)) +
" (deg) yaw from local ENU (local axis, ZYX)")
self.create_rovio_init_info()
success = True
message = "Service call to reset Rovio internal state sent"
return (success, message)
except rospy.ServiceException, e:
success = False
message = "Service call to reset Rovio internal state failed: %s" % e
return (success, message)
def save_params(pos, rot, filename):
rot_homo = np.eye(4)
rot_homo[:3, :3] = rot
rot_quat = tf_trans.quaternion_from_matrix(rot_homo)
optitrak_params = {"position": pos.T.A[0].tolist(), "orientation": rot_quat.tolist()}
print optitrak_params
rosparam.upload_params("optitrak_calibration", optitrak_params)
rospy.sleep(0.5)
rosparam.dump_params(filename, "optitrak_calibration")
def get_ext(cameraid):
ret = get_numbers_from_file(os.environ['CODE_BASE'] + '/catkin_ws/src/passive_vision/camerainfo/' + cameraid + '.pose.txt')
print cameraid, ret
trans = [ret[3], ret[7], ret[11]]
ret[3] = 0
ret[7] = 0
ret[11] = 0
quat = tfm.quaternion_from_matrix(np.array(ret).reshape((4,4)))
return trans + quat.tolist()
def interpolate_ep(self, ep_a, ep_b, t_vals):
pos_a, rot_a = ep_a
pos_b, rot_b = ep_b
num_samps = len(t_vals)
pos_waypoints = np.array(pos_a) + np.array(
np.tile(pos_b - pos_a, (1, num_samps))) * np.array(t_vals)
pos_waypoints = [np.mat(pos).T for pos in pos_waypoints.T]
rot_homo_a, rot_homo_b = np.eye(4), np.eye(4)
rot_homo_a[:3, :3] = rot_a
rot_homo_b[:3, :3] = rot_b
quat_a = tf_trans.quaternion_from_matrix(rot_homo_a)
quat_b = tf_trans.quaternion_from_matrix(rot_homo_b)
rot_waypoints = []
for t in t_vals:
cur_quat = tf_trans.quaternion_slerp(quat_a, quat_b, t)
rot_waypoints.append(
np.mat(tf_trans.quaternion_matrix(cur_quat))[:3, :3])
return zip(pos_waypoints, rot_waypoints)
def correct_orientation(device, orientation):
if device.device_class in DEVICE_ORIENTATION_CORRECTION:
start_orient_mtx = t.quaternion_matrix(orientation)
correction = DEVICE_ORIENTATION_CORRECTION[device.device_class]
result_mtx = t.concatenate_matrices(start_orient_mtx, correction)
result_list = t.quaternion_from_matrix(result_mtx)
return tuple(result_list)
else:
return orientation
def mat44_to_pose(mat):
"""
Convert 4*4 transformation matrix to pose msg (no header information added)
:param mat:
:return:
"""
trans = Point(*tuple(transformations.translation_from_matrix(mat)))
rot = Quaternion(*tuple(transformations.quaternion_from_matrix(mat)))
return Pose(trans, rot)
def fwd_kin_trajectory(self, joint_trajectory):
endEffTrajectory = np.zeros((joint_trajectory.shape[0], 7))
for i in range(joint_trajectory.shape[0]):
T, tmp = self.fwd_kin(joint_trajectory[i, :])
pos = T[0:3, 3]
quat = tf_tran.quaternion_from_matrix(T)
endEffTrajectory[i, :] = np.hstack((pos, quat))
return endEffTrajectory
def apply (self, ot):
rotmat1 = self.toRotMat()
rotmat2 = ot.toRotMat()
q = trafo.quaternion_from_matrix(rotmat1.dot(rotmat2))
return Pose (self.timestamp,
rotmat1[0][0:3].dot([ot.x, ot.y, ot.z]) + self.x,
rotmat1[1][0:3].dot([ot.x, ot.y, ot.z]) + self.y,
rotmat1[2][0:3].dot([ot.x, ot.y, ot.z]) + self.z,
q[0], q[1], q[2], q[3])
def get_relative_pose(object_a, object_b):
object_a_mat = get_object_pose_as_matrix(object_a)
object_b_mat = get_object_pose_as_matrix(object_b)
rel_mat = numpy.linalg.inv(object_a_mat).dot(object_b_mat)
trans = transformations.translation_from_matrix(rel_mat)
rot = transformations.quaternion_from_matrix(rel_mat)
rot = [rot[3], rot[0], rot[1], rot[2]]
print "pose: [%f, %f, %f, %f, %f, %f, %f]" % (trans[0], trans[1], trans[2], rot[0], rot[1], rot[2], rot[3])
return (trans, rot)
def transform_to_pose(matrix):
pose = Pose()
print(matrix)
quat4 = tft.quaternion_from_matrix(matrix)
pose.orientation = Quaternion(quat4[0], quat4[1], quat4[2], quat4[3])
vector3 = np.dot(matrix, np.array([0, 0, 0, 1]))[:3]
pose.position = Point(vector3[0], vector3[1], vector3[2])
print pose
return pose
def transform_to_pose(matrix):
pose = Pose()
quat_from_mat = tft.quaternion_from_matrix(matrix)
pose.orientation = Quaternion(quat_from_mat[0], quat_from_mat[1], quat_from_mat[2], quat_from_mat[3])
x = matrix[0][3]
y = matrix[1][3]
z = matrix[2][3]
pose.position = Point(x, y, z)
return pose
def apply (self, ot):
rotmat1 = self.toRotMat()
rotmat2 = ot.toRotMat()
q = trafo.quaternion_from_matrix(rotmat1.dot(rotmat2))
return Pose (self.timestamp,
rotmat1[0][0:3].dot([ot.x, ot.y, ot.z]) + self.x,
rotmat1[1][0:3].dot([ot.x, ot.y, ot.z]) + self.y,
rotmat1[2][0:3].dot([ot.x, ot.y, ot.z]) + self.z,
q[0], q[1], q[2], q[3])
def align_assembly_frames(self, assemble_quat):
rot = quaternion_matrix(assemble_quat)
rot_x = np.array([[1.0, 0.0, 0.0, 0.0],
[0.0, -1.0, 0.0, 0.0],
[0.0, 0.0, -1.0, 0.0],
[0.0, 0.0, 0.0, 1.0]])
rot = np.dot(rot, rot_x)
new_assemble_quat = quaternion_from_matrix(rot)
return new_assemble_quat
def normal_to_quaternion(normal): # type: (np.ndarray) -> np.ndarray
rotation_matrix = np.zeros((4, 4))
rotation_matrix[3, 3] = 1.0
rotation_matrix[:3, :3] = np.vstack((
normal,
np.cross(np.array([0.0, 0.0, 1.0]), normal),
np.array([0.0, 0.0, 1.0]),
)).T
return transformations.quaternion_from_matrix(rotation_matrix)
def _inverse_tuples(t):
trans, rot = t
euler = tr.euler_from_quaternion(rot)
m = tr.compose_matrix(translate=trans, angles=euler)
m_inv = tr.inverse_matrix(m)
trans_inv = tr.translation_from_matrix(m_inv)
rot_inv = tr.rotation_matrix(*tr.rotation_from_matrix(m_inv))
quat_inv = tr.quaternion_from_matrix(rot_inv)
return (trans_inv, quat_inv)
def __init__(self, cell_id, markers_topic, world_frame, cell_frame,
main_cell_frame, tf_listener):
self.cell_id = cell_id
self.markers_topic = markers_topic
self.calibrated = False
self.positions = [
np.array([0, 0, 0], dtype='f8'),
np.array([0, 0, 0], dtype='f8'),
np.array([0, 0, 0], dtype='f8'),
np.array([0, 0, 0], dtype='f8')
]
self.avg = 100
self.cnt = [0, 0, 0, 0]
self.cell_frame = cell_frame
self.world_frame = world_frame
self.listener = tf_listener # type: tf.TransformListener()
self.main_cell_frame = main_cell_frame
self.transformation = Transform()
m = rospy.get_param("~" + self.cell_id + "/calibration_matrix", None)
self.markers_sub = rospy.Subscriber(self.markers_topic,
AlvarMarkers,
self.markers_cb,
queue_size=1)
self.marker_detection_enable_publisher = rospy.Publisher(
"/art/" + self.cell_id +
"/ar_track_alvar_bundle_calibration/enable_detection",
Bool,
queue_size=1,
latch=True)
self.x_offset = 0
self.y_offset = 0
self.z_offset = 0
self.x_rotate_offset = 0
self.y_rotate_offset = 0
self.z_rotate_offset = 0
if m is not None:
m = np.matrix(ast.literal_eval(m))
self.transformation.rotation = ArtCalibrationHelper.normalize_vector(
transformations.quaternion_from_matrix(m))
self.transformation.translation = transformations.translation_from_matrix(
m)
self.calibrated = True
rospy.loginfo("Cell: " + str(self.cell_id) +
" calibration loaded from param")
self.stop_marker_detection()
else:
self.start_marker_detection()
rospy.loginfo("Cell: " + str(self.cell_id) + " ready")
def get_asm_pose_in_gazebo(self, obj_name, asm_pose, z_offset):
obj_pose = self.client_getModel(obj_name, "table").pose
obj_tf_mat = self.get_tf_mat_from_pose(obj_pose)
asm_tf_mat = self.get_tf_mat_from_pose(asm_pose)
target_tf_mat = tf.concatenate_matrices(obj_tf_mat, asm_tf_mat)
target_tr = list(tf.translation_from_matrix(target_tf_mat))
target_tr[2] += z_offset
target_quat = tf.quaternion_from_matrix(target_tf_mat)
return self.get_pose_by_tr_quat(target_tr, target_quat)
def pose_callback(self,msg): #print 'got msg' frame_id = msg.header pose = msg.pose p = tft.translation_matrix([pose.position.x,pose.position.y,pose.position.z]) rot = tft.quaternion_matrix([pose.orientation.x,pose.orientation.y,pose.orientation.z,pose.orientation.w]) Tcam_to_cb = dot(p,rot) #print 'Tcam_to_cb',Tcam_to_cb #Tworld_to_cb = dot(tft.translation_matrix([self.x_offset,self.y_offset,self.z_offset]),tft.euler_matrix(0,0,pi/2)) Tworld_to_cb = dot(tft.translation_matrix([self.x_offset,self.y_offset,self.z_offset]),array(tb_to_mat(self.yaw, self.pitch, self.roll))) #print 'Tworld_to_cb',Tworld_to_cb if self.invert_tf: Tworld_to_cb = tft.inverse_matrix(Tworld_to_cb) Tworld_to_cam = dot(Tworld_to_cb,tft.inverse_matrix(Tcam_to_cb)) #print 'Tworld_to_cam',Tworld_to_cam Tworld_to_cam_p = Tworld_to_cam[0:3,3] Tworld_to_cam_q = tft.quaternion_from_matrix(Tworld_to_cam) pub_msg = PoseStamped() pub_msg.header.stamp = msg.header.stamp pub_msg.header.frame_id = '/world' pub_msg.pose.position = Point(*(Tworld_to_cam_p.tolist())) pub_msg.pose.orientation = Quaternion(*(Tworld_to_cam_q.tolist())) #print pub_msg self.pub.publish(pub_msg) pub_cb_msg = PoseStamped() pub_cb_msg.header.stamp = msg.header.stamp pub_cb_msg.header.frame_id = '/world' pub_cb_msg.pose.position = Point(*(Tworld_to_cb[0:3,3].tolist())) pub_cb_msg.pose.orientation = Quaternion(*(tft.quaternion_from_matrix(Tworld_to_cb).tolist())) self.pub_cb.publish(pub_cb_msg) self.transform = Tworld_to_cam self.transform_frame = msg.header.frame_id self.br.sendTransform(Tworld_to_cam_p,Tworld_to_cam_q,msg.header.stamp,self.transform_frame,'/world')
def find_controlled_tool_pose(self, target_pose):
# find current tool location
t_B_c = self.get_transform("torso_lift_link", self.tool_frame)
# find error in position
err_pos = target_pose[:3,3] - t_B_c[:3,3]
# find error in angle
err_ang = util.quaternion_dist(target_pose, t_B_c)
# find control values
u_x = self.x_pid.update_state(err_pos[0,0])
u_y = self.y_pid.update_state(err_pos[1,0])
u_z = self.z_pid.update_state(err_pos[2,0]) + self.grav_comp
u_pos = np.mat([u_x, u_y, u_z]).T
u_a = self.a_pid.update_state(err_ang)
# quat_diff = (np.array(tf_trans.quaternion_from_matrix(target_pose)) -
# np.array(tf_trans.quaternion_from_matrix(t_B_c)))
# u_qx = self.qx_pid.update_state(quat_diff[0])
# u_qy = self.qy_pid.update_state(quat_diff[1])
# u_qz = self.qz_pid.update_state(quat_diff[2])
# u_qw = self.qw_pid.update_state(quat_diff[3])
# u_quat = np.array([u_qx, u_qy, u_qz, u_qw])
# u_quat = u_quat / np.linalg.norm(u_quat)
# find commanded frame of tool
# rotation
u_a = np.clip(u_a, 0, 1)
ei_q_slerp = tf_trans.quaternion_slerp(tf_trans.quaternion_from_matrix(t_B_c),
tf_trans.quaternion_from_matrix(target_pose),
u_a)
new_quat = ei_q_slerp
# new_quat = tf_trans.quaternion_multiply(tf_trans.quaternion_from_matrix(t_B_c),
# u_quat)
t_B_new = np.mat(tf_trans.quaternion_matrix(new_quat))
# position
u_pos_clipped = np.clip(u_pos, -self.u_pos_max, self.u_pos_max)
t_B_new[:3,3] = t_B_c[:3,3] + u_pos_clipped
# publish informative topics
self.goal_pub.publish(util.pose_mat_to_stamped_msg("torso_lift_link", target_pose))
self.cur_pub.publish(util.pose_mat_to_stamped_msg("torso_lift_link", t_B_c))
self.cmd_pub.publish(util.pose_mat_to_stamped_msg("torso_lift_link", t_B_new))
return t_B_new, err_pos, err_ang
def timercb(self, data):
if not self.running_flag:
return
if self.listener.frameExists(SIMFRAME) and self.listener.frameExists(CONTFRAME):
t = self.listener.getLatestCommonTime(SIMFRAME, CONTFRAME)
try:
position, quaternion = self.listener.lookupTransform(SIMFRAME, CONTFRAME, t)
except (tf.Exception):
rospy.logerr("Could not transform from "\
"{0:s} to {1:s}".format(SIMFRAME,CONTFRAME))
return
else:
rospy.logerr("Could not find required frames "\
"for transformation from {0:s} to {1:s}".format(SIMFRAME,CONTFRAME))
return
# now we can use this position to integrate the trep simulation:
ucont = np.zeros(NU)
ucont[self.system.kin_configs.index(self.system.get_config('xs'))] = position[0]
ucont[self.system.kin_configs.index(self.system.get_config('ys'))] = position[1]
ucont[self.system.kin_configs.index(self.system.get_config('zs'))] = position[2]
# step integrator:
try:
self.mvi.step(self.mvi.t2 + DT, k2=ucont)
except trep.ConvergenceError as e:
rospy.loginfo("Could not take step: %s"%e.message)
return
# if we successfully integrated, let's publish the point and the tf
p = PointStamped()
p.header.stamp = rospy.Time.now()
p.header.frame_id = SIMFRAME
# get transform from trep world to mass frame:
gwm = self.system.get_frame(MASSFRAME).g()
ptrans = gwm[0:3, -1]
# print ptrans
p.point.x = ptrans[0]
p.point.y = ptrans[1]
p.point.z = ptrans[2]
self.mass_pub.publish(p)
# now we can send the transform:
qtrans = TR.quaternion_from_matrix(gwm)
self.br.sendTransform(ptrans, qtrans, p.header.stamp, MASSFRAME, SIMFRAME)
# now we can publish the markers:
for m in self.markers.markers:
m.header.stamp = p.header.stamp
self.mass_marker.pose = GM.Pose(position=GM.Point(*ptrans))
p1 = GM.Point(*ptrans)
p2 = GM.Point(*ucont)
self.link_marker.points = [p1, p2]
self.marker_pub.publish(self.markers)
# now we can render the forces:
self.render_forces()
return
