equilibrium_point_control: samples.py Source File

Go to the documentation of this file.
00001 
00002 
00003 
00004     ## Pull back along a straight line (-ve x direction)
00005     # @param arm - 'right_arm' or 'left_arm'
00006     # @param distance - how far back to pull.
00007     def pull_back_cartesian_control(self, arm, distance, logging_fn):
00008         cep, _ = self.robot.get_cep_jtt(arm)
00009         cep_end = cep + distance * np.matrix([-1., 0., 0.]).T
00010         self.dist_left = distance
00011 
00012         def eq_gen_pull_back(cep):
00013             logging_fn(arm)
00014             if self.dist_left <= 0.:
00015                 return 'done', None
00016             step_size = 0.01
00017             cep[0,0] -= step_size
00018             self.dist_left -= step_size
00019             if cep[0,0] < 0.4:
00020                 return 'very close to the body: %.3f'%cep[0,0], None
00021             return '', (cep, None)
00022         
00023         arg_list = [cep]
00024         s = self.epc_motion(eq_gen_pull_back, 0.1, arm, arg_list,
00025                     control_function = self.robot.set_cep_jtt)
00026         print s
00027 
00028     def move_till_hit(self, arm, vec=np.matrix([0.3,0.,0.]).T, force_threshold=3.0,
00029                       speed=0.1, bias_FT=True):
00030         unit_vec =  vec/np.linalg.norm(vec)
00031         time_step = 0.1
00032         dist = np.linalg.norm(vec)
00033         step_size = speed * time_step
00034         cep_start, _ = self.robot.get_cep_jtt(arm)
00035         cep = copy.copy(cep_start)
00036         def eq_gen(cep):
00037             force = self.robot.get_wrist_force(arm, base_frame = True)
00038             force_projection = force.T*unit_vec *-1 # projection in direction opposite to motion.
00039             print 'force_projection:', force_projection
00040             if force_projection>force_threshold:
00041                 return 'done', None
00042             elif np.linalg.norm(force)>45.:
00043                 return 'large force', None
00044             elif np.linalg.norm(cep_start-cep) >= dist:
00045                 return 'reached without contact', None
00046             else:
00047                 cep_t = cep + unit_vec * step_size
00048                 cep[0,0] = cep_t[0,0]
00049                 cep[1,0] = cep_t[1,0]
00050                 cep[2,0] = cep_t[2,0]
00051                 return '', (cep, None)
00052 
00053         if bias_FT:
00054             self.robot.bias_wrist_ft(arm)
00055         rospy.sleep(0.5)
00056         return self.epc_motion(eq_gen, time_step, arm, [cep],
00057                                control_function = self.robot.set_cep_jtt)
00058 
00059     def cep_gen_surface_follow(self, arm, move_dir, force_threshold,
00060                                cep, cep_start):
00061         wrist_force = self.robot.get_wrist_force(arm, base_frame=True)
00062         if wrist_force[0,0] < -3.:
00063             cep[0,0] -= 0.002
00064         if wrist_force[0,0] > -1.:
00065             cep[0,0] += 0.003
00066     
00067         if cep[0,0] > (cep_start[0,0]+0.05):
00068             cep[0,0] = cep_start[0,0]+0.05
00069     
00070         step_size = 0.002
00071         cep_t = cep + move_dir * step_size
00072         cep[0,0] = cep_t[0,0]
00073         cep[1,0] = cep_t[1,0]
00074         cep[2,0] = cep_t[2,0]
00075 
00076         v = cep - cep_start
00077         if (wrist_force.T * move_dir)[0,0] < -force_threshold:
00078             stop = 'got a hook'
00079         elif np.linalg.norm(wrist_force) > 50.:
00080             stop = 'force is large %f'%(np.linalg.norm(wrist_force))
00081         elif (v.T * move_dir)[0,0] > 0.20:
00082             stop = 'moved a lot without a hook'
00083         else:
00084             stop = ''
00085         return stop, (cep, None)
00086 
00087 
00088     def go_pose(self, arm, goal_pos, goal_rot):
00089         time_step = 0.02
00090         def eq_gen():
00091             jep = self.robot.get_jep(arm)
00092             cep, rot_cep = self.robot.arms.FK_all(arm, jep)
00093             q = self.robot.get_joint_angles(arm)
00094             ee, ee_rot = self.robot.arms.FK_all(arm, q)
00095             err = goal_pos - ee
00096             err_mag = np.linalg.norm(err)
00097             step_size = min(0.001, err_mag)
00098             cep_new = cep + err/err_mag * step_size
00099             jep_new = self.robot.IK(arm, cep_new, rot_cep, jep)
00100 
00101             stop = ''
00102             if len(jep_new) != 7 or jep_new is None:
00103                 stop = "IK failure"
00104                 return stop, None
00105             if err_mag < 0.01:
00106                 stop = 'Reached'
00107                 return stop, None
00108 
00109             return stop, (jep_new, time_step*1.2)
00110         
00111         return self.epc_motion(eq_gen, time_step, arm, [],
00112                                control_function=self.robot.set_jep)
00113 
00114     def rot_mat_interpolate(self, start_rot, end_rot, num):
00115         start_quat = tr.matrix_to_quaternion(start_rot)
00116         end_quat = tr.matrix_to_quaternion(end_rot)
00117         mat_list = []
00118         for fraction in np.linspace(0, 1, num):
00119             cur_quat = tf_trans.quaternion_slerp(start_quat, end_quat, fraction)
00120             mat_list.append(tr.quaternion_to_matrix(cur_quat))
00121         return mat_list
00122 
00123 # mag = np.sqrt(np.dot(quat_a, quat_a) * np.dot(quat_b, quat_b))
00124 # assert mag != 0
00125 # return np.arccos(np.dot(quat_a, quat_b) / mag)
00126         
00127 
00128     def interpolated_linear_trajectory(self, arm, start_pos, start_rot, end_pos, end_rot, 
00129                                                   offset_pos, offset_rot):
00130         time_step = 0.02
00131         velocity = 0.01
00132         dist = np.linalg.norm(np.array(end_pos) - np.array(start_pos))
00133         time_req = dist / velocity
00134         steps_req = time_req / time_step
00135         self.pos_waypoints = np.dstack([np.linspace(start_pos[0], end_pos[0], steps_req), 
00136                                         np.linspace(start_pos[1], end_pos[1], steps_req), 
00137                                         np.linspace(start_pos[2], end_pos[2], steps_req)])[0]
00138         self.rot_waypoints = self.rot_mat_interpolate(start_rot, end_rot, steps_req)
00139         self.prev_err_mag = 0
00140         self.eq_gen_step = 0
00141 
00142         def eq_gen():
00143             jep = self.robot.get_jep(arm)
00144             cep, rot_cep = self.robot.arms.FK_all(arm, jep)
00145             q = self.robot.get_joint_angles(arm)
00146             ee_pos, ee_rot = self.robot.arms.FK_all(arm, q)
00147             cur_goal_pos = self.pos_waypoints[self.eq_gen_step]
00148             cur_goal_rot = self.rot_waypoints[self.eq_gen_step]
00149 
00150             # calculate position error
00151             err_pos = cur_goal_pos - ee_pos
00152             
00153             # calculate rotation error
00154             err_rot =  ee_rot.T * cur_goal_rot
00155             angle, direc, point = tf_trans.rotation_from_matrix(err_rot)
00156             cep_rot_new = tf_trans.rotation_matrix(x * angle, direc, point)
00157             ang_step_size = min(0.01, err_angle)
00158 
00159             cep_new = cep + err/err_pos_mag * pos_step_size
00160             jep_new = self.robot.IK(arm, cep_new, rot_cep, jep)
00161 
00162             self.eq_gen_step += 1
00163 
00164             stop = ''
00165             if len(jep_new) != 7 or jep_new is None:
00166                 stop = "IK failure"
00167                 return stop, None
00168             if err_mag < 0.01:
00169                 stop = 'Reached'
00170                 return stop, None
00171 
00172             return stop, (jep_new, time_step*1.2)
00173         
00174         return self.epc_motion(eq_gen, time_step, arm, [],
00175                                control_function=self.robot.set_jep)
00176 
00177 
00178     def go_jep(self, arm , goal_jep, speed=math.radians(20),
00179                rapid_call_func = None):
00180         start_jep = self.robot.get_jep(arm)
00181         diff_jep = np.array(goal_jep) - np.array(start_jep)
00182         time_step = 0.02
00183         max_ch = np.max(np.abs(diff_jep))
00184         total_time = max_ch / speed
00185         n_steps = max(np.round(total_time / time_step + 0.5), 1)
00186         jep_step = diff_jep / n_steps
00187         step_num = 0
00188         jep = copy.copy(start_jep)
00189 
00190         def eq_gen(l):
00191             jep = l[0]
00192             step_num = l[1]
00193             if step_num < n_steps:
00194                 q = list(np.array(jep) + jep_step)
00195                 stop = ''
00196             else:
00197                 q = None
00198                 stop = 'Reached'
00199             step_num += 1
00200             l[0] = q
00201             l[1] = step_num
00202             return stop, (q, time_step*1.2)
00203         
00204         return self.epc_motion(eq_gen, time_step, arm, [[jep, step_num]],
00205                                control_function=self.robot.set_jep,
00206                                rapid_call_func = rapid_call_func)
00207 
00208 
00209 
00210 if __name__ == '__main__':
00211     import pr2_arms.pr2_arms as pa
00212     rospy.init_node('epc_pr2', anonymous = True)
00213     rospy.logout('epc_pr2: ready')
00214 
00215     pr2_arms = pa.PR2Arms()
00216     epc = EPC(pr2_arms)
00217 
00218     r_arm, l_arm = 0, 1
00219     arm = r_arm
00220 
00221     if False:
00222         q = epc.robot.get_joint_angles(arm)
00223         epc.robot.set_jep(arm, q)
00224         ea = [0, 0, 0, 0, 0, 0, 0]
00225         raw_input('Hit ENTER to go_jep')
00226         epc.go_jep(arm, ea, math.radians(30.))
00227 
00228     if False:
00229         raw_input('Hit ENTER to go_jep')
00230         q = epc.robot.get_joint_angles(arm)
00231         epc.robot.set_jep(arm, pr2_arms.wrap_angles(q))
00232         p, r = epc.robot.arms.FK_all(arm, q)
00233         q = epc.robot.IK(arm, p, r)
00234         epc.go_jep(arm, q, math.radians(30.))
00235         
00236         raw_input('Hit ENTER to go_pose')
00237         goal_rot = r
00238         goal_pos = p + np.matrix([0.2, 0., 0.]).T
00239 #goal_pos = np.mat([0.889, -0.1348, -0.0868]).T
00240         res = epc.go_pose(arm, goal_pos, goal_rot)
00241         print 'go_pose result:', res[0]
00242         print 'goal_pos:', goal_pos.A1
00243 
00244         q = epc.robot.get_joint_angles(arm)
00245         p, r = epc.robot.arms.FK_all(arm, q)
00246         print 'current position:', p.A1
00247 
00248     if True:
00249         start_pos = [0, 0, 0]
00250         end_pos = [0.05, 0.02, -0.03]
00251         start_rot, end_rot = tr.rotX(1), tr.rotY(1)
00252         epc.interpolated_linear_trajectory(arm, start_pos, start_rot, end_pos, end_rot)
00253 
00254 
00255 
00256 
00257