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 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 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_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 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 setup_forearm_taxels_transforms(self):
n_circum = 4
n_axis = 3
self.link_name_forearm = '/wrist_LEFT'
rad = 0.04
dist_along_axis = 0.065
angle_along_circum = 2 * math.pi / n_circum
offset_along_axis = 0.02
offset_along_circum = math.radians(-45)
n_taxels = n_circum * n_axis
self.tar_forearm.data = [None for i in range(n_taxels)]
# mapping the taxels to the raw ADC list.
idx_list = [6, 9, 0, 3, 7, 10, 1, 4, 8, 11, 2, 5]
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_forearm.data[idx_list[i * n_circum + j]] = t
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 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 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
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 test_elbow_angle():
firenze = hr.M3HrlRobot(connect=False)
rot_mat = tr.Rz(math.radians(-90.))*tr.Ry(math.radians(-90))
x_list = [0.55,0.45,0.35]
y = -0.2
z = -0.23
for x in x_list:
p0 = np.matrix([x,y,z]).T
q = firenze.IK('right_arm',p0,rot_mat)
# q[1] = math.radians(15)
# q = firenze.IK('right_arm',p0,rot_mat,q_guess = q)
elbow_angle = firenze.elbow_plane_angle('right_arm',q)
print '---------------------------------------'
print 'ee position:', p0.A1
# print 'joint angles:', q
print 'elbow_angle:', math.degrees(elbow_angle)
def get_firm_hook(self, hook_angle):
rot_mat = tr.Rz(hook_angle - hook_3dprint_angle) * tr.Ry(
math.radians(-90))
# move in the +x until contact.
vec = np.matrix([0.08, 0., 0.]).T
self.firenze.move_till_hit('right_arm',
vec=vec,
force_threshold=2.0,
rot=rot_mat,
speed=0.05)
# now move in direction of hook.
vec = tr.rotX(-hook_angle) * np.matrix([0., 0.05, 0.]).T
self.firenze.move_till_hit('right_arm',
vec=vec,
force_threshold=5.0,
rot=rot_mat,
speed=0.05,
bias_FT=False)
self.firenze.set_arm_settings(self.settings_r, None)
self.firenze.step()
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 pose_arm(self, hook_angle):
print 'press ENTER to pose the robot.'
k = m3t.get_keystroke()
if k != '\r':
print 'You did not press ENTER.'
return
# settings_r_orig = copy.copy(self.firenze.arm_settings['right_arm'])
settings_torque_gc = hr.MekaArmSettings(
stiffness_list=[0., 0., 0., 0., 0.], control_mode='torque_gc')
self.firenze.set_arm_settings(settings_torque_gc, None)
self.firenze.step()
print 'hit ENTER to end posing, something else to exit'
k = m3t.get_keystroke()
p = self.firenze.end_effector_pos('right_arm')
q = self.firenze.get_joint_angles('right_arm')
# self.firenze.set_arm_settings(settings_r_orig,None)
self.firenze.set_arm_settings(self.settings_stiff, None)
self.firenze.set_joint_angles('right_arm', q)
self.firenze.step()
self.firenze.set_joint_angles('right_arm', q)
self.firenze.step()
rot_mat = tr.Rz(hook_angle - hook_3dprint_angle) * tr.Ry(
math.radians(-90))
self.firenze.go_cartesian('right_arm', p, rot_mat, speed=0.1)
print 'hit ENTER after making finer adjustment, something else to exit'
k = m3t.get_keystroke()
p = self.firenze.end_effector_pos('right_arm')
q = self.firenze.get_joint_angles('right_arm')
self.firenze.set_joint_angles('right_arm', q)
self.firenze.step()
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)
def rot_mat_from_angles(hook, surface):
rot_mat = tr.Rz(hook) * tr.Rx(surface) * tr.Ry(math.radians(-90))
return rot_mat
if __name__ == '__main__':
settings_r = hr.MekaArmSettings(
stiffness_list=[0.1939, 0.6713, 0.997, 0.7272, 0.75])
firenze = hr.M3HrlRobot(connect=True, right_arm_settings=settings_r)
print 'hit a key to power up the arms.'
k = m3t.get_keystroke()
firenze.power_on()
print 'hit a key to test IK'
k = m3t.get_keystroke()
rot = tr.Ry(math.radians(-90))
p = np.matrix([0.3, -0.40, -0.2]).T
firenze.motors_on()
#firenze.go_cartesian('right_arm', p, rot)
# jep from springloaded door, trial 15
jep = [
-0.30365041761032346, 0.3490658503988659, 0.59866827092412689,
1.7924513637028943, 0.4580617747379146, -0.13602429148726047,
-0.48610218950666179
]
firenze.go_jointangles('right_arm', jep)
print 'hit a key to record equilibrium position'
k = m3t.get_keystroke()
use_right_arm = False
arm = 'left_arm'
else:
use_left_arm = False
use_right_arm = True
arm = 'right_arm'
cmg = CompliantMotionGenerator(move_segway_flag, use_right_arm,
use_left_arm)
print 'hit a key to power up the arms.'
k=m3t.get_keystroke()
cmg.firenze.power_on()
if reposition_flag:
rot_mat = tr.Rz(hook_angle)*tr.Ry(math.radians(-90))
cmg.firenze.pose_robot(arm,rot_mat)
hook_location = cmg.firenze.end_effector_pos(arm)
g = cmg.reposition_robot(hook_location)
cmg.firenze.go_cartesian(arm,g,rot_mat,speed=0.1)
if search_hook_flag:
hook_angle = math.radians(ha)
surface_angle = math.radians(0.)
p = np.matrix([0.45,-0.2,-0.23]).T
if arm == 'left_arm':
p[1,0] = p[1,0] * -1
print 'hit a key to search and hook.'
k=m3t.get_keystroke()
res, jep = cmg.search_and_hook(arm, hook_angle, p,
surface_angle)
rospy.init_node('fabric_skin_registration_node')
rdc = spc.RawDataClient('/fabric_skin/taxels/raw_data')
bias_mn, bias_std = compute_bias(rdc, 20)
for i in range(n_axis):
for j in range(n_circum):
raw_input('Press on taxel and hit ENTER')
dat = rdc.get_raw_data(True)
min_idx = np.argmin(dat - bias_mn)
ang = j * angle_along_circum + offset_along_circum
x = rad * math.cos(ang)
y = rad * math.sin(ang)
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)
tar.data[min_idx].translation.x = x
tar.data[min_idx].translation.y = y
tar.data[min_idx].translation.z = z
tar.data[min_idx].rotation.x = quat[0]
tar.data[min_idx].rotation.y = quat[1]
tar.data[min_idx].rotation.z = quat[2]
tar.data[min_idx].rotation.w = quat[3]
d = {}
d['tf_name'] = tf_link_name
d['transform_array_response'] = tar
ut.save_pickle(d, 'taxel_registration_dict.pkl')
def pull(self,
hook_angle,
force_threshold,
use_utm=False,
use_camera=False,
strategy='line_neg_x',
pull_loc=None,
info_string=''):
''' force_threshold - max force at which to stop pulling.
hook_angle - radians(0, -90, 90 etc.)
0 - horizontal, -pi/2 hook points up, +pi/2 hook points down
use_utm - to take 3D scans or not.
use_camera - to take pictures from the camera or not.
strategy - 'line_neg_x': move eq point along -x axis.
'piecewise_linear': try and estimate circle and move along it.
'control_radial_force': try and keep the radial force constant
'control_radial_dist'
pull_loc - 3x1 np matrix of location for pulling. If None then arm will go into
gravity comp and user can show the location.
info_string - string saved with key 'info' in the pkl.
'''
if use_utm:
self.firenze.step()
armconfig1 = self.firenze.get_joint_angles('right_arm')
plist1, slist1 = self.scan_3d()
if use_camera:
cam_plist1, cam_imlist1 = self.image_region()
else:
cam_plist1, cam_imlist1 = None, None
rot_mat = tr.Rz(hook_angle - hook_3dprint_angle) * tr.Ry(
math.radians(-90))
if pull_loc == None:
self.pose_arm(hook_angle)
pull_loc = self.firenze.end_effector_pos('right_arm')
ut.save_pickle(
pull_loc,
'pull_loc_' + info_string + '_' + ut.formatted_time() + '.pkl')
else:
pt1 = copy.copy(pull_loc)
pt1[0, 0] = pt1[0, 0] - 0.1
print 'pt1:', pt1.A1
print 'pull_loc:', pull_loc.A1
self.firenze.go_cartesian('right_arm', pt1, rot_mat, speed=0.2)
self.firenze.go_cartesian('right_arm',
pull_loc,
rot_mat,
speed=0.07)
print 'press ENTER to pull'
k = m3t.get_keystroke()
if k != '\r':
return
time_dict = {}
time_dict['before_hook'] = time.time()
print 'first getting a good hook'
self.get_firm_hook(hook_angle)
time.sleep(0.5)
time_dict['before_pull'] = time.time()
print 'pull begins'
stiffness_scale = self.settings_r.stiffness_scale
vec = tr.rotX(-hook_angle) * np.matrix(
[0., 0.05 / stiffness_scale, 0.]).T
self.keep_hook_vec = vec
self.hook_maintain_dist_plane = np.dot(vec.A1, np.array([0., 1., 0.]))
self.eq_pt_cartesian = self.firenze.end_effector_pos('right_arm') + vec
q_eq = self.firenze.IK('right_arm', self.eq_pt_cartesian, rot_mat)
self.firenze.go_jointangles('right_arm', q_eq, speed=math.radians(30))
self.q_guess = q_eq
# self.q_guess = self.firenze.get_joint_angles('right_arm')
self.pull_trajectory = at.JointTrajectory()
self.jt_torque_trajectory = at.JointTrajectory()
self.eq_pt_trajectory = at.JointTrajectory()
self.force_trajectory = at.ForceTrajectory()
self.firenze.step()
start_config = self.firenze.get_joint_angles('right_arm')
self.eq_IK_rot_mat = rot_mat # for equi pt generators.
self.eq_force_threshold = force_threshold
self.hooked_location_moved = False # flag to indicate when the hooking location started moving.
self.prev_force_mag = np.linalg.norm(
self.firenze.get_wrist_force('right_arm'))
self.eq_motion_vec = np.matrix([-1., 0., 0.]).T
self.slip_count = 0
if strategy == 'line_neg_x':
result = self.compliant_motion(self.equi_pt_generator_line, 0.025)
elif strategy == 'control_radial_force':
self.cartesian_pts_list = []
self.piecewise_force_threshold = force_threshold
self.rad_guess = 1.0
self.cx = 0.6
self.cy = -self.rad_guess
self.start() # start the circle estimation thread
result = self.compliant_motion(
self.equi_pt_generator_control_radial_force, 0.025)
else:
raise RuntimeError('unknown pull strategy: ', strategy)
if result == 'slip: force step decrease' or result == 'danger of self collision':
self.firenze.motors_off()
print 'powered off the motors.'
print 'Compliant motion result:', result
print 'Original force threshold:', force_threshold
print 'Adapted force threshold:', self.eq_force_threshold
time_dict['after_pull'] = time.time()
d = {
'actual': self.pull_trajectory,
'eq_pt': self.eq_pt_trajectory,
'force': self.force_trajectory,
'torque': self.jt_torque_trajectory,
'stiffness': self.firenze.arm_settings['right_arm'],
'info': info_string,
'force_threshold': force_threshold,
'eq_force_threshold': self.eq_force_threshold,
'hook_angle': hook_angle,
'result': result,
'strategy': strategy,
'time_dict': time_dict
}
self.firenze.step()
armconfig2 = self.firenze.get_joint_angles('right_arm')
if use_utm:
plist2, slist2 = self.scan_3d()
d['start_config'] = start_config
d['armconfig1'] = armconfig1
d['armconfig2'] = armconfig2
d['l1'], d['l2'] = 0., -0.055
d['scanlist1'], d['poslist1'] = slist1, plist1
d['scanlist2'], d['poslist2'] = slist2, plist2
d['cam_plist1'] = cam_plist1
d['cam_imlist1'] = cam_imlist1
ut.save_pickle(
d, 'pull_trajectories_' + d['info'] + '_' + ut.formatted_time() +
'.pkl')
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 rollTtool_MA( residuals = np.zeros(6) ):
rotResid, dispResid = residualXform( residuals )
rot = rotResid * tr.Ry( math.radians( -90.0 ))
disp = dispResid + np.matrix([ 0.0476, 0.0, 0.0 ]).T
return tr.composeHomogeneousTransform(rot, disp)
def generate_pointcloud(pos_list,
scan_list,
min_angle,
max_angle,
l1,
l2,
save_scan=False,
max_dist=np.Inf,
min_dist=-np.Inf,
min_tilt=-np.Inf,
max_tilt=np.Inf,
get_intensities=False,
reject_zero_ten=True):
''' pos_list - list of motor positions (in steps)
scan_list - list of UrgScan objects at the corres positions.
l1,l2 - parameterizing the transformation (doc/ folder)
save_scan - pickle 3xN numpy matrix of points.
max_dist - ignore points at distance > max_dist
min_dist - ignore points at distance < max_dist
min_tilt, max_tilt - min and max tilt angles (radians)
+ve tilts the hokuyo down.
get_intensites - additionally return intensity values
returns 3xN numpy matrix of 3d coords of the points, returns (3xN, 1xN) including the intensity values if get_intensites=True
'''
t0 = time.time()
allpts = []
allintensities = []
pos_arr = np.array(pos_list)
scan_arr = np.array(scan_list)
idxs = np.where(np.multiply(pos_arr <= max_tilt, pos_arr >= min_tilt))
pos_list = pos_arr[idxs].tolist()
scan_list = scan_arr[idxs].tolist()
n_scans = len(scan_list)
if n_scans > 1:
scan_list = copy.deepcopy(scan_list)
# remove graze in across scans.
ranges_mat = []
for s in scan_list:
ranges_mat.append(s.ranges.T)
ranges_mat = np.column_stack(ranges_mat)
start_index = hp.angle_to_index(scan_list[0], min_angle)
end_index = hp.angle_to_index(scan_list[0], max_angle)
for r in ranges_mat[start_index:end_index + 1]:
hp.remove_graze_effect(r,
np.matrix(pos_list),
skip=1,
graze_angle_threshold=math.radians(169.))
for i, s in enumerate(scan_list):
s.ranges = ranges_mat[:, i].T
for p, s in zip(pos_list, scan_list):
mapxydata = hp.get_xy_map(
s,
min_angle,
max_angle,
max_dist=max_dist,
min_dist=min_dist,
reject_zero_ten=reject_zero_ten,
sigmoid_hack=True,
get_intensities=get_intensities) # pts is 2xN
if get_intensities == True:
pts, intensities = mapxydata
allintensities.append(intensities)
else:
pts = mapxydata
pts = np.row_stack((pts, np.zeros(pts.shape[1]))) # pts is now 3xN
pts = tr.Ry(-p) * (pts + np.matrix((l1, 0, -l2)).T)
allpts.append(pts)
allpts = np.column_stack(allpts)
if save_scan:
date_name = ut.formatted_time()
ut.save_pickle(allpts, date_name + '_cloud.pkl')
t1 = time.time()
# print 'Time to generate point cloud:', t1-t0
# allpts = tr.rotZ(math.radians(5))*allpts
if get_intensities == True:
allintensities = np.column_stack(allintensities)
return allpts, allintensities
else:
return allpts
def laserTtilt(tiltAng, residuals = np.zeros(6) ):
rotResid, dispResid = residualXform( residuals )
rot = rotResid * tr.Ry( +1.0 * tiltAng )
disp = dispResid + np.matrix([ 0.03354, 0.0, 0.23669 ]).T
return tr.composeHomogeneousTransform(rot, disp)
# cmg.pull(math.radians(ha), ft,use_utm=scan_flag,use_camera=camera_flag,
# strategy = 'piecewise_linear',pull_loc=pull_loc,info_string=info_string)
cmg.pull(math.radians(ha),
ft,
use_utm=scan_flag,
use_camera=camera_flag,
strategy='control_radial_force',
pull_loc=pull_loc,
info_string=info_string)
# cmg.pull(math.radians(ha), ft,use_utm=scan_flag,use_camera=camera_flag,
# strategy = 'line_neg_x',pull_loc=pull_loc,info_string=info_string)
if firm_hook_flag:
hook_angle = math.radians(ha)
p = np.matrix([0.3, -0.25, -0.2]).T
rot_mat = tr.Rz(hook_angle - hook_3dprint_angle) * tr.Ry(
math.radians(-90))
cmg.firenze.go_cartesian('right_arm', p, rot_mat, speed=0.1)
print 'hit a key to get a firm hook.'
k = m3t.get_keystroke()
cmg.get_firm_hook(hook_angle)
print 'hit a key to end everything'
k = m3t.get_keystroke()
cmg.stop()
# cmg = CompliantMotionGenerator()
# print 'hit a key to test IK'
# k=m3t.get_keystroke()
# cmg.get_firm_hook(ha)
#----------- non-class functions test --------------------
