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 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 get_wrist_force(self, arm):
if arm == 'right_arm' and self.ftclient_r != None:
return self.get_wrist_force_netft('r')
if arm == 'left_arm' and self.ftclient_l != None:
return self.get_wrist_force_netft('l')
m = []
lc = self.fts[arm]
m.append(lc.get_Fx_mN() / 1000.)
m.append(lc.get_Fy_mN() / 1000.)
m.append(-lc.get_Fz_mN() / 1000.)
m.append(lc.get_Tx_mNm() / 1000.)
m.append(lc.get_Ty_mNm() / 1000.)
m.append(-lc.get_Tz_mNm() / 1000.)
m = np.matrix(m).T
r = tr.Rz(math.radians(-30.0))
m1 = r * m[0:3, :]
m1[1, 0] = -m1[1, 0]
m1[2, 0] = -m1[2, 0]
m2 = r * m[3:, :]
m2[1, 0] = -m2[1, 0]
m2[2, 0] = -m2[2, 0]
return np.row_stack((m1, m2))
def get_wrist_torque(self, arm):
m = []
lc = self.fts[arm]
m = tr.Rz(math.radians(-30.0)) * np.matrix(m).T
m[1, 0] = -m[1, 0]
m[2, 0] = -m[2, 0]
return m
def search_and_hook(self, arm, hook_angle, hook_loc, angle,
hooking_force_threshold = 5.):
#rot_mat = tr.Rz(hook_angle) * tr.Rx(angle) * tr.Ry(math.radians(-90))
rot_mat = rot_mat_from_angles(hook_angle, angle)
if arm == 'right_arm':
hook_dir = np.matrix([0.,1.,0.]).T # hook direc in home position
elif arm == 'left_arm':
hook_dir = np.matrix([0.,-1.,0.]).T # hook direc in home position
else:
raise RuntimeError('Unknown arm: %s', arm)
start_loc = hook_loc + rot_mat.T * hook_dir * -0.03 # 3cm direc opposite to hook.
# vector normal to surface and pointing into the surface.
normal_tl = tr.Rz(-angle) * np.matrix([1.0,0.,0.]).T
pt1 = start_loc - normal_tl * 0.1
pt1[2,0] -= 0.02 # funny error in meka control code? or is it gravity comp?
self.firenze.go_cartesian(arm, pt1, rot_mat, speed=0.2)
vec = normal_tl * 0.2
s, jep = self.firenze.move_till_hit(arm, vec=vec, force_threshold=2.0,
rot=rot_mat, speed=0.07)
self.eq_pt_cartesian = self.firenze.FK(arm, jep)
self.eq_pt_cartesian_ts = self.firenze.FK(arm, jep)
self.start_pos = copy.copy(self.eq_pt_cartesian)
self.q_guess = jep
move_dir = rot_mat.T * hook_dir
arg_list = [arm, move_dir, rot_mat, hooking_force_threshold]
s, jep = self.compliant_motion(self.equi_generator_surface_follow, 0.05,
arm, arg_list)
return s, jep
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 fill_grid(self, pts, ignore_z=False):
if ignore_z:
idx = np.where(
np.min(
np.multiply(pts[0:2, :] > self.brf[0:2, :],
pts[0:2, :] < self.tlb[0:2, :]), 0))[1]
else:
idx = np.where(
np.min(
np.multiply(pts[0:3, :] > self.brf,
pts[0:3, :] < self.tlb), 0))[1]
if idx.shape[1] == 0:
print 'aha!'
return
pts = pts[:, idx.A1.tolist()]
# Find coordinates
p_all = np.round((pts[0:3, :] - self.brf) / self.resolution)
# Rotate points
pts[0:3, :] = tr.Rz(self.rotation_z).T * pts[0:3, :]
for i, p in enumerate(p_all.astype('int').T):
if ignore_z:
p[0, 2] = 0
if np.any(p < 0) or np.any(p >= self.grid_shape.T):
continue
tup = tuple(p.A1)
self.grid_points_list[tup[0] + self.grid_shape[0, 0] * tup[1] +
self.grid_shape[0, 0] *
self.grid_shape[1, 0] * tup[2]].append(
pts[:, i])
self.grid[tuple(p.A1)] += 1
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 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 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 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 reposition_robot(self,hook_location,turn_angle,hook_angle,position_number=2):
h = self.workspace_dict[hook_angle]['height']
max_z_height = 1.3
min_z_height = 0.5
h_desired = self.z.get_position_meters()+hook_location[2,0]-h
print 'h_desired:',h_desired
print 'h:',h
h_achieve = ut.bound(h_desired,max_z_height,min_z_height)
h_err = h_desired-h_achieve
h = h+h_err
print 'h_achieve:',h_achieve
wkd = self.workspace_dict[hook_angle]['pts']
k = wkd.keys()
k_idx = np.argmin(np.abs(np.array(k)-h))
wrkspc_pts = wkd[k[k_idx]]
# good_location = np.matrix([0.45,-0.35,h]).T
good_location = wrkspc_pts.mean(1)
good_location = np.matrix(good_location.A1.tolist()+[h]).T
if position_number == 1:
good_location[0,0] += 0.0
good_location[1,0] -= 0.1
elif position_number == 2:
good_location[0,0] += 0.0
good_location[1,0] -= 0.0
elif position_number == 3:
good_location[0,0] += 0.0
good_location[1,0] += 0.1
else:
good_location[0,0] -= 0.1
good_location[1,0] -= 0.0
good_location = mcf.mecanumTglobal(mcf.globalTtorso(good_location))
hook_location = mcf.mecanumTglobal(mcf.globalTtorso(hook_location))
v = tr.Rz(-turn_angle)*good_location
move_vector = hook_location - v
pose1 = self.segway_pose.get_pose()
self.segway_command_node.go_xya_pos_local(move_vector[0,0],move_vector[1,0],
turn_angle,blocking=False)
self.z.torque_move_position(h_achieve)
print 'before waiting for segway to stop moving.'
self.segway_command_node.wait_for_tolerance_pos(0.03,0.03,math.radians(4))
print 'after segway has stopped moving.'
pose2 = self.segway_pose.get_pose()
hl_new = vop.transform_pts_local(hook_location[0:2,:],pose1,pose2)
h_err = h_desired-self.z.get_position_meters()
g = np.matrix(hl_new.A1.tolist()+[good_location[2,0]+h_err]).T
g = mcf.torsoTglobal(mcf.globalTmecanum(g))
angle_turned = pose2[2]-pose1[2]
angle_error = tr.angle_within_mod180(turn_angle-angle_turned)
self.segway_pose.set_origin() # re-origining for the manipulation behaviors
return g, angle_error
def get_wrist_force(self, arm):
m = []
lc = self.fts[arm]
m.append(lc.get_Fx_mN() / 1000.)
m.append(lc.get_Fy_mN() / 1000.)
m.append(-lc.get_Fz_mN() / 1000.)
m = tr.Rz(math.radians(-30.0)) * np.matrix(m).T
m[1, 0] = -m[1, 0]
m[2, 0] = -m[2, 0]
return m
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 get_wrist_force_netft(self, arm):
if arm == 'r':
w = self.ftclient_r.read()
elif arm == 'l':
w = self.ftclient_l.read()
r = tr.Rz(math.radians(30.))
f = r * w[0:3]
t = r * w[0:3]
f[1, 0] = f[1, 0] * -1
t[1, 0] = t[1, 0] * -1
return np.row_stack((f, t))
def error_function(params):
mx, my, ma = params[0], params[1], params[2]
mx, my, ma = mx / scale_x, my / scale_y, ma / scale_a
#x,y = params[0],params[1]
pts_in = point_contained(mx, my, ma, cx, cy, rad, pts, start_angle,
end_angle, buffer)
p = tr.Rz(ma) * curr_pos - np.matrix([mx, my, 0.]).T
p_eq = tr.Rz(ma) * eq_pos - np.matrix([mx, my, 0.]).T
dist_moved = math.sqrt(mx * mx + my * my) + abs(ma) * 0.2
dist_bndry = dist_from_boundary(p_eq, bndry, pts)
alpha = math.pi - (start_angle - ma)
if alpha < min_alpha:
alpha_cost = min_alpha - alpha
elif alpha > max_alpha:
alpha_cost = alpha - max_alpha
else:
alpha_cost = 0.
alpha_cost = alpha_cost * alpha_weight
move_cost = dist_moved * dist_moved_weight
bndry_cost = dist_bndry * bndry_weight
pts_in_cost = pts_in.shape[1] / 1000. * pts_in_weight
# print '---------------------------------'
# print 'alpha:',math.degrees(alpha)
# print 'alpha_cost:',alpha_cost
# print 'mx,my:',mx,my
# print 'dist_moved:',dist_moved
# print 'dist_bndry:',dist_bndry
# print 'pts_in.shape[1]:',pts_in.shape[1]
# print 'move_cost:', move_cost
# print 'bndry_cost:',bndry_cost
# print 'pts_in_cost:',pts_in_cost
# return -pts_in.shape[1]+dist_moved - bndry_cost
err = -pts_in_cost - bndry_cost + move_cost + alpha_cost
# print 'error function value:',err
return err
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 create_grid(brf,
tlb,
resolution,
pos_list,
scan_list,
l1,
l2,
display_flag=False,
show_pts=True,
rotation_angle=0.,
occupancy_threshold=1):
''' rotation angle - about the Z axis.
'''
max_dist = np.linalg.norm(tlb) + 0.2
min_dist = brf[0, 0]
min_angle, max_angle = math.radians(-60), math.radians(60)
all_pts = generate_pointcloud(pos_list,
scan_list,
min_angle,
max_angle,
l1,
l2,
max_dist=max_dist,
min_dist=min_dist)
rot_mat = tr.Rz(rotation_angle)
all_pts_rot = rot_mat * all_pts
gr = og3d.occupancy_grid_3d(brf,
tlb,
resolution,
rotation_z=rotation_angle)
gr.fill_grid(all_pts_rot)
gr.to_binary(occupancy_threshold)
if display_flag == True:
if show_pts:
d3m.plot_points(all_pts, color=(0., 0., 0.))
cube_tups = gr.grid_lines(rotation_angle=rotation_angle)
d3m.plot_cuboid(cube_tups)
return gr
def create_overhead_grasp_choice_grid(pt,
pos_list,
scan_list,
l1,
l2,
far_dist,
display_list=None):
# y_pos = max(pt[1,0]+0.1,0.1)
# y_neg = min(pt[1,0]-0.1,-0.1)
#
# brf = np.matrix([0.2,y_neg,0.0]).T
# tlb = np.matrix([pt[0,0]+far_dist,y_pos,pt[2,0]+0.75]).T
#
# resolution = np.matrix([0.02,0.02,0.02]).T
y_pos = 0.1
y_neg = -0.1
r = np.linalg.norm(pt[0:2, 0])
brf = np.matrix([0.25, y_neg, 0.0]).T
tlb = np.matrix([r + far_dist, y_pos, pt[2, 0] + 0.75]).T
resolution = np.matrix([0.02, 0.02, 0.02]).T
rotation_angle = math.atan2(pt[1, 0], pt[0, 0])
print 'rotation_angle:', math.degrees(rotation_angle)
gr = create_grid(brf,
tlb,
resolution,
pos_list,
scan_list,
l1,
l2,
display_list,
rotation_angle=rotation_angle)
if display_list != None:
collide_pts = gr.grid_to_points()
if collide_pts.shape[1] > 0:
collide_pts = tr.Rz(rotation_angle).T * gr.grid_to_points()
display_list.insert(0, pu.PointCloud(collide_pts, color=(0, 0, 0)))
return gr
def point_contained(mx, my, ma, cx, cy, rad, pts, start_angle, end_angle,
buffer):
if abs(mx) > 0.2 or abs(my) > 0.2 or abs(ma) > math.radians(40):
# print 'too large a motion for point_contained'
return np.array([[]])
pts_t = pts + np.matrix([mx, my]).T
pts_t = tr.Rz(-ma)[0:2, 0:2] * pts_t
r, t = cartesian_to_polar(pts_t - np.matrix([cx, cy]).T)
t = np.mod(t, math.pi * 2) # I want theta to be b/w 0 and 2pi
if start_angle < end_angle:
f = np.row_stack((r < rad + buffer, r > rad - buffer / 2.,
t < end_angle, t > start_angle))
else:
f = np.row_stack((r < rad + buffer, r > rad - buffer / 2.,
t < start_angle, t > end_angle))
idxs = np.where(np.all(f, 0))
r_filt = r[idxs]
t_filt = t[idxs]
return polar_to_cartesian(r_filt, t_filt) + np.matrix([cx, cy]).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 create_vertical_plane_grid(pt,
pos_list,
scan_list,
l1,
l2,
rotation_angle,
display_list=None):
rot_mat = tr.Rz(rotation_angle)
t_pt = rot_mat * pt
brf = t_pt + np.matrix([-0.2, -0.3, -0.2]).T
tlb = t_pt + np.matrix([0.2, 0.3, 0.2]).T
resolution = np.matrix([0.005, 0.02, 0.02]).T
return create_grid(brf,
tlb,
resolution,
pos_list,
scan_list,
l1,
l2,
display_list,
rotation_angle=rotation_angle,
occupancy_threshold=1)
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()
def tiltTpan(panAng, residuals = np.zeros(6) ):
rotResid, dispResid = residualXform( residuals )
rot = rotResid * tr.Rz( -1.0 * panAng )
disp = dispResid + np.matrix([ 0.07124, 0.0, 0.02243 ]).T
return tr.composeHomogeneousTransform(rot, disp)
def rollTtool_pointer( residuals = np.zeros(6) ):
rotResid, dispResid = residualXform( residuals )
rot = rotResid * tr.Rz( math.radians( -10.0 ))
disp = dispResid + np.matrix([ 0.008, 0.0, 0.0 ]).T
return tr.composeHomogeneousTransform(rot, disp)