ode_sim_arms.py
Go to the documentation of this file.
00001 
00002 import numpy as np, math
00003 from threading import RLock
00004 import sys, copy
00005 import matplotlib.pyplot as pp
00006 
00007 import roslib; roslib.load_manifest('hrl_software_simulation_darpa_m3')
00008 
00009 import rospy
00010 import PyKDL as kdl
00011 
00012 from equilibrium_point_control.hrl_arm import HRLArm, HRLArmKinematics
00013 
00014 import hrl_lib.geometry as hg
00015 import hrl_lib.kdl_utils as ku
00016 import hrl_lib.viz as hv
00017 
00018 from hrl_msgs.msg import FloatArrayBare
00019 from visualization_msgs.msg import Marker
00020 from hrl_haptic_manipulation_in_clutter_msgs.msg import MechanicalImpedanceParams
00021 
00022 
00023 class ODESimArm(HRLArm):
00024     def __init__(self):
00025         rospy.loginfo("Loading ODESimArm")
00026         kinematics = RobotSimulatorKDL() # KDL chain.
00027         HRLArm.__init__(self, kinematics)
00028 
00029         # at some point of time, Advait might move these to hrl_arm.py
00030         # and make things consistent with Cody. For now (Feb 7, 2012)
00031         # sticking to this file.
00032         self.kp = None
00033         self.kd = None
00034 
00035         self.jep_pub = rospy.Publisher('/sim_arm/command/jep', FloatArrayBare)
00036         self.cep_marker_pub = rospy.Publisher('/sim_arm/viz/cep', Marker)
00037 
00038         self.impedance_pub = rospy.Publisher('/sim_arm/command/joint_impedance',
00039                                              MechanicalImpedanceParams)
00040 
00041         rospy.Subscriber('/sim_arm/joint_angles', FloatArrayBare,
00042                          self.joint_states_cb)
00043         rospy.Subscriber('/sim_arm/joint_angle_rates', FloatArrayBare,
00044                          self.joint_rates_cb)
00045         rospy.Subscriber('/sim_arm/jep', FloatArrayBare, self.jep_cb)
00046         rospy.Subscriber('/sim_arm/joint_impedance', MechanicalImpedanceParams,
00047                          self.impedance_params_cb)
00048 
00049         rospy.sleep(1.)
00050         rospy.loginfo("Finished loading SimpleArmManger")
00051 
00052     def jep_cb(self, msg):
00053         with self.lock:
00054             self.ep = copy.copy(msg.data)
00055 
00056     def joint_states_cb(self, data):
00057         with self.lock:
00058             self.q = copy.copy(data.data)
00059 
00060     def joint_rates_cb(self, data):
00061         with self.lock:
00062             self.qdot = copy.copy(data.data)
00063 
00064     def impedance_params_cb(self, data):
00065         with self.lock:
00066             self.kp = copy.copy(data.k_p.data)
00067             self.kd = copy.copy(data.k_d.data)
00068 
00069     def get_joint_velocities(self):
00070         with self.lock:
00071             qdot = copy.copy(self.qdot)
00072         return qdot
00073 
00074     def set_ep(self, jep, duration=0.15):
00075         f = FloatArrayBare(jep)
00076         self.jep_pub.publish(f)
00077         self.publish_rviz_markers()
00078 
00079     def publish_rviz_markers(self):
00080         # publish the CEP marker.
00081         jep = self.get_ep()
00082         cep, r = self.kinematics.FK(jep)
00083         o = np.matrix([0.,0.,0.,1.]).T
00084         cep_marker = hv.single_marker(cep, o, 'sphere',
00085                         '/torso_lift_link', color=(0., 0., 1., 1.),
00086                         scale = (0.02, 0.02, 0.02), duration=0.)
00087 
00088         cep_marker.header.stamp = rospy.Time.now()
00089         self.cep_marker_pub.publish(cep_marker)
00090 
00091     # returns kp, kd
00092     # np arrays of stiffness and damping of the virtual springs.
00093     def get_joint_impedance(self):
00094         with self.lock:
00095             return np.array(self.kp), np.array(self.kd)
00096 
00097     # kp, kd - list or np array of stiffness and damping.
00098     def set_joint_impedance(self, kp, kd):
00099         im = MechanicalImpedanceParams()
00100         im.k_p.data = kp
00101         im.k_d.data = kd
00102         self.impedance_pub.publish(im)
00103 
00104 
00105 ##
00106 # KDL for kinematics etc.
00107 class RobotSimulatorKDL(HRLArmKinematics):
00108     def __init__(self):
00109         HRLArmKinematics.__init__(self, n_jts = 3)
00110         self.right_chain = self.create_right_chain()
00111         fk, ik_v, ik_p, jac = self.create_solvers(self.right_chain)
00112 
00113         self.right_fk = fk
00114         self.right_ik_v = ik_v
00115         self.right_ik_p = ik_p
00116         self.right_jac = jac
00117         self.right_tooltip = np.matrix([0.,0.,0.]).T
00118 
00119         # marc joint limits.
00120         self.min_jtlim_arr = np.radians(np.array([-150., -63, 0.]))
00121         self.max_jtlim_arr = np.radians(np.array([150., 162, 159.]))
00122 
00123         # advait joint limits.
00124 #        self.min_jtlim_arr = np.radians(np.array([-75., -63+65, 0+2.]))
00125 #        self.max_jtlim_arr = np.radians(np.array([75., 162, 159-1.]))
00126 
00127     def create_right_chain(self):
00128         height = 0.0
00129         linkage_offset_from_ground = np.matrix([0., 0., height]).T
00130         self.linkage_offset_from_ground = linkage_offset_from_ground
00131         b_jt_q_ind = [True, True, True]
00132         self.n_jts = len(b_jt_q_ind)
00133 
00134         torso_half_width = 0.196
00135         upper_arm_length = 0.334
00136         forearm_length = 0.288 #+ 0.115
00137 
00138         ee_location = np.matrix([0., -torso_half_width-upper_arm_length-forearm_length, height]).T
00139         b_jt_anchor = [[0, 0, height], [0, -torso_half_width, height],
00140                        [0., -torso_half_width-upper_arm_length, height]]
00141         b_jt_axis = [[0,0,1],[0,0,1], [0,0,1]]
00142 
00143         ch = kdl.Chain()
00144         prev_vec = np.copy(linkage_offset_from_ground.A1)
00145         n = len(b_jt_q_ind)
00146 
00147         for i in xrange(n-1):
00148             if b_jt_axis[i][0] == 1 and b_jt_axis[i][1] == 0 and b_jt_axis[i][2] == 0:
00149                 kdl_jt = kdl.Joint(kdl.Joint.RotX)
00150             elif b_jt_axis[i][0] == 0 and b_jt_axis[i][1] == 1 and b_jt_axis[i][2] == 0:
00151                 kdl_jt = kdl.Joint(kdl.Joint.RotY)
00152             elif b_jt_axis[i][0] == 0 and b_jt_axis[i][1] == 0 and b_jt_axis[i][2] == 1:
00153                 kdl_jt = kdl.Joint(kdl.Joint.RotZ)
00154             else:
00155                 print "can't do off-axis joints yet!!!"
00156 
00157             np_vec = np.array(b_jt_anchor[i+1])
00158             diff_vec = np_vec-prev_vec
00159             prev_vec = np_vec
00160             kdl_vec = kdl.Vector(diff_vec[0], diff_vec[1], diff_vec[2])            
00161             ch.addSegment(kdl.Segment(kdl_jt, kdl.Frame(kdl_vec)))
00162 
00163         np_vec = np.copy(ee_location.A1)
00164         diff_vec = np_vec-prev_vec
00165 
00166         if b_jt_axis[n-1][0] == 1 and b_jt_axis[n-1][1] == 0 and b_jt_axis[n-1][2] == 0:
00167             kdl_jt = kdl.Joint(kdl.Joint.RotX)
00168         elif b_jt_axis[n-1][0] == 0 and b_jt_axis[n-1][1] == 1 and b_jt_axis[n-1][2] == 0:
00169             kdl_jt = kdl.Joint(kdl.Joint.RotY)
00170         elif b_jt_axis[n-1][0] == 0 and b_jt_axis[n-1][1] == 0 and b_jt_axis[n-1][2] == 1:
00171             kdl_jt = kdl.Joint(kdl.Joint.RotZ)
00172         else:
00173             print "can't do off-axis joints yet!!!"
00174 
00175         kdl_vec = kdl.Vector(diff_vec[0], diff_vec[1], diff_vec[2])            
00176         ch.addSegment(kdl.Segment(kdl_jt, kdl.Frame(kdl_vec)))
00177         
00178         return ch
00179 
00180     def create_solvers(self, ch):
00181          fk = kdl.ChainFkSolverPos_recursive(ch)
00182          ik_v = kdl.ChainIkSolverVel_pinv(ch)
00183          ik_p = kdl.ChainIkSolverPos_NR(ch, fk, ik_v)
00184          jac = kdl.ChainJntToJacSolver(ch)
00185          return fk, ik_v, ik_p, jac
00186 
00187     def FK_kdl(self, q, link_number):
00188         fk = self.right_fk
00189         endeffec_frame = kdl.Frame()
00190         kinematics_status = fk.JntToCart(q, endeffec_frame,
00191                                          link_number)
00192         if kinematics_status >= 0:
00193             return endeffec_frame
00194         else:
00195             rospy.loginfo('Could not compute forward kinematics.')
00196             return None
00197 
00198     ## returns point in torso lift link.
00199     def FK_vanilla(self, q, link_number=None):
00200         if link_number == None:
00201             link_number = self.n_jts
00202         link_number = min(link_number, self.n_jts)
00203 
00204         q = self.list_to_kdl(q)
00205         frame = self.FK_kdl(q, link_number)
00206         pos = frame.p
00207         pos = ku.kdl_vec_to_np(pos)
00208         pos = pos + self.linkage_offset_from_ground
00209         m = frame.M
00210         rot = ku.kdl_rot_to_np(m)
00211 
00212         return pos, rot
00213 
00214     def kdl_to_list(self, q):
00215         if q == None:
00216             return None
00217         n = self.n_jts
00218         q_list = [0] * n
00219         for i in xrange(n):
00220             q_list[i] = q[i]
00221         return q_list
00222 
00223     def list_to_kdl(self, q):
00224         if q == None:
00225             return None
00226         n = len(q)
00227         q_kdl = kdl.JntArray(n)
00228         for i in xrange(n):
00229             q_kdl[i] = q[i]
00230         return q_kdl
00231 
00232     ## compute Jacobian at point pos. 
00233     # p is in the ground coord frame.
00234     def Jacobian(self, q, pos=None):
00235         if pos == None:
00236             pos = self.FK(q)[0]
00237 
00238         v_list = []
00239         w_list = []
00240 
00241         #ee, r = self.FK(q)
00242         #pos = ee
00243 
00244         for i in xrange(self.n_jts):
00245             p, rot = self.FK_vanilla(q, i)
00246             r = pos - p
00247             z_idx = self.right_chain.getSegment(i).getJoint().getType() - 1
00248             z = rot[:, z_idx]
00249             v_list.append(np.matrix(np.cross(z.A1, r.A1)).T)
00250             w_list.append(z)
00251 
00252         J = np.row_stack((np.column_stack(v_list), np.column_stack(w_list)))
00253         return J
00254 
00255     def clamp_to_joint_limits(self, q):
00256         return np.clip(q, self.min_jtlim_arr, self.max_jtlim_arr)
00257 
00258     def within_joint_limits(self, q, active_joints = [0,1,2],
00259                             delta_list=[0.,0.,0.]):
00260         min_arr = self.min_jtlim_arr[active_joints]
00261         max_arr = self.max_jtlim_arr[active_joints]
00262 
00263         q_arr = np.array(q)[active_joints]
00264         d_arr = np.array(delta_list)[active_joints]
00265         return np.all((q_arr <= max_arr+d_arr, q_arr >= min_arr+d_arr))
00266 
00267     def get_joint_limits(self):
00268         return self.min_jtlim_arr, self.max_jtlim_arr
00269 
00270     # plot the arm using matplotlib.
00271     # flip_xy - x will point up and +ve y will be to the left. This is
00272     # to match how we look at the arm in rviz.
00273     def plot_arm(self, q, color, alpha, flip_xy):
00274         pts = [[0.,0.,0.]]
00275         for i in range(len(q)):
00276             p,_ = self.FK(q, i+1)
00277             pts.append(p.A1.tolist())
00278 
00279         pts_2d = np.array(pts)[:,0:2]
00280         direc_list = (pts_2d[1:] - pts_2d[:-1]).tolist()
00281 
00282         for i, d in enumerate(direc_list):
00283             d_vec = np.matrix(d).T
00284             d_vec = d_vec / np.linalg.norm(d_vec)
00285             w = np.cross(d_vec.A1, np.array([0., 0., 1.])) * 0.03/2
00286             x1 = pts_2d[i,0]
00287             y1 = pts_2d[i,1]
00288             x2 = pts_2d[i+1,0]
00289             y2 = pts_2d[i+1,1]
00290 
00291             x_data = np.array([x1+w[0], x1-w[0], x2-w[0], x2+w[0], x1+w[0]])
00292             y_data = np.array([y1+w[1], y1-w[1], y2-w[1], y2+w[1], y1+w[1]])
00293 
00294             if flip_xy:
00295                 tmp = y_data
00296                 y_data = x_data
00297                 x_data = -tmp
00298 
00299             pp.plot(x_data, y_data, '-', alpha=alpha, color=color,
00300                     linewidth=2)
00301 
00302 
00303 if __name__ == '__main__':
00304     rospy.init_node('ode_arms_test')
00305     ode_arm = ODESimArm()
00306 
00307     #test_jep = np.radians([30., 20., 45.])
00308     test_jep = np.radians([0., 0., 0.])
00309     p, _ = ode_arm.kinematics.FK(test_jep, 3)
00310     print 'p:', p.A1
00311 
00312 #    while not rospy.is_shutdown():
00313 #    ode_arm.set_jep(arm, test_jep)
00314 #        rospy.sleep(0.1)
00315 
00316 
00317 


hrl_software_simulation_darpa_m3
Author(s): Marc Killpack and Advait Jain. Advisor: Prof. Charlie Kemp. Healthcare Robotics Lab, Georgia Tech
autogenerated on Wed Nov 27 2013 11:35:07