def setup_pps_left_taxels_transforms(self):
self.link_name_left_pps = '/%s_gripper_l_finger_tip_link'%(self.arm)
n_taxels = 3
self.tar_left_pps.data = [None for i in range(n_taxels)]
idx_list = [0, 1, 2]
x_disp = [0.03, 0.012, 0.012]
y_disp = [-0.01, -0.01, -0.01]
z_disp = [0.0, -0.011, 0.011]
x_ang = [0., math.pi, 0.]
y_ang = [-math.pi/2., 0., 0.]
z_ang = [0., 0., 0.]
for i in range(n_taxels):
t = Transform()
t.translation.x = x_disp[i]
t.translation.y = y_disp[i]
t.translation.z = z_disp[i]
rot_mat = tr.Rz(z_ang[i])*tr.Ry(y_ang[i])*tr.Rx(x_ang[i])
quat = tr.matrix_to_quaternion(rot_mat)
t.rotation.x = quat[0]
t.rotation.y = quat[1]
t.rotation.z = quat[2]
t.rotation.w = quat[3]
self.tar_left_pps.data[idx_list[i]] = t
def setup_forearm_twenty_two_taxels_transforms(self):
self.link_name_forearm = '/%s_forearm_link'%(self.arm)
n_taxels = 22
self.tar_forearm.data = [Transform()] * n_taxels # [Transform() for i in range(n_taxels)] #[None for i in range(n_taxels)]
idx_list = [14, 10, 16, 8, 9, 12, 0, 1, 4, 19, 21, 15, 7, 13, 2, 3, 6, 5, 20, 17, 11, 18]
x_disp = [.16, .23, .3, .16, .23, .3, .16, .23, .3, .16, .23, .3, .17, .28, .17, .28, .17, .28, .17, .28, .34, .34]
y_disp = [0., 0., 0., -0.06, -0.06, -0.06, 0., 0., 0., 0.06, 0.06, 0.06, -0.05, -0.05, -0.05, -0.05, 0.05, 0.05, 0.05, 0.05 ,-0.05, 0.05]
z_disp = [0.04, 0.02, 0.03, 0., 0., 0., -0.05, -0.05, -0.05, 0., 0., 0., 0.04, 0.02, -0.04, -0.04, -0.04, -0.04, 0.04, 0.02, 0., 0.]
x_ang = [0, 0, 0, -math.pi/2, -math.pi/2, -math.pi/2, math.pi, math.pi, math.pi, math.pi/2, math.pi/2, math.pi/2, -math.pi/4, -math.pi/4, -3*math.pi/4, -3*math.pi/4, 3*math.pi/4, 3*math.pi/4, math.pi/4, math.pi/4, math.radians(-30), math.radians(30)]
y_ang = [-math.pi/4, math.radians(-15), math.radians(20), 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, math.radians(-60), math.radians(-60)]
z_ang = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
for i in range(n_taxels):
t = Transform()
t.translation.x = x_disp[i]
t.translation.y = y_disp[i]
t.translation.z = z_disp[i]
rot_mat = tr.Rz(z_ang[i])*tr.Ry(y_ang[i])*tr.Rx(x_ang[i])
quat = tr.matrix_to_quaternion(rot_mat)
t.rotation.x = quat[0]
t.rotation.y = quat[1]
t.rotation.z = quat[2]
t.rotation.w = quat[3]
self.tar_forearm.data[idx_list[i]] = t
def handoff_pos(self, msg):
#pos = np.matrix([0.6977, -0.03622, 0.2015]).T
#ang = tr.Rx(math.radians(0.))
print msg
pos = np.matrix([msg.x, msg.y, msg.z]).T
ang = tr.Rx(msg.ang)
q = [0, 0, 0, 0, 0, 0, 0]
j = self.robot.IK('right_arm', pos, ang, self.target_ja)
#j = self.robot.IK('right_arm', pos, ang, q)
self.robot.set_jointangles('right_arm', j, 3.0)
# Tactile Sensor detector
rospy.sleep(rospy.Duration(0.5))
self.ts.thresh_detect(3000)
# Release object
self.robot.open_gripper(self.arm)
rospy.sleep(rospy.Duration(2.0))
# Stow
self.robot.set_jointangles(self.arm, self.start_ja, 3.0)
rospy.sleep(rospy.Duration(3.0))
return True
def setup_gripper_right_link_taxels_transforms(self):
self.link_name_gripper_right_link = '/%s_gripper_r_finger_link'%(self.arm)
n_taxels = 4
self.tar_gripper_right_link.data = [None for i in range(n_taxels)]
idx_list = [0, 1, 2, 3]
x_disp = [.03, .04, .03, 0.1]
y_disp = [-0.02, -0.04, -0.02, -0.02]
z_disp = [0.03, 0., -0.03, 0.]
x_ang = [0, -math.pi/2, math.pi, -math.pi/2]
y_ang = [math.radians(-5), 0, math.radians(5), 0]
z_ang = [0, math.radians(-10), 0, -math.pi/4]
for i in range(n_taxels):
t = Transform()
t.translation.x = x_disp[i]
t.translation.y = y_disp[i]
t.translation.z = z_disp[i]
rot_mat = tr.Rz(z_ang[i])*tr.Ry(y_ang[i])*tr.Rx(x_ang[i])
quat = tr.matrix_to_quaternion(rot_mat)
t.rotation.x = quat[0]
t.rotation.y = quat[1]
t.rotation.z = quat[2]
t.rotation.w = quat[3]
self.tar_gripper_right_link.data[idx_list[i]] = t
def ft_to_camera_3(force_tool, moment_tool, hand_rot_matrix, number,
return_moment_cam = False):
# hc == hand checkerboard
hc_rot_tool = tr.Rx(math.radians(90)) * tr.Ry(math.radians(180.)) * tr.Rz(math.radians(30.))
while number != 1:
hc_rot_tool = tr.Ry(math.radians(90.)) * hc_rot_tool
number = number-1
force_hc = hc_rot_tool * force_tool
moment_hc = hc_rot_tool * moment_tool
p_ft_hooktip = np.matrix([0.0, -0.08, 0.00]).T # vector from FT sensor to the tip of the hook in hook checker coordinates.
# p_ft_hooktip = np.matrix([0.0, -0.08 - 0.034, 0.00]).T # vector from FT sensor to the tip of the hook in hook checker coordinates.
p_ft_hooktip = hand_rot_matrix * p_ft_hooktip
force_cam = hand_rot_matrix * force_hc # force at hook base in camera coordinates
moment_cam = hand_rot_matrix * moment_hc
force_at_hook_tip = force_cam
moment_at_hook_tip = moment_cam + np.matrix(np.cross(-p_ft_hooktip.A1, force_cam.A1)).T
# if np.linalg.norm(moment_at_hook_tip) > 0.7:
# import pdb; pdb.set_trace()
if return_moment_cam:
return force_at_hook_tip, moment_at_hook_tip, moment_cam
else:
return force_at_hook_tip, moment_at_hook_tip
def camTlaser(res=np.zeros(7)):
# @ Duke, res = np.array([0.8, 0.9, -1.7, 3.1, 0.061, 0.032, -0.035 ])
rot = tr.Ry(math.radians(0.0 + res[0])) * tr.Rz(
math.radians(0.0 + res[1])) * tr.Rx(
math.radians(-90.0 + res[2])) * tr.Rz(math.radians(-90.0 + res[3]))
disp = np.matrix([res[4], res[5], res[6]]).T + np.matrix([0.0, 0.0, 0.0]).T
return tr.composeHomogeneousTransform(rot, disp)
def setup_gripper_palm_taxels_transforms(self):
self.link_name_gripper_palm = '/%s_gripper_palm_link'%(self.arm)
n_taxels = 2
self.tar_gripper_palm.data = [None for i in range(n_taxels)]
idx_list = [0, 1]
x_disp = [0.06, 0.06]
y_disp = [-0.02, 0.02]
z_disp = [0., 0.]
x_ang = [-math.pi/2, math.pi/2]
y_ang = [0, 0]
z_ang = [math.pi/4, -math.pi/4]
for i in range(n_taxels):
t = Transform()
t.translation.x = x_disp[i]
t.translation.y = y_disp[i]
t.translation.z = z_disp[i]
rot_mat = tr.Rz(z_ang[i])*tr.Ry(y_ang[i])*tr.Rx(x_ang[i])
quat = tr.matrix_to_quaternion(rot_mat)
t.rotation.x = quat[0]
t.rotation.y = quat[1]
t.rotation.z = quat[2]
t.rotation.w = quat[3]
self.tar_gripper_palm.data[idx_list[i]] = t
def residualXform( residuals ):
'''
residuals are np.array([ Rz2, Rx, Rz1, dx, dy, dz ])
returns rotResid, dispResid
'''
rotResid = tr.Rz( residuals[0] ) * tr.Rx( residuals[1] ) * tr.Rz( residuals[2] )
dispResid = np.matrix([ residuals[3], residuals[4], residuals[5] ]).T
return rotResid, dispResid
def ft_to_camera(force_tool, hand_rot_matrix, number):
# hc == hand checkerboard
hc_rot_tool = tr.Rx(math.radians(90)) * tr.Ry(math.radians(180.)) * tr.Rz(
math.radians(30.))
while number != 1:
hc_rot_tool = tr.Ry(math.radians(90.)) * hc_rot_tool
number = number - 1
force_hc = hc_rot_tool * force_tool
return hand_rot_matrix * force_hc
def ft_to_camera(force_tool, hand_rot_matrix, hand_pos_matrix, mech_pos_matrix, number):
# hc == hand checkerboard
hc_rot_tool = tr.Rx(math.radians(90)) * tr.Ry(math.radians(180.)) * tr.Rz(math.radians(30.))
while number != 1:
hc_rot_tool = tr.Ry(math.radians(90.)) * hc_rot_tool
number = number-1
force_hc = hc_rot_tool * force_tool
p_hc_ft = np.matrix([0.04, 0.01, 0.09]).T # vector from hook checkerboard origin to the base of the FT sensor in hook checker coordinates.
# vec from FT sensor to mechanism checker origin in camera coordinates.
p_ft_mech = -hand_pos_matrix + mech_pos_matrix - hand_rot_matrix * p_hc_ft
force_cam = hand_rot_matrix * force_hc # force at hook base in camera coordinates
moment_cam = hand_rot_matrix * moment_hc
force_at_mech_origin = force_cam
moment_at_mech_origin = moment_cam + np.matrix(np.cross(-p_ft_mech.A1, force_cam.A1)).T
return hand_rot_matrix * force_hc
def setup_upperarm_taxels_transforms(self):
n_circum = 4
n_axis = 1
self.link_name_upperarm = '/%s_upper_arm_link'%(self.arm)
angle_along_circum = 2*math.pi / n_circum
offset_along_circum = math.radians(0)
n_taxels = n_circum * n_axis
self.tar_upperarm.data = [None for i in range(n_taxels)]
# mapping the taxels to the raw ADC list.
# 0 - top; 1 - left; 2 - bottom; 3 - right
idx_list = [3, 1, 2, 0]
x_disp = [.3, .3, .3, 0]
y_disp = [0.06, 0, -0.06, 0]
z_disp = [-0.03, -0.09, -0.03, 0]
x_ang = [0, 0, 3*math.pi/2, 0]
y_ang = [math.pi/2, math.pi, 0, 0]
z_ang = [math.pi/2, 0, 0, 0]
for i in range(n_axis):
for j in range(n_circum):
t = Transform()
t.translation.x = x_disp[j]
t.translation.y = y_disp[j]
t.translation.z = z_disp[j]
rot_mat = tr.Rz(z_ang[j])*tr.Ry(y_ang[j])*tr.Rx(x_ang[j])
quat = tr.matrix_to_quaternion(rot_mat)
t.rotation.x = quat[0]
t.rotation.y = quat[1]
t.rotation.z = quat[2]
t.rotation.w = quat[3]
self.tar_upperarm.data[idx_list[i*n_circum+j]] = t
def utmcam0Tglobal_mat(ang):
thok0Tglobal_mat = tr.invertHomogeneousTransform(_globalT['thok0'])
# servo angle.
disp = np.matrix([0.,0.,0.]).T
tmat = tr.composeHomogeneousTransform(tr.Ry(ang),disp)*thok0Tglobal_mat
# cameraTlaser from thok_cam_calib.py
x = 0.012
y = -0.056
z = 0.035
r1 = 0.
r2 = 0.
r3 = -0.7
disp = np.matrix([-x,-y,-z]).T
r = tr.Rz(math.radians(-90))*tr.Ry(math.radians(90.))
disp = r*disp
r = r*tr.Rx(math.radians(r1))
r = r*tr.Ry(math.radians(r2))
r = r*tr.Rz(math.radians(r3))
t = tr.composeHomogeneousTransform(r, disp)
tmat = t*tmat
return tmat
self.set_joint_angles_traj(arm, q_arr, dur_arr)
if __name__ == '__main__':
rospy.init_node(node_name, anonymous=True)
log("Node initialized")
pr2_arm = PR2Arms()
if False:
q = [0, 0, 0, 0, 0, 0, 0]
pr2_arm.set_jointangles('right_arm', q)
ee_pos = simparm.FK('right_arm', q)
log('FK result:' + ee_pos.A1)
ee_pos[0, 0] -= 0.1
q_ik = pr2_arm.IK('right_arm', ee_pos, tr.Rx(0.), q)
log('q_ik:' + [math.degrees(a) for a in q_ik])
rospy.spin()
if False:
p = np.matrix([0.9, -0.3, -0.15]).T
#rot = tr.Rx(0.)
rot = tr.Rx(math.radians(90.))
pr2_arm.set_cartesian('right_arm', p, rot)
# #------ testing gripper opening and closing ---------
# raw_input('Hit ENTER to begin')
# pr2_arm.open_gripper(0)
# raw_input('Hit ENTER to close')
# pr2_arm.close_gripper(0, effort = 15)
def rot_mat_from_angles(hook, surface):
rot_mat = tr.Rz(hook) * tr.Rx(surface) * tr.Ry(math.radians(-90))
return rot_mat
rospy.init_node('epc_pr2', anonymous = True)
rospy.logout('epc_pr2: ready')
pr2 = hrl_pr2.HRL_PR2()
epc = EPC(pr2)
arm = 'right_arm'
if False:
ea = [0, 0, 0, 0, 0, 0, 0]
ea = epc.robot.get_joint_angles(arm)
rospy.logout('Going to starting position')
epc.robot.set_jointangles(arm, ea, duration=4.0)
raw_input('Hit ENTER to pull')
epc.pull_back(arm, ea, tr.Rx(0), 0.2)
if True:
p = np.matrix([0.9, -0.3, -0.15]).T
rot = tr.Rx(0.)
rot = tr.Rx(math.radians(90.))
rospy.logout('Going to starting position')
# epc.robot.open_gripper(arm)
epc.robot.set_cartesian(arm, p, rot)
# raw_input('Hit ENTER to close the gripper')
# epc.robot.close_gripper(arm)
raw_input('Hit ENTER to pull')
epc.pull_back_cartesian_control(arm, p, rot, 0.4)
def setup_wrist_taxels_transforms(self):
self.link_name_wrist = '/handmount_LEFT'
n_circum = 4
dist_along_axis = 0.065
angle_along_circum = 2 * math.pi / n_circum
offset_along_circum = math.radians(-45)
self.tar_wrist.data = [None for i in range(13)]
# mapping the taxels to the raw ADC list.
idx_list = [6, 9, 2, 5]
n_axis = 1
rad = 0.03
offset_along_axis = -0.04
for i in range(n_axis):
for j in range(n_circum):
t = Transform()
ang = j * angle_along_circum + offset_along_circum
t.translation.x = rad * math.cos(ang)
t.translation.y = rad * math.sin(ang)
t.translation.z = offset_along_axis + i * dist_along_axis
rot_mat = tr.Rz(-ang) * tr.Ry(math.radians(-90))
quat = tr.matrix_to_quaternion(rot_mat)
t.rotation.x = quat[0]
t.rotation.y = quat[1]
t.rotation.z = quat[2]
t.rotation.w = quat[3]
self.tar_wrist.data[idx_list[i * n_circum + j]] = t
# mapping the taxels to the raw ADC list.
idx_list = [8, 11, 0, 3, 7, 10, 1, 4]
n_axis = 2
rad = 0.02
offset_along_axis = -0.17
for i in range(n_axis):
for j in range(n_circum):
t = Transform()
ang = j * angle_along_circum + offset_along_circum
t.translation.x = rad * math.cos(ang)
t.translation.y = rad * math.sin(ang)
t.translation.z = offset_along_axis + i * dist_along_axis
rot_mat = tr.Rz(-ang) * tr.Ry(math.radians(-90))
quat = tr.matrix_to_quaternion(rot_mat)
t.rotation.x = quat[0]
t.rotation.y = quat[1]
t.rotation.z = quat[2]
t.rotation.w = quat[3]
self.tar_wrist.data[idx_list[i * n_circum + j]] = t
t = Transform()
t.translation.x = 0.
t.translation.y = 0
t.translation.z = -0.2
rot_mat = tr.Rx(math.radians(180))
quat = tr.matrix_to_quaternion(rot_mat)
t.rotation.x = quat[0]
t.rotation.y = quat[1]
t.rotation.z = quat[2]
t.rotation.w = quat[3]
self.tar_wrist.data[12] = t
def panTroll(rollAng, residuals = np.zeros(6) ):
rotResid, dispResid = residualXform( residuals )
rot = rotResid * tr.Rx( -1.0 * rollAng )
disp = dispResid + np.matrix([0.02021, 0.0, 0.04236 ]).T
return tr.composeHomogeneousTransform(rot, disp)
import math, numpy as np
from rviz_marker_test import *
from hrl_msgs.msg import FloatArray
if __name__ == '__main__':
ft_client = ftc.FTClient('force_torque_ft2')
ft_client.bias()
marker_pub = rospy.Publisher('/test_marker', Marker)
ati_pub = rospy.Publisher('/ati_ft', FloatArray)
p_st = np.matrix([0.,0.,0.]).T
force_frame_id = 'r_wrist_roll_link'
while not rospy.is_shutdown():
ft = ft_client.read(fresh = True)
rmat = tr.Rx(math.radians(180.)) * tr.Ry(math.radians(-90.)) * tr.Rz(math.radians(30.))
force = rmat * ft[0:3,:]
print 'Force:', force.A1
# force is now in the 'robot' coordinate frame.
force_scale = 0.1
p_end = p_st + force * force_scale
marker = get_marker_arrow(p_st, p_end, force_frame_id)
marker_pub.publish(marker)
farr = FloatArray()
farr.header.stamp = rospy.rostime.get_rostime()
farr.header.frame_id = force_frame_id
farr.data = (-force).A1.tolist()
ati_pub.publish(farr)
# rospy.sleep(0.1)