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()
00027 HRLArm.__init__(self, kinematics)
00028
00029
00030
00031
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
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
00092
00093 def get_joint_impedance(self):
00094 with self.lock:
00095 return np.array(self.kp), np.array(self.kd)
00096
00097
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
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
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
00124
00125
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
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
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
00233
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
00242
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
00271
00272
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
00308 test_jep = np.radians([0., 0., 0.])
00309 p, _ = ode_arm.kinematics.FK(test_jep, 3)
00310 print 'p:', p.A1
00311
00312
00313
00314
00315
00316
00317