def get_kdl_chain_lwr(self):
from PyKDL import Frame, Rotation, Vector, Joint, Chain, Segment
from math import pi
# arm segments that behave exactly like the DLR system. (Last pose in the middle of the potato)
#Some parts of the DLR system take into account the TCP_T_EE transformation, for example to the Flange
arm_segments = [
Segment(Joint(Joint.None),
Frame(Rotation.Quaternion(0.428, 0.261, -0.435, 0.748), Vector(0.222, -0.339, 0.949))),
Segment(Joint(Joint.None),
Frame(Rotation.Identity(), Vector(0.0, 0.0, 0.11))),
Segment(Joint(Joint.RotZ),
Frame(Rotation.RotX(-pi/2), Vector(0.0, 0.0, 0.20))),
Segment(Joint(Joint.RotZ, -1),
Frame(Rotation.RotX(pi/2), Vector(0.0, -0.20, 0.0))),
Segment(Joint(Joint.RotZ),
Frame(Rotation.RotX(pi/2), Vector(0, 0, .20))),
Segment(Joint(Joint.RotZ, -1),
Frame(Rotation.RotX(-pi/2), Vector(0, 0.2, 0))),
Segment(Joint(Joint.RotZ),
Frame(Rotation.RotX(-pi/2), Vector(0, 0, 0.19))),
Segment(Joint(Joint.RotZ, -1),
Frame(Rotation.RotX(pi/2), Vector(0, 0, 0.0))), #-0.078
Segment(Joint(Joint.RotZ),
Frame(Rotation.Identity(), Vector(0, 0, 0.0))),
]
chain = Chain()
for segment in arm_segments:
chain.addSegment(segment)
return(chain)
def build_chain(self):
self.chain = Chain()
for e in self._config:
v = Vector(e["xyzrpy"][0], e["xyzrpy"][1], e["xyzrpy"][2])
r = Rotation.RPY(e["xyzrpy"][3], e["xyzrpy"][4], e["xyzrpy"][5])
j = Joint(e["name"], Joint.None)
f = Frame(r, v)
if e["type"] == "rotx":
axis = Vector(1.0, 0.0, 0.0)
j = Joint(e["name"], f.p, f.M * axis, Joint.RotAxis)
elif e["type"] == "roty":
axis = Vector(0.0, 1.0, 0.0)
j = Joint(e["name"], f.p, f.M * axis, Joint.RotAxis)
elif e["type"] == "rotz":
axis = Vector(0.0, 0.0, 1.0)
j = Joint(e["name"], f.p, f.M * axis, Joint.RotAxis)
elif e["type"] == "transx":
axis = Vector(1.0, 0.0, 0.0)
j = Joint(e["name"], f.p, f.M * axis, Joint.TransAxis)
elif e["type"] == "transy":
axis = Vector(0.0, 1.0, 0.0)
j = Joint(e["name"], f.p, f.M * axis, Joint.TransAxis)
elif e["type"] == "transz":
axis = Vector(0.0, 0.0, 1.0)
j = Joint(e["name"], f.p, f.M * axis, Joint.TransAxis)
self.chain.addSegment(
Segment(e["name"] + "seg", j, f, RigidBodyInertia()))
self.fksolverpos = ChainFkSolverPos_recursive(self.chain)
def compute_effector_position(self, msg):
positions = JntArray(3)
chain = Chain()
kdl_frame = Frame()
d = 0
theta = 0
order = [1, 2, 0]
for i in order:
joint = self.params[i]
a = joint["a"]
d = joint["d"]
alpha = joint["alpha"]
theta = joint["theta"]
frame = kdl_frame.DH(a, alpha, d, theta)
chain.addSegment(Segment(Joint(Joint.RotZ), frame))
positions[i] = msg.position[i]
fk_solver = ChainFkSolverPos_recursive(chain)
result = Frame()
fk_solver.JntToCart(positions, result)
kdl_pose = PoseStamped()
kdl_pose.header.frame_id = 'base_link'
kdl_pose.header.stamp = rospy.Time.now()
kdl_pose.pose.position.x = result.p[0]
kdl_pose.pose.position.y = result.p[1]
kdl_pose.pose.position.z = result.p[2]
quat = result.M.GetQuaternion()
kdl_pose.pose.orientation.x = quat[0]
kdl_pose.pose.orientation.y = quat[1]
kdl_pose.pose.orientation.z = quat[2]
kdl_pose.pose.orientation.w = quat[3]
return kdl_pose
def __init__(self,parent,handedness):
'''parent is the finger parent'''
#kinematics
self.parent=parent
self.config_hand=self.parent.config_hand
ch=self.config_hand
#from hands_kin import hands_kin, fingers_lim, fingers_coupling, motor_locked_joints, sensor_locked_joints, sensor_finger_coupling
from PyKDL import Chain,ChainFkSolverPos_recursive,JntArray,Frame
from PyKDL import ChainFkSolverVel_recursive,FrameVel
from PyKDL import ChainJntToJacSolver, Jacobian, JntArrayVel
from PyKDL import ChainIkSolverVel_wdls, Twist
self.segments=ch.hands_kin[handedness][self.parent.num_finger]
self.limits=ch.fingers_lim[self.parent.num_finger]
self.coupling_matrix=ch.fingers_coupling[self.parent.num_finger]
self.sensor_finger_coupling=ch.sensor_finger_coupling
self.motor_locked_joints=ch.motor_locked_joints[self.parent.num_finger]
self.sensor_locked_joints=ch.sensor_locked_joints[self.parent.num_finger]
print "Motor_locked", self.motor_locked_joints
print "Sensor_locked", self.sensor_locked_joints
#chain motor
self.motor_chain=Chain()
for segment in self.segments:
self.motor_chain.addSegment(segment)
self.motor_chain.setCoupling(self.motor_locked_joints,self.coupling_matrix.tolist())
#chain sensor
self.sensor_chain=Chain(self.motor_chain)
self.sensor_chain.setCoupling(self.sensor_locked_joints,self.sensor_finger_coupling.tolist())
#pos fk
self.pfk_solver=ChainFkSolverPos_recursive(self.motor_chain)
self.cur_jpos_kdl=JntArray(self.motor_chain.getNrOfJoints())
self.cur_frame_kdl=Frame()
#vel fk
self.vfk_solver=ChainFkSolverVel_recursive(self.motor_chain)
self.cur_jvel_kdl=JntArrayVel(self.motor_chain.getNrOfJoints()) #TODO for the thumb we don't have joint velocity yet, it has to be calculated.
self.cur_framevel_kdl=FrameVel()
#vel ifk
from numpy import finfo, double
self.smallest=finfo(double).eps
self.vik_solver=ChainIkSolverVel_wdls(self.motor_chain,self.smallest)
#TODO: make set weights methods
self.qdot_out_kdl=JntArray(self.motor_chain.getNrOfUnlockedJoints())
self.tw=Twist()
#jac solver
self.jac_solver_sensors=ChainJntToJacSolver(self.sensor_chain)
self.jac_solver_sensors
self.jac_sensors=Jacobian(self.sensor_chain.getNrOfIndJoints())
self.forces=array([0.0]*6)
#finger calibration values
#angle offsets in radians of the finger joints
self.base_angle_offset=0.0
self.proximal_angle_offset=0.0
self.distal_angle_offset=0.0
self.distal2_angle_offset=0.0
#weight matrices for the statics inverse
self.static_weight_left=diag([1.0, 1.0, 1.0, 0.0, 0.0, 0.0])
self.static_weight_right=identity(self.sensor_chain.getNrOfIndJoints())
self.static_weight_right=identity(self.sensor_chain.getNrOfUnlockedJoints())
class Kinematics(object):
'''This deals with the finger kinematics, forces, speeds, and positions in cartesian are here calculated, also the inverses of this'''
def __init__(self,parent,handedness):
'''parent is the finger parent'''
#kinematics
self.parent=parent
self.config_hand=self.parent.config_hand
ch=self.config_hand
#from hands_kin import hands_kin, fingers_lim, fingers_coupling, motor_locked_joints, sensor_locked_joints, sensor_finger_coupling
from PyKDL import Chain,ChainFkSolverPos_recursive,JntArray,Frame
from PyKDL import ChainFkSolverVel_recursive,FrameVel
from PyKDL import ChainJntToJacSolver, Jacobian, JntArrayVel
from PyKDL import ChainIkSolverVel_wdls, Twist
self.segments=ch.hands_kin[handedness][self.parent.num_finger]
self.limits=ch.fingers_lim[self.parent.num_finger]
self.coupling_matrix=ch.fingers_coupling[self.parent.num_finger]
self.sensor_finger_coupling=ch.sensor_finger_coupling
self.motor_locked_joints=ch.motor_locked_joints[self.parent.num_finger]
self.sensor_locked_joints=ch.sensor_locked_joints[self.parent.num_finger]
print "Motor_locked", self.motor_locked_joints
print "Sensor_locked", self.sensor_locked_joints
#chain motor
self.motor_chain=Chain()
for segment in self.segments:
self.motor_chain.addSegment(segment)
self.motor_chain.setCoupling(self.motor_locked_joints,self.coupling_matrix.tolist())
#chain sensor
self.sensor_chain=Chain(self.motor_chain)
self.sensor_chain.setCoupling(self.sensor_locked_joints,self.sensor_finger_coupling.tolist())
#pos fk
self.pfk_solver=ChainFkSolverPos_recursive(self.motor_chain)
self.cur_jpos_kdl=JntArray(self.motor_chain.getNrOfJoints())
self.cur_frame_kdl=Frame()
#vel fk
self.vfk_solver=ChainFkSolverVel_recursive(self.motor_chain)
self.cur_jvel_kdl=JntArrayVel(self.motor_chain.getNrOfJoints()) #TODO for the thumb we don't have joint velocity yet, it has to be calculated.
self.cur_framevel_kdl=FrameVel()
#vel ifk
from numpy import finfo, double
self.smallest=finfo(double).eps
self.vik_solver=ChainIkSolverVel_wdls(self.motor_chain,self.smallest)
#TODO: make set weights methods
self.qdot_out_kdl=JntArray(self.motor_chain.getNrOfUnlockedJoints())
self.tw=Twist()
#jac solver
self.jac_solver_sensors=ChainJntToJacSolver(self.sensor_chain)
self.jac_solver_sensors
self.jac_sensors=Jacobian(self.sensor_chain.getNrOfIndJoints())
self.forces=array([0.0]*6)
#finger calibration values
#angle offsets in radians of the finger joints
self.base_angle_offset=0.0
self.proximal_angle_offset=0.0
self.distal_angle_offset=0.0
self.distal2_angle_offset=0.0
#weight matrices for the statics inverse
self.static_weight_left=diag([1.0, 1.0, 1.0, 0.0, 0.0, 0.0])
self.static_weight_right=identity(self.sensor_chain.getNrOfIndJoints())
self.static_weight_right=identity(self.sensor_chain.getNrOfUnlockedJoints())
def get_cart_pos(self,segment_num=-1):
if self.pfk_solver.JntToCart(self.cur_jpos_kdl,self.cur_frame_kdl,segment_num)<0:
print "Error in cart pos calculation"
def get_cart_vel(self):
#TODO: not tested yet
# for i,(angle,speed) in enumerate(zip(self.parent.cur_joint_pos,self.parent.cur_joint_vel)):
# self.cur_jpos_kdl[i]=angle
# self.cur_jvel_kdl.q[i]=angle
# self.cur_jvel_kdl.qdot[i]=speed
if self.vfk_solver.JntToCart(self.cur_jvel_kdl,self.cur_framevel_kdl)<0:
print "Error in cart vel calculation"
def calc_vik(self,tw_list):
if len(tw_list)==6:
for i in xrange(6):
self.tw[i]=tw_list[i]
if self.vik_solver.CartToJnt(self.cur_jpos_kdl,self.tw,self.qdot_out_kdl)<0:
print "Error in velocity inverse kinematics"
return(-1)
else:
print "Error wrong number of values"
return(-1)
return(0)
def update_cur_jvel_kdl(self):
j=0
temp=concatenate((self.parent.cur_joint_vel,array([self.parent.cur_joint_vel[-1]])))
for i,locked in enumerate(self.sensor_locked_joints):
self.cur_jvel_kdl.q[i]=self.cur_jpos_kdl[i]
if locked:
self.cur_jvel_kdl.qdot[i]=temp[j]
j+=1
else:
self.cur_jvel_kdl.qdot[i]=0.0
def update_cur_jpos_kdl(self):
j=0
temp=concatenate((self.parent.cur_joint_pos,array([self.parent.cur_joint_pos[-1]])))
for i,locked in enumerate(self.sensor_locked_joints):
if locked:
self.cur_jpos_kdl[i]=temp[j]
j+=1
else:
self.cur_jpos_kdl[i]=0.0
def get_forces(self):
self.update_cur_jpos_kdl()
#print "Test1"
#print "Cur jpos kdl", [self.cur_jpos_kdl[i] for i in xrange(8)]
if self.jac_solver_sensors.JntToJac(self.cur_jpos_kdl,self.jac_sensors,True)<0:
print "Error in sensor jacobian calculation"
return
#print "Test2"
jac_sensors_np=kdl_jac_to_numpy_array(self.jac_sensors)
# print "pinv(jac.T)", pinv(jac_sensors_np.T)
# print "cur torques", self.parent.cur_torques
#print "Test3"
try:
self.forces=dot(weighted_pseudo_inverse(jac_sensors_np.T,self.static_weight_right,self.static_weight_left),self.parent.cur_torques)
except Exception,e:
print "Error during force calculation, force value not updated. Error:", e
class DhChain:
def __init__(self, config=[[0, 0, 0, 0]]):
# Determine number of links
# import code; code.interact(local=locals())
# try:
self._M = len(config['dh'])
rospy.logdebug("Initializing dh chain with [%u] links", self._M)
param_mat = config['dh']
# self._length = param_mat.size # The number of params needed to configure this chain
# self._config = param_mat # Mx4 matrix. Each row is a link, containing [theta, alpha, a, d]
self._length = len(param_mat * 6)
self._config = param_mat
self._cov_dict = config['cov']
self._gearing = config['gearing']
# except:
# self._cov_dict=config['cov']
# self._M=len(self._cov_dict['joint_angles'])
# param_mat = numpy.matrix([[0]*4]*self._M, float)
# self._length = param_mat.size # The number of params needed to configure this chain
# self._config = param_mat # Mx4 matrix. Each row is a link, containing [theta, alpha, a, d]
# self._gearing=[0]*self._M
def calc_free(self, free_config):
# import code; code.interact(local=locals())
# print len(free_config['dh'])
# print self._M
if free_config is None:
free_config = {}
if 'dh' not in free_config:
free_config['dh']["xyzrpy"] = [[0] * 6] * self._M
#else:
#print free_config["dh"]
#print [f for f in free_config["dh"]]
#free_config['dh'] = [f["xyzrpy"] for f in free_config["dh"]]
if 'gearing' not in free_config:
free_config['gearing'] = [0] * self._M
assert (len(free_config['dh']) == self._M)
assert (len(free_config['gearing']) == self._M)
# Flatten the config
flat_config = []
if isinstance(free_config["dh"][0], dict):
for cfg in free_config["dh"]:
flat_config.extend(cfg["xyzrpy"])
else:
flat_config = sum(free_config['dh'], [])
assert (len(flat_config) == self._M * 6)
flat_config = flat_config + free_config['gearing']
# Convert int list into bool list
flat_free = [x == 1 for x in flat_config]
return flat_free
def params_to_config(self, param_vec):
assert (param_vec.shape == (self._M * 7, 1)
) # 6 pose params + 1 gearing per joint
dh_param_vec = param_vec[0:(self._M * 6), 0]
gearing_param_vec = param_vec[(self._M * 6):, 0]
param_mat = reshape(dh_param_vec.T, (-1, 6))
config_dict = {}
config_dict['dh'] = self._config
for sc, c in zip(config_dict["dh"], param_mat):
sc["xyzrpy"] = c.tolist()[:][0]
config_dict['gearing'] = (array(gearing_param_vec)[:, 0]).tolist()
config_dict['cov'] = self._cov_dict
return config_dict
# Convert column vector of params into config
def inflate(self, param_vec):
param_mat = param_vec[:self._M * 6, :]
config = reshape(param_mat, (-1, 6))
for sc, c in zip(self._config, config):
sc["xyzrpy"] = c.tolist()[:][0]
gearing = array(param_vec[self._M * 6:, :])[:, 0].tolist()
if len(gearing) is len(self._gearing):
self._gearing = gearing
# Return column vector of config
def deflate(self):
# param_vec = reshape(self._config, (-1,1))
# param_vec = numpy.concatenate([param_vec, matrix(self._gearing).T])
param_vec = []
for joint in self._config:
param_vec.extend(joint["xyzrpy"])
param_vec.extend(self._gearing)
param_vec = matrix(param_vec).T
return param_vec
# Returns # of params needed for inflation & deflation
def get_length(self):
return self._M * 7
# Returns 4x4 numpy matrix of the pose of the tip of
# the specified link num. Assumes the last link's tip
# when link_num < 0
def fk(self, chain_state, link_num=-2):
if link_num < 0:
link_num -= 1
link_num += 1
if link_num < 0:
link_num = self._M
pos_scaled = [
cur_pos * cur_gearing for cur_pos, cur_gearing in zip(
chain_state.actual.positions, self._gearing)
]
#pos_scaled = JntArray(pos_scaled)
jnt_states = JntArray(len(pos_scaled))
for i, s in enumerate(pos_scaled):
jnt_states[i] = s
self.build_chain()
F1 = Frame()
assert (0 == self.fksolverpos.JntToCart(jnt_states, F1, link_num))
out = []
for i in range(3):
line = []
for j in range(3):
line.append(F1.M[i, j])
line.append(F1.p[i])
out.append(line)
out.append([0, 0, 0, 1])
out = matrix(out)
return out
def build_chain(self):
self.chain = Chain()
for e in self._config:
v = Vector(e["xyzrpy"][0], e["xyzrpy"][1], e["xyzrpy"][2])
r = Rotation.RPY(e["xyzrpy"][3], e["xyzrpy"][4], e["xyzrpy"][5])
j = Joint(e["name"], Joint.None)
f = Frame(r, v)
if e["type"] == "rotx":
axis = Vector(1.0, 0.0, 0.0)
j = Joint(e["name"], f.p, f.M * axis, Joint.RotAxis)
elif e["type"] == "roty":
axis = Vector(0.0, 1.0, 0.0)
j = Joint(e["name"], f.p, f.M * axis, Joint.RotAxis)
elif e["type"] == "rotz":
axis = Vector(0.0, 0.0, 1.0)
j = Joint(e["name"], f.p, f.M * axis, Joint.RotAxis)
elif e["type"] == "transx":
axis = Vector(1.0, 0.0, 0.0)
j = Joint(e["name"], f.p, f.M * axis, Joint.TransAxis)
elif e["type"] == "transy":
axis = Vector(0.0, 1.0, 0.0)
j = Joint(e["name"], f.p, f.M * axis, Joint.TransAxis)
elif e["type"] == "transz":
axis = Vector(0.0, 0.0, 1.0)
j = Joint(e["name"], f.p, f.M * axis, Joint.TransAxis)
self.chain.addSegment(
Segment(e["name"] + "seg", j, f, RigidBodyInertia()))
self.fksolverpos = ChainFkSolverPos_recursive(self.chain)
