def r_cart_state_cb(self, msg):
try:
trans, quat = self.tf_lstnr.lookupTransform('/torso_lift_link',
'r_gripper_tool_frame', rospy.Time(0))
rot = tr.quaternion_to_matrix(quat)
tip = np.matrix([0.12, 0., 0.]).T
self.r_ee_pos = rot*tip + np.matrix(trans).T
self.r_ee_rot = rot
marker = Marker()
marker.header.frame_id = 'torso_lift_link'
time_stamp = rospy.Time.now()
marker.header.stamp = time_stamp
marker.ns = 'aloha land'
marker.type = Marker.SPHERE
marker.action = Marker.ADD
marker.pose.position.x = self.r_ee_pos[0,0]
marker.pose.position.y = self.r_ee_pos[1,0]
marker.pose.position.z = self.r_ee_pos[2,0]
size = 0.02
marker.scale.x = size
marker.scale.y = size
marker.scale.z = size
marker.lifetime = rospy.Duration()
marker.id = 71
marker.pose.orientation.x = 0
marker.pose.orientation.y = 0
marker.pose.orientation.z = 0
marker.pose.orientation.w = 1
color = (0.5, 0., 0.0)
marker.color.r = color[0]
marker.color.g = color[1]
marker.color.b = color[2]
marker.color.a = 1.
self.marker_pub.publish(marker)
ros_pt = msg.x_desi_filtered.pose.position
x, y, z = ros_pt.x, ros_pt.y, ros_pt.z
self.r_cep_pos = np.matrix([x, y, z]).T
pt = rot.T * (np.matrix([x,y,z]).T - np.matrix(trans).T)
pt = pt + tip
self.r_cep_pos_hooktip = rot*pt + np.matrix(trans).T
ros_quat = msg.x_desi_filtered.pose.orientation
quat = (ros_quat.x, ros_quat.y, ros_quat.z, ros_quat.w)
self.r_cep_rot = tr.quaternion_to_matrix(quat)
except:
return
def r_cart_state_cb(self, msg):
trans, quat = self.tf_lstnr.lookupTransform('/torso_lift_link',
'r_gripper_tool_frame', rospy.Time(0))
rot = tr.quaternion_to_matrix(quat)
tip = np.matrix([0.12, 0., 0.]).T
self.r_ee_pos = rot*tip + np.matrix(trans).T
self.r_ee_rot = rot
marker = Marker()
marker.header.frame_id = 'torso_lift_link'
time_stamp = rospy.Time.now()
marker.header.stamp = time_stamp
marker.ns = 'aloha land'
marker.type = Marker.SPHERE
marker.action = Marker.ADD
marker.pose.position.x = self.r_ee_pos[0,0]
marker.pose.position.y = self.r_ee_pos[1,0]
marker.pose.position.z = self.r_ee_pos[2,0]
size = 0.02
marker.scale.x = size
marker.scale.y = size
marker.scale.z = size
marker.lifetime = rospy.Duration()
marker.id = 71
marker.pose.orientation.x = 0
marker.pose.orientation.y = 0
marker.pose.orientation.z = 0
marker.pose.orientation.w = 1
color = (0.5, 0., 0.0)
marker.color.r = color[0]
marker.color.g = color[1]
marker.color.b = color[2]
marker.color.a = 1.
self.marker_pub.publish(marker)
ros_pt = msg.x_desi_filtered.pose.position
x, y, z = ros_pt.x, ros_pt.y, ros_pt.z
self.r_cep_pos = np.matrix([x, y, z]).T
pt = rot.T * (np.matrix([x,y,z]).T - np.matrix(trans).T)
pt = pt + tip
self.r_cep_pos_hooktip = rot*pt + np.matrix(trans).T
ros_quat = msg.x_desi_filtered.pose.orientation
quat = (ros_quat.x, ros_quat.y, ros_quat.z, ros_quat.w)
self.r_cep_rot = tr.quaternion_to_matrix(quat)
def FK(self, arm, q):
fk_req = GetPositionFKRequest()
fk_req.header.frame_id = 'torso_lift_link'
if arm == 0:
fk_req.fk_link_names.append('r_wrist_roll_link')
else:
fk_req.fk_link_names.append('l_wrist_roll_link')
fk_req.robot_state.joint_state.name = self.joint_names_list[arm]
fk_req.robot_state.joint_state.position = q
fk_resp = self.fk_srv[arm].call(fk_req)
if fk_resp.error_code.val == fk_resp.error_code.SUCCESS:
x = fk_resp.pose_stamped[0].pose.position.x
y = fk_resp.pose_stamped[0].pose.position.y
z = fk_resp.pose_stamped[0].pose.position.z
pos = [x, y, z]
q1 = fk_resp.pose_stamped[0].pose.orientation.x
q2 = fk_resp.pose_stamped[0].pose.orientation.y
q3 = fk_resp.pose_stamped[0].pose.orientation.z
q4 = fk_resp.pose_stamped[0].pose.orientation.w
quat = [q1,q2,q3,q4]
rot = tr.quaternion_to_matrix(quat)
else:
rospy.logerr('Forward kinematics failed')
return None, None
return pos, rot
def r_cart_state_cb(self, msg):
trans, quat = self.tf_lstnr.lookupTransform('/torso_lift_link',
'r_gripper_tool_frame', rospy.Time(0))
rot = tr.quaternion_to_matrix(quat)
tip = np.matrix([0.12, 0., 0.]).T
self.r_ee_pos = rot*tip + np.matrix(trans).T
self.r_ee_rot = rot
ros_pt = msg.x_desi_filtered.pose.position
x, y, z = ros_pt.x, ros_pt.y, ros_pt.z
self.r_cep_pos = np.matrix([x, y, z]).T
pt = rot.T * (np.matrix([x,y,z]).T - np.matrix(trans).T)
pt = pt + tip
self.r_cep_pos_hooktip = rot*pt + np.matrix(trans).T
ros_quat = msg.x_desi_filtered.pose.orientation
quat = (ros_quat.x, ros_quat.y, ros_quat.z, ros_quat.w)
self.r_cep_rot = tr.quaternion_to_matrix(quat)
def publish_skin_contact(self, ft):
# -ve sign because we want the force that the robot is applying
# on the world.
ft = -np.matrix(ft).T
force = ft[0:3,:]
f_mag = np.linalg.norm(force)
torque = ft[3:,:]
msg = SkinContact()
if self.use_right_arm:
frm_id = '/handmount_RIGHT'
else:
frm_id = '/handmount_LEFT'
msg.header.frame_id = '/torso_lift_link'
msg.header.stamp = rospy.Time.now()
if f_mag > 2.0:
t, q = self.tf_lstnr.lookupTransform('/torso_lift_link',
frm_id, rospy.Time(0))
t = np.matrix(t).reshape(3,1)
rot = tr.quaternion_to_matrix(q)
# approx location of the FT sensor in the handmount_LEFT
# or handmount_RIGHT frame.
pt = np.matrix([0., 0., -0.06]).T
# using a fixed location for the contact, for now.
if False:
# here I can try and compute the line of action of
# the force and then attempt to find the contact
# location by taking the intersection of the line of
# action with the surface of the cylinder.
# Too complicated for now (Sept 29, 2011)
p_oc = np.cross(force.A1, torque.A1) / (force.T * force)[0,0]
pt = np.matrix(p_oc).T + pt
# now transform vectors into the torso_lift_link
force = rot * force
n = force / f_mag
pt = rot * pt + t
msg.locations.append(Point(pt[0,0], pt[1,0], pt[2,0]))
msg.forces.append(Vector3(force[0,0], force[1,0], force[2,0]))
msg.normals.append(Vector3(n[0,0], n[1,0], n[2,0]))
msg.pts_x.append(FloatArrayBare([pt[0,0]]))
msg.pts_y.append(FloatArrayBare([pt[1,0]]))
msg.pts_z.append(FloatArrayBare([pt[2,0]]))
if self.use_right_arm:
msg.link_names.append('end_effector_RIGHT')
else:
msg.link_names.append('end_effector_LEFT')
self.sc_pub.publish(msg)
def publish_skin_contact(self, ft):
# -ve sign because we want the force that the robot is applying
# on the world.
ft = -np.matrix(ft).T
force = ft[0:3, :]
f_mag = np.linalg.norm(force)
torque = ft[3:, :]
msg = SkinContact()
if self.use_right_arm:
frm_id = '/handmount_RIGHT'
else:
frm_id = '/handmount_LEFT'
msg.header.frame_id = '/torso_lift_link'
msg.header.stamp = rospy.Time.now()
if f_mag > 2.0:
t, q = self.tf_lstnr.lookupTransform('/torso_lift_link', frm_id,
rospy.Time(0))
t = np.matrix(t).reshape(3, 1)
rot = tr.quaternion_to_matrix(q)
# approx location of the FT sensor in the handmount_LEFT
# or handmount_RIGHT frame.
pt = np.matrix([0., 0., -0.06]).T
# using a fixed location for the contact, for now.
if False:
# here I can try and compute the line of action of
# the force and then attempt to find the contact
# location by taking the intersection of the line of
# action with the surface of the cylinder.
# Too complicated for now (Sept 29, 2011)
p_oc = np.cross(force.A1, torque.A1) / (force.T * force)[0, 0]
pt = np.matrix(p_oc).T + pt
# now transform vectors into the torso_lift_link
force = rot * force
n = force / f_mag
pt = rot * pt + t
msg.locations.append(Point(pt[0, 0], pt[1, 0], pt[2, 0]))
msg.forces.append(Vector3(force[0, 0], force[1, 0], force[2, 0]))
msg.normals.append(Vector3(n[0, 0], n[1, 0], n[2, 0]))
msg.pts_x.append(FloatArrayBare([pt[0, 0]]))
msg.pts_y.append(FloatArrayBare([pt[1, 0]]))
msg.pts_z.append(FloatArrayBare([pt[2, 0]]))
if self.use_right_arm:
msg.link_names.append('end_effector_RIGHT')
else:
msg.link_names.append('end_effector_LEFT')
self.sc_pub.publish(msg)
def taxel_array_cb(ta, callback_args):
sc_pu, tf_lstnr = callback_args
sc = SkinContact()
sc.header.frame_id = '/torso_lift_link' # has to be this and no other coord frame.
sc.header.stamp = ta.header.stamp
pts = np.column_stack((ta.centers_x, ta.centers_y, ta.centers_z))
nrmls = np.column_stack((ta.normals_x, ta.normals_y, ta.normals_z))
fs = np.column_stack((ta.values_x, ta.values_y, ta.values_z))
t1, q1 = tf_lstnr.lookupTransform(sc.header.frame_id,
ta.header.frame_id,
rospy.Time(0))
#ta.header.stamp)
t1 = np.matrix(t1).reshape(3,1)
r1 = tr.quaternion_to_matrix(q1)
if ta.link_names == []:
real_skin_patch = True
else:
real_skin_patch = False
# transform to the torso_lift_link frame.
pts = r1 * np.matrix(pts).T + t1
nrmls = r1 * np.matrix(nrmls).T
fs = r1 * np.matrix(fs).T
fmags = ut.norm(fs).A1
# for fabric sensor and meka sensor, Advait moved the thresholding
# etc within the calibration node. (June 13, 2012)
if not real_skin_patch:
idxs = np.where(fmags > 0.5)[0]
else:
idxs = np.where(fmags > 0.001)[0]
for i in idxs:
p = pts[:,i]
n1 = nrmls[:,i]
n2 = fs[:,i]
if real_skin_patch:
link_name = ta.header.frame_id
else:
link_name = ta.link_names[i]
sc.locations.append(Point(p[0,0], p[1,0], p[2,0]))
sc.forces.append(Vector3(n2[0,0], n2[1,0], n2[2,0]))
sc.normals.append(Vector3(n1[0,0], n1[1,0], n1[2,0]))
sc.link_names.append(link_name)
sc.pts_x.append(FloatArrayBare(p[0,:].A1))
sc.pts_y.append(FloatArrayBare(p[1,:].A1))
sc.pts_z.append(FloatArrayBare(p[2,:].A1))
sc_pub.publish(sc)
def taxel_array_cb(ta, callback_args):
sc_pu, tf_lstnr = callback_args
sc = SkinContact()
sc.header.frame_id = '/torso_lift_link' # has to be this and no other coord frame.
sc.header.stamp = ta.header.stamp
pts = np.column_stack((ta.centers_x, ta.centers_y, ta.centers_z))
nrmls = np.column_stack((ta.normals_x, ta.normals_y, ta.normals_z))
fs = np.column_stack((ta.values_x, ta.values_y, ta.values_z))
t1, q1 = tf_lstnr.lookupTransform(sc.header.frame_id, ta.header.frame_id,
rospy.Time(0))
#ta.header.stamp)
t1 = np.matrix(t1).reshape(3, 1)
r1 = tr.quaternion_to_matrix(q1)
if ta.link_names == []:
real_skin_patch = True
else:
real_skin_patch = False
# transform to the torso_lift_link frame.
pts = r1 * np.matrix(pts).T + t1
nrmls = r1 * np.matrix(nrmls).T
fs = r1 * np.matrix(fs).T
fmags = ut.norm(fs).A1
# for fabric sensor and meka sensor, Advait moved the thresholding
# etc within the calibration node. (June 13, 2012)
if not real_skin_patch:
idxs = np.where(fmags > 0.5)[0]
else:
idxs = np.where(fmags > 0.001)[0]
for i in idxs:
p = pts[:, i]
n1 = nrmls[:, i]
n2 = fs[:, i]
if real_skin_patch:
link_name = ta.header.frame_id
else:
link_name = ta.link_names[i]
sc.locations.append(Point(p[0, 0], p[1, 0], p[2, 0]))
sc.forces.append(Vector3(n2[0, 0], n2[1, 0], n2[2, 0]))
sc.normals.append(Vector3(n1[0, 0], n1[1, 0], n1[2, 0]))
sc.link_names.append(link_name)
sc.pts_x.append(FloatArrayBare(p[0, :].A1))
sc.pts_y.append(FloatArrayBare(p[1, :].A1))
sc.pts_z.append(FloatArrayBare(p[2, :].A1))
sc_pub.publish(sc)
def get_forces(self, bias = True):
# later I might be looping over all the different objects,
# returning a dictionary of <object_id: force_vector>
f = self.obj1_ftc.read(without_bias = not bias)
f = f[0:3, :]
trans, quat = self.tf_lstnr.lookupTransform('/torso_lift_link',
'/ft2',
rospy.Time(0))
rot = tr.quaternion_to_matrix(quat)
f = rot * f
return -f # the negative is intentional (Advait, Nov 24. 2010.)
def precompute_taxel_location_and_normal(self):
resp = self.local_coord_frames_srv()
pos_list = []
nrml_list = []
for t in resp.data:
pos = (t.translation.x, t.translation.y, t.translation.z)
pos_list.append(pos)
q = (t.rotation.x, t.rotation.y, t.rotation.z, t.rotation.w)
rot_mat = tr.quaternion_to_matrix(q)
nrml_list.append(rot_mat[:, 2].A1)
self.pos_arr = np.array(pos_list)
self.nrml_arr = np.array(nrml_list)
def get_wrist_force(self, arm, bias=True, base_frame=False):
if arm != 0:
rospy.logerr('Unsupported arm: %d' % arm)
raise RuntimeError('Unimplemented function')
f = self.r_arm_ftc.read(without_bias=not bias)
f = f[0:3, :]
if base_frame:
trans, quat = self.tf_lstnr.lookupTransform(
'/torso_lift_link', '/ft2', rospy.Time(0))
rot = tr.quaternion_to_matrix(quat)
f = rot * f
return -f # the negative is intentional (Advait, Nov 24. 2010.)
def precompute_taxel_location_and_normal(self):
resp = self.local_coord_frames_srv()
pos_list = []
nrml_list = []
for t in resp.data:
pos = (t.translation.x, t.translation.y, t.translation.z)
pos_list.append(pos)
q = (t.rotation.x, t.rotation.y, t.rotation.z, t.rotation.w)
rot_mat = tr.quaternion_to_matrix(q)
nrml_list.append(rot_mat[:,2].A1)
self.pos_arr = np.array(pos_list)
self.nrml_arr = np.array(nrml_list)
def get_wrist_force(self, arm, bias = True, base_frame = False):
if arm != 0:
rospy.logerr('Unsupported arm: %d'%arm)
raise RuntimeError('Unimplemented function')
f = self.r_arm_ftc.read(without_bias = not bias)
f = f[0:3, :]
if base_frame:
trans, quat = self.tf_lstnr.lookupTransform('/torso_lift_link',
'/ft2', rospy.Time(0))
rot = tr.quaternion_to_matrix(quat)
f = rot * f
return -f # the negative is intentional (Advait, Nov 24. 2010.)
def pose_cb(data, d):
if len(data.objects) != 2:
return
for obj in data.objects:
p = obj.pose.position
pos = np.matrix([p.x, p.y, p.z]).T
q = obj.pose.orientation
rot = tr.quaternion_to_matrix([q.x, q.y, q.z, q.w])
t = data.header.stamp.to_sec()
d[obj.type]['pos'] = pos
d[obj.type]['rot'] = rot
def pose_cb(data, d):
if len(data.objects) != 2:
return
for obj in data.objects:
p = obj.pose.position
pos = np.matrix([p.x, p.y, p.z]).T
q = obj.pose.orientation
rot = tr.quaternion_to_matrix([q.x, q.y, q.z, q.w])
t = data.header.stamp.to_sec()
d[obj.type]['pos'] = pos
d[obj.type]['rot'] = rot
def make_darci_ee_marker(ps,
color,
orientation=None,
marker_array=None,
control=None,
mesh_id_start=0,
ns="darci_ee",
offset=-0.14):
mesh_id = mesh_id_start
# this is the palm piece
mesh = Marker()
mesh.ns = ns
#mesh.mesh_use_embedded_materials = True
mesh.scale = Vector3(1., 1., 1.)
mesh.color = ColorRGBA(*color)
# stub_end_effector.dae
# stub_end_effector_mini45.dae
# tube_with_ati_collisions.dae
# wedge_blender.dae
# mesh.mesh_resource = "package://hrl_dynamic_mpc/meshes/tube_with_ati_collisions.dae"
mesh.mesh_resource = "package://hrl_dynamic_mpc/meshes/Darci_Flipper.stl"
mesh.type = Marker.MESH_RESOURCE
#rot_default = tr.Ry(np.radians(-90))*tr.Rz(np.radians(90))
rot_default = tr.Ry(np.radians(0)) * tr.Rz(np.radians(90))
if orientation == None:
orientation = [0, 0, 0, 1]
rot_buf = tr.quaternion_to_matrix(orientation)
orientation = tr.matrix_to_quaternion(rot_buf * rot_default)
mesh.pose.orientation = Quaternion(*orientation)
mesh.pose.position.x = offset
if control != None:
control.markers.append(mesh)
elif marker_array != None:
mesh.pose.position = ps.pose.position
mesh.header.frame_id = ps.header.frame_id
mesh.id = mesh_id
marker_array.markers.append(mesh)
if control != None:
control.interaction_mode = InteractiveMarkerControl.BUTTON
return control
elif marker_array != None:
return marker_array
def transformTaxelArray(self, ta_msg, new_frame):
# Get the transformation from the desired frame to current frame
if ta_msg.header.frame_id == "":
return ta_msg
self.tf_lstnr.waitForTransform(new_frame, ta_msg.header.frame_id,
rospy.Time(0), rospy.Duration(4.0))
t1, q1 = self.tf_lstnr.lookupTransform(new_frame,
ta_msg.header.frame_id,
rospy.Time(0))
t1 = np.matrix(t1).reshape(3, 1)
r1 = tr.quaternion_to_matrix(q1)
# Create new message data structure
new_ta_msg = copy.copy(ta_msg)
new_ta_msg.header.frame_id = new_frame
# Perform the transformation
pts = np.column_stack(
(ta_msg.centers_x, ta_msg.centers_y, ta_msg.centers_z))
nrmls = np.column_stack(
(ta_msg.normals_x, ta_msg.normals_y, ta_msg.normals_z))
values = np.column_stack(
(ta_msg.values_x, ta_msg.values_y, ta_msg.values_z))
pts = r1 * np.matrix(pts).T + t1
nrmls = r1 * np.matrix(nrmls).T
values = r1 * np.matrix(values).T
# Reformat the transformed data to be repackaged as a TaxelArray message
pts_array = np.asarray(pts)
nrmls_array = np.asarray(nrmls)
values_array = np.asarray(values)
new_ta_msg.centers_x = pts_array[0, :].tolist()
new_ta_msg.centers_y = pts_array[1, :].tolist()
new_ta_msg.centers_z = pts_array[2, :].tolist()
new_ta_msg.normals_x = nrmls_array[0, :].tolist()
new_ta_msg.normals_y = nrmls_array[1, :].tolist()
new_ta_msg.normals_z = nrmls_array[2, :].tolist()
new_ta_msg.values_x = values_array[0, :].tolist()
new_ta_msg.values_y = values_array[1, :].tolist()
new_ta_msg.values_z = values_array[2, :].tolist()
return new_ta_msg
def detect_artag(self):
try:
rospy.logout("Finding the AR tag..")
self.pub_rate.sleep()
(self.ar_pos, rot) = self.tf_listener.lookupTransform("/torso_lift_link",
self.marker_frame, rospy.Time(0))
self.pub_rate.sleep()
gripper_rot = hrl_tr.rotY(math.pi/2) #gripper facing -z direction
self.ar_rot = hrl_tr.quaternion_to_matrix(rot)*gripper_rot
rospy.logout("Found AR tag!\nPosition: "+pplist(self.ar_pos)+"\nQuaterion: "+pplist(rot))
self.ar_ep = []
self.ar_ep.append(np.matrix(self.ar_pos).T)
self.ar_ep.append(self.ar_rot)
self.found_tag = True
except:
rospy.logout('AARtagDetect: Transform failed for '+self.tool)
return False
def detect_artag(self):
try:
rospy.logout("Finding the AR tag..")
self.pub_rate.sleep()
(self.ar_pos, rot) = self.tf_listener.lookupTransform("/torso_lift_link",
self.marker_frame, rospy.Time(0))
self.pub_rate.sleep()
gripper_rot = hrl_tr.rotY(math.pi/2) #gripper facing -z direction
self.ar_rot = hrl_tr.quaternion_to_matrix(rot)*gripper_rot
rospy.logout("Found AR tag!\nPosition: "+pplist(self.ar_pos)+"\nQuaterion: "+pplist(rot))
self.ar_ep = []
self.ar_ep.append(np.matrix(self.ar_pos).T)
self.ar_ep.append(self.ar_rot)
self.found_tag = True
except:
rospy.logout('AARtagDetect: Transform failed for '+self.tool)
return False
def make_darci_ee_marker(ps, color, orientation = None,
marker_array=None, control=None,
mesh_id_start = 0, ns = "darci_ee",
offset=-0.14):
mesh_id = mesh_id_start
# this is the palm piece
mesh = Marker()
mesh.ns = ns
#mesh.mesh_use_embedded_materials = True
mesh.scale = Vector3(1.,1.,1.)
mesh.color = ColorRGBA(*color)
# stub_end_effector.dae
# stub_end_effector_mini45.dae
# tube_with_ati_collisions.dae
# wedge_blender.dae
# mesh.mesh_resource = "package://hrl_dynamic_mpc/meshes/tube_with_ati_collisions.dae"
mesh.mesh_resource = "package://hrl_dynamic_mpc/meshes/Darci_Flipper.stl"
mesh.type = Marker.MESH_RESOURCE
#rot_default = tr.Ry(np.radians(-90))*tr.Rz(np.radians(90))
rot_default = tr.Ry(np.radians(0))*tr.Rz(np.radians(90))
if orientation == None:
orientation = [0, 0, 0, 1]
rot_buf = tr.quaternion_to_matrix(orientation)
orientation = tr.matrix_to_quaternion(rot_buf*rot_default)
mesh.pose.orientation = Quaternion(*orientation)
mesh.pose.position.x = offset
if control != None:
control.markers.append( mesh )
elif marker_array != None:
mesh.pose.position = ps.pose.position
mesh.header.frame_id = ps.header.frame_id
mesh.id = mesh_id
marker_array.markers.append(mesh)
if control != None:
control.interaction_mode = InteractiveMarkerControl.BUTTON
return control
elif marker_array != None:
return marker_array
def pose_cb(data, d):
global t_pose_cb
t_pose_cb = time.time()
for obj in data.objects:
p = obj.pose.position
pos = np.matrix([p.x, p.y, p.z]).T
q = obj.pose.orientation
rot = tr.quaternion_to_matrix([q.x, q.y, q.z, q.w])
t = data.header.stamp.to_sec()
d[obj.type]['pos_list'].append(pos)
d[obj.type]['rot_list'].append(rot)
d[obj.type]['time_list'].append(t)
print '>>>>>>>>>>>>>>>>>>>>>>>>>>>>'
print 'pos:', pos.T
print 'rot:', rot
def transformTaxelArray(self, ta_msg, new_frame):
# Get the transformation from the desired frame to current frame
if ta_msg.header.frame_id == "":
return ta_msg
self.tf_lstnr.waitForTransform(new_frame, ta_msg.header.frame_id, rospy.Time(0), rospy.Duration(4.0))
t1, q1 = self.tf_lstnr.lookupTransform(new_frame, ta_msg.header.frame_id, rospy.Time(0))
t1 = np.matrix(t1).reshape(3,1)
r1 = tr.quaternion_to_matrix(q1)
# Create new message data structure
new_ta_msg = copy.copy(ta_msg)
new_ta_msg.header.frame_id = new_frame
# Perform the transformation
pts = np.column_stack((ta_msg.centers_x, ta_msg.centers_y, ta_msg.centers_z))
nrmls = np.column_stack((ta_msg.normals_x, ta_msg.normals_y, ta_msg.normals_z))
values = np.column_stack((ta_msg.values_x, ta_msg.values_y, ta_msg.values_z))
pts = r1 * np.matrix(pts).T + t1
nrmls = r1 * np.matrix(nrmls).T
values = r1 * np.matrix(values).T
# Reformat the transformed data to be repackaged as a TaxelArray message
pts_array = np.asarray(pts)
nrmls_array = np.asarray(nrmls)
values_array = np.asarray(values)
new_ta_msg.centers_x = pts_array[0, :].tolist()
new_ta_msg.centers_y = pts_array[1, :].tolist()
new_ta_msg.centers_z = pts_array[2, :].tolist()
new_ta_msg.normals_x = nrmls_array[0, :].tolist()
new_ta_msg.normals_y = nrmls_array[1, :].tolist()
new_ta_msg.normals_z = nrmls_array[2, :].tolist()
new_ta_msg.values_x = values_array[0, :].tolist()
new_ta_msg.values_y = values_array[1, :].tolist()
new_ta_msg.values_z = values_array[2, :].tolist()
return new_ta_msg
def pose_cb(data, d):
global t_pose_cb
t_pose_cb = time.time()
for obj in data.objects:
p = obj.pose.position
pos = np.matrix([p.x, p.y, p.z]).T
q = obj.pose.orientation
rot = tr.quaternion_to_matrix([q.x, q.y, q.z, q.w])
t = data.header.stamp.to_sec()
d[obj.type]['pos_list'].append(pos)
d[obj.type]['rot_list'].append(rot)
d[obj.type]['time_list'].append(t)
print '>>>>>>>>>>>>>>>>>>>>>>>>>>>>'
print 'pos:', pos.T
print 'rot:', rot
def l_cart_state_cb(self, msg):
ros_pt = msg.x_desi_filtered.pose.position
self.l_cep_pos = np.matrix([ros_pt.x, ros_pt.y, ros_pt.z]).T
ros_quat = msg.x_desi_filtered.pose.orientation
quat = (ros_quat.x, ros_quat.y, ros_quat.z, ros_quat.w)
self.l_cep_rot = tr.quaternion_to_matrix(quat)
def l_cart_state_cb(self, msg):
ros_pt = msg.x_desi_filtered.pose.position
self.l_cep_pos = np.matrix([ros_pt.x, ros_pt.y, ros_pt.z]).T
ros_quat = msg.x_desi_filtered.pose.orientation
quat = (ros_quat.x, ros_quat.y, ros_quat.z, ros_quat.w)
self.l_cep_rot = tr.quaternion_to_matrix(quat)
def make_pr2_gripper_marker(ps, color, orientation = None,
marker_array=None, control=None,
mesh_id_start = 0, ns = "pr2_gripper",
offset=-0.19):
mesh_id = mesh_id_start
# this is the palm piece
mesh = Marker()
mesh.ns = ns
#mesh.mesh_use_embedded_materials = True
mesh.scale = Vector3(1.,1.,1.)
mesh.color = ColorRGBA(*color)
mesh.mesh_resource = "package://pr2_description/meshes/gripper_v0/gripper_palm.dae"
mesh.type = Marker.MESH_RESOURCE
if orientation != None:
mesh.pose.orientation = Quaternion(*orientation)
else:
mesh.pose.orientation = Quaternion(0,0,0,1)
mesh.pose.position.x = offset
if control != None:
control.markers.append( mesh )
elif marker_array != None:
mesh.pose.position = ps.pose.position
mesh.header.frame_id = ps.header.frame_id
mesh.id = mesh_id
mesh_id = mesh_id+1
marker_array.markers.append(mesh)
# amount to open the gripper for each finger
angle_open = 0.4
if orientation == None:
rot0 = np.matrix(np.eye(3))
else:
rot0 = tr.quaternion_to_matrix(orientation)
if marker_array != None:
T0 = tr.composeHomogeneousTransform(rot0, [ps.point.x, ps.point.y, ps.point.z])
else:
T0 = tr.composeHomogeneousTransform(rot0, [offset, 0.0, 0.])
#transforming the left finger and finger tip
rot1 = tr.rot_angle_direction(angle_open, np.matrix([0, 0, 1]))
T1 = tr.composeHomogeneousTransform(rot1, [0.07691, 0.01, 0.])
rot2 = tr.rot_angle_direction(-1*angle_open, np.matrix([0, 0, 1]))
T2 = tr.composeHomogeneousTransform(rot2, [0.09137, 0.00495, 0.])
T_proximal = T0*T1
T_distal = T0*T1*T2
finger_pos = tr.tft.translation_from_matrix(T_proximal)
finger_rot = tr.tft.quaternion_from_matrix(T_proximal)
tip_pos = tr.tft.translation_from_matrix(T_distal)
tip_rot = tr.tft.quaternion_from_matrix(T_distal)
#making the marker for the left finger
mesh = Marker()
mesh.ns = ns
#mesh.mesh_use_embedded_materials = True
mesh.scale = Vector3(1.,1.,1.)
mesh.mesh_resource = "package://pr2_description/meshes/gripper_v0/l_finger.dae"
mesh.color = ColorRGBA(*color)
mesh.type = Marker.MESH_RESOURCE
mesh.pose.orientation = Quaternion(*finger_rot)
mesh.pose.position = Point(*finger_pos)
if control != None:
control.markers.append( mesh )
elif marker_array != None:
mesh.header.frame_id = ps.header.frame_id
mesh.id = mesh_id
mesh_id = mesh_id+1
marker_array.markers.append(mesh)
mesh = Marker()
mesh.ns = ns
#mesh.mesh_use_embedded_materials = True
mesh.scale = Vector3(1.,1.,1.)
mesh.color = ColorRGBA(*color)
mesh.mesh_resource = "package://pr2_description/meshes/gripper_v0/l_finger_tip.dae"
mesh.type = Marker.MESH_RESOURCE
mesh.pose.orientation = Quaternion(*tip_rot)
mesh.pose.position = Point(*tip_pos)
if control != None:
control.markers.append( mesh )
elif marker_array != None:
mesh.header.frame_id = ps.header.frame_id
mesh.id = mesh_id
mesh_id = mesh_id+1
marker_array.markers.append(mesh)
#transforming the right finger and finger tip
rot1 = tr.rot_angle_direction(3.14, np.matrix([1, 0, 0]))*tr.rot_angle_direction(angle_open, np.matrix([0, 0, 1]))
T1 = tr.composeHomogeneousTransform(rot1, [0.07691, -0.01, 0.])
rot2 = tr.rot_angle_direction(-1*angle_open, np.matrix([0, 0, 1]))
T2 = tr.composeHomogeneousTransform(rot2, [0.09137, 0.00495, 0.])
T_proximal = T0*T1
T_distal = T0*T1*T2
finger_pos = tr.tft.translation_from_matrix(T_proximal)
finger_rot = tr.tft.quaternion_from_matrix(T_proximal)
tip_pos = tr.tft.translation_from_matrix(T_distal)
tip_rot = tr.tft.quaternion_from_matrix(T_distal)
#making the marker for the right finger
mesh = Marker()
mesh.ns = ns
#mesh.mesh_use_embedded_materials = True
mesh.scale = Vector3(1.,1.,1.)
mesh.color = ColorRGBA(*color)
mesh.mesh_resource = "package://pr2_description/meshes/gripper_v0/l_finger.dae"
mesh.type = Marker.MESH_RESOURCE
mesh.pose.orientation = Quaternion(*finger_rot)
mesh.pose.position = Point(*finger_pos)
if control != None:
control.markers.append( mesh )
elif marker_array != None:
mesh.header.frame_id = ps.header.frame_id
mesh.id = mesh_id
mesh_id = mesh_id+1
marker_array.markers.append(mesh)
mesh = Marker()
mesh.ns = ns
#mesh.mesh_use_embedded_materials = True
mesh.scale = Vector3(1.,1.,1.)
mesh.color = ColorRGBA(*color)
mesh.mesh_resource = "package://pr2_description/meshes/gripper_v0/l_finger_tip.dae"
mesh.type = Marker.MESH_RESOURCE
mesh.pose.orientation = Quaternion(*tip_rot)
mesh.pose.position = Point(*tip_pos)
if control != None:
control.markers.append( mesh )
elif marker_array != None:
mesh.header.frame_id = ps.header.frame_id
mesh.id = mesh_id
mesh_id = mesh_id+1
marker_array.markers.append(mesh)
if control != None:
control.interaction_mode = InteractiveMarkerControl.BUTTON
return control
elif marker_array != None:
return marker_array
def make_pr2_gripper_marker(ps,
color,
orientation=None,
marker_array=None,
control=None,
mesh_id_start=0,
ns="pr2_gripper",
offset=-0.19):
mesh_id = mesh_id_start
# this is the palm piece
mesh = Marker()
mesh.ns = ns
#mesh.mesh_use_embedded_materials = True
mesh.scale = Vector3(1., 1., 1.)
mesh.color = ColorRGBA(*color)
mesh.mesh_resource = "package://pr2_description/meshes/gripper_v0/gripper_palm.dae"
mesh.type = Marker.MESH_RESOURCE
if orientation != None:
mesh.pose.orientation = Quaternion(*orientation)
else:
mesh.pose.orientation = Quaternion(0, 0, 0, 1)
mesh.pose.position.x = offset
if control != None:
control.markers.append(mesh)
elif marker_array != None:
mesh.pose.position = ps.pose.position
mesh.header.frame_id = ps.header.frame_id
mesh.id = mesh_id
mesh_id = mesh_id + 1
marker_array.markers.append(mesh)
# amount to open the gripper for each finger
angle_open = 0.4
if orientation == None:
rot0 = np.matrix(np.eye(3))
else:
rot0 = tr.quaternion_to_matrix(orientation)
if marker_array != None:
T0 = tr.composeHomogeneousTransform(
rot0, [ps.point.x, ps.point.y, ps.point.z])
else:
T0 = tr.composeHomogeneousTransform(rot0, [offset, 0.0, 0.])
#transforming the left finger and finger tip
rot1 = tr.rot_angle_direction(angle_open, np.matrix([0, 0, 1]))
T1 = tr.composeHomogeneousTransform(rot1, [0.07691, 0.01, 0.])
rot2 = tr.rot_angle_direction(-1 * angle_open, np.matrix([0, 0, 1]))
T2 = tr.composeHomogeneousTransform(rot2, [0.09137, 0.00495, 0.])
T_proximal = T0 * T1
T_distal = T0 * T1 * T2
finger_pos = tr.tft.translation_from_matrix(T_proximal)
finger_rot = tr.tft.quaternion_from_matrix(T_proximal)
tip_pos = tr.tft.translation_from_matrix(T_distal)
tip_rot = tr.tft.quaternion_from_matrix(T_distal)
#making the marker for the left finger
mesh = Marker()
mesh.ns = ns
#mesh.mesh_use_embedded_materials = True
mesh.scale = Vector3(1., 1., 1.)
mesh.mesh_resource = "package://pr2_description/meshes/gripper_v0/l_finger.dae"
mesh.color = ColorRGBA(*color)
mesh.type = Marker.MESH_RESOURCE
mesh.pose.orientation = Quaternion(*finger_rot)
mesh.pose.position = Point(*finger_pos)
if control != None:
control.markers.append(mesh)
elif marker_array != None:
mesh.header.frame_id = ps.header.frame_id
mesh.id = mesh_id
mesh_id = mesh_id + 1
marker_array.markers.append(mesh)
mesh = Marker()
mesh.ns = ns
#mesh.mesh_use_embedded_materials = True
mesh.scale = Vector3(1., 1., 1.)
mesh.color = ColorRGBA(*color)
mesh.mesh_resource = "package://pr2_description/meshes/gripper_v0/l_finger_tip.dae"
mesh.type = Marker.MESH_RESOURCE
mesh.pose.orientation = Quaternion(*tip_rot)
mesh.pose.position = Point(*tip_pos)
if control != None:
control.markers.append(mesh)
elif marker_array != None:
mesh.header.frame_id = ps.header.frame_id
mesh.id = mesh_id
mesh_id = mesh_id + 1
marker_array.markers.append(mesh)
#transforming the right finger and finger tip
rot1 = tr.rot_angle_direction(3.14, np.matrix(
[1, 0, 0])) * tr.rot_angle_direction(angle_open, np.matrix([0, 0, 1]))
T1 = tr.composeHomogeneousTransform(rot1, [0.07691, -0.01, 0.])
rot2 = tr.rot_angle_direction(-1 * angle_open, np.matrix([0, 0, 1]))
T2 = tr.composeHomogeneousTransform(rot2, [0.09137, 0.00495, 0.])
T_proximal = T0 * T1
T_distal = T0 * T1 * T2
finger_pos = tr.tft.translation_from_matrix(T_proximal)
finger_rot = tr.tft.quaternion_from_matrix(T_proximal)
tip_pos = tr.tft.translation_from_matrix(T_distal)
tip_rot = tr.tft.quaternion_from_matrix(T_distal)
#making the marker for the right finger
mesh = Marker()
mesh.ns = ns
#mesh.mesh_use_embedded_materials = True
mesh.scale = Vector3(1., 1., 1.)
mesh.color = ColorRGBA(*color)
mesh.mesh_resource = "package://pr2_description/meshes/gripper_v0/l_finger.dae"
mesh.type = Marker.MESH_RESOURCE
mesh.pose.orientation = Quaternion(*finger_rot)
mesh.pose.position = Point(*finger_pos)
if control != None:
control.markers.append(mesh)
elif marker_array != None:
mesh.header.frame_id = ps.header.frame_id
mesh.id = mesh_id
mesh_id = mesh_id + 1
marker_array.markers.append(mesh)
mesh = Marker()
mesh.ns = ns
#mesh.mesh_use_embedded_materials = True
mesh.scale = Vector3(1., 1., 1.)
mesh.color = ColorRGBA(*color)
mesh.mesh_resource = "package://pr2_description/meshes/gripper_v0/l_finger_tip.dae"
mesh.type = Marker.MESH_RESOURCE
mesh.pose.orientation = Quaternion(*tip_rot)
mesh.pose.position = Point(*tip_pos)
if control != None:
control.markers.append(mesh)
elif marker_array != None:
mesh.header.frame_id = ps.header.frame_id
mesh.id = mesh_id
mesh_id = mesh_id + 1
marker_array.markers.append(mesh)
if control != None:
control.interaction_mode = InteractiveMarkerControl.BUTTON
return control
elif marker_array != None:
return marker_array
