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 --------------------