00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025 import numpy as np, math
00026 import copy
00027 from threading import RLock
00028
00029 import roslib; roslib.load_manifest('hrl_haptic_mpc')
00030
00031 try:
00032 import hrl_lib.geometry as hg
00033 except ImportError, e:
00034 print '<hrl_arm.py> WARNING:', e
00035
00036
00037 class HRLArm():
00038 def __init__(self, kinematics):
00039
00040 self.kinematics = kinematics
00041 self.ep = None
00042 self.kp = None
00043 self.kd = None
00044 self.q = None
00045 self.qdot = None
00046 self.joint_names_list = None
00047 self.lock = RLock()
00048
00049 def get_joint_velocities(self):
00050 with self.lock:
00051 return copy.copy(self.qdot)
00052
00053 def get_joint_angles(self):
00054 with self.lock:
00055 return copy.copy(self.q)
00056
00057 def set_ep(self, *args):
00058 raise RuntimeError('Unimplemented Function')
00059
00060
00061 def publish_rviz_markers(self):
00062 raise RuntimeError('Unimplemented Function')
00063
00064 def get_ep(self):
00065 with self.lock:
00066 return copy.copy(self.ep)
00067
00068
00069
00070 def get_joint_impedance(self):
00071 with self.lock:
00072 return copy.copy(self.kp), copy.copy(self.kd)
00073
00074 def get_joint_names(self):
00075 with self.lock:
00076 return copy.copy(self.joint_names_list)
00077
00078
00079 def freeze(self):
00080 self.set_ep(self.ep)
00081
00082 def get_end_effector_pose(self):
00083 return self.kinematics.FK(self.get_joint_angles())
00084
00085
00086 class HRLArmKinematics():
00087 def __init__(self, n_jts):
00088 self.tooltip_pos = np.matrix([0.,0.,0.]).T
00089 self.tooltip_rot = np.matrix(np.eye(3))
00090 self.n_jts = n_jts
00091
00092
00093 def FK_vanilla(self, q, link_number=None):
00094 raise RuntimeError('Unimplemented Function')
00095
00096
00097
00098
00099 def FK(self, q, link_number=None):
00100 if link_number == None:
00101 link_number = self.n_jts
00102 if link_number > self.n_jts:
00103 raise RuntimeError('Link Number is greater than n_jts: %d'%link_number)
00104 pos, rot = self.FK_vanilla(q, link_number)
00105
00106 if link_number == self.n_jts:
00107 tooltip_baseframe = rot * self.tooltip_pos
00108 pos += tooltip_baseframe
00109 rot = rot * self.tooltip_rot
00110 return pos, rot
00111
00112
00113
00114
00115
00116
00117
00118 def IK(self, pos, rot, q_guess=None):
00119 last_link_pos = pos - rot * self.tooltip_rot.T * self.tooltip_pos
00120 last_link_rot = rot * self.tooltip_rot.T
00121 return self.IK_vanilla(last_link_pos, last_link_rot, q_guess)
00122
00123
00124 def IK_vanilla(self, p, rot, q_guess=None):
00125 raise RuntimeError('Unimplemented Function')
00126
00127
00128
00129 def IK(self, p, rot, q_guess=None):
00130
00131 pass
00132
00133
00134 def jacobian(self, q, pos=None):
00135 raise RuntimeError('Unimplemented Function')
00136
00137
00138 def get_joint_limits(self):
00139 raise RuntimeError('Unimplemented Function')
00140
00141
00142 def set_tooltip(self, p, rot=np.matrix(np.eye(3))):
00143 self.tooltip_pos = copy.copy(p)
00144 self.tooltip_rot = copy.copy(rot)
00145
00146
00147
00148
00149
00150
00151 def arm_config_to_points_list(self, q):
00152 return [self.FK(q, i)[0].A1[0:2] for i in range(len(q)+1)]
00153
00154
00155
00156 def distance_from_ee_along_arm(self, q, pt):
00157 p_l = self.arm_config_to_points_list(q)
00158 ee = self.FK(q)[0]
00159 d_ee = hg.distance_along_curve(ee, p_l)
00160 d_pt = hg.distance_along_curve(pt, p_l)
00161 assert(d_ee >= d_pt)
00162 return d_ee - d_pt
00163
00164
00165 def distance_from_arm(self, q, pt):
00166 p_l = self.arm_config_to_points_list(q)
00167 return hg.distance_from_curve(pt, p_l)
00168
00169
00170
00171
00172
00173
00174
00175
00176 def is_contact_at_joint(self, pt, q, dist_threshold):
00177 pts_list = [self.FK(q, i)[0].A1 for i in range(len(q)+1)]
00178 proj_pt = hg.project_point_on_curve(pt, pts_list)
00179
00180
00181 for jt in pts_list[:-1]:
00182 dist = np.linalg.norm(np.matrix(jt).T-proj_pt)
00183 if dist <= dist_threshold:
00184 return True
00185 return False