def getUpperLimbAngles(self, tf, side):
"""
Defines the joint angles of the human arm, starting from the position
of the tf generated accordin to the data coming from the kinect
@param tf tf
@param 'L' for left upper limb, 'R' for right upper limb
"""
if side == 'L':
#4: elbow (-x), #3: shoulder 3 (y), #2: shoulder 2 (z), #1: shoulder (-x) in kin frame
self.last_updated, sys_shoulder = utils.getSkeletonTransformation(self.id, tf, 'left_shoulder', self.kin_frame, self.last_updated)
self.last_updated, sys_elbow = utils.getSkeletonTransformation(self.id, tf, 'left_elbow', self.kin_frame, self.last_updated)
self.last_updated, sys_hand = utils.getSkeletonTransformation(self.id, tf, 'left_hand', self.kin_frame, self.last_updated)
else:
#4: elbow (-x), #3: shoulder 3 (-y), #2: shoulder 2 (-z), #1: shoulder (-x) in kin frame
self.last_updated, sys_shoulder = utils.getSkeletonTransformation(self.id, tf, 'right_shoulder', self.kin_frame, self.last_updated)
self.last_updated, sys_elbow = utils.getSkeletonTransformation(self.id, tf, 'right_elbow', self.kin_frame, self.last_updated)
self.last_updated, sys_hand = utils.getSkeletonTransformation(self.id, tf, 'right_hand', self.kin_frame, self.last_updated)
if sys_shoulder is None or sys_elbow is None or sys_hand is None:
return None
sys_sh_elb = numpy.dot(numpy.linalg.inv(sys_shoulder), sys_elbow)
vect_es = (sys_shoulder[0:3,3] - sys_elbow[0:3,3])/ \
numpy.linalg.norm([sys_shoulder[0:3,3] - sys_elbow[0:3,3]])
vect_he = (sys_elbow[0:3,3] - sys_hand[0:3,3])/ \
numpy.linalg.norm([sys_elbow[0:3,3] - sys_hand[0:3,3]])
vect_norm_es_eh = numpy.cross(vect_he, vect_es)
q4 = - numpy.arccos(numpy.asscalar(numpy.dot(vect_he.T,vect_es))) #[0,-pi]
if side == 'L':
q2 = numpy.asscalar(numpy.arcsin(utils.checkArg(-vect_es[0]))) #[-pi/2,pi/2]
else:
q2 = numpy.asscalar(numpy.arcsin(utils.checkArg(vect_es[0])))
if sys_sh_elb[1,3] > 0: #projection on y of the sys origin. if y positive, add pi/2
q2 = q2 + numpy.sign(q2)*numpy.pi/2 #[-pi,pi]
if numpy.abs(numpy.cos(q2)) > 0.1:
q1 = numpy.asscalar(numpy.arccos(utils.checkArg(vect_es[1]/numpy.cos(q2)))) #[0,pi]
if numpy.asscalar(numpy.arcsin(utils.checkArg(-vect_es[2]/numpy.cos(q2)))) < 0: #[-pi,pi]
q1 = -q1
q3 = numpy.asscalar(numpy.arccos(utils.checkArg(-vect_norm_es_eh[0]/numpy.cos(q2))))
else:
q1 = None
q3 = None
return [q1, q2, q3, q4]
def getLowerLimbAngles(self, tf, side):
"""
Defines the joint angles of the human legs, starting from the position
of the tf generated accordin to the data coming from the kinect
@param tf tf
@param 'L' for left lower limb, 'R' for right lower limb
"""
if side == 'L':
self.last_updated, sys_hip = utils.getSkeletonTransformation(self.id, tf, 'left_hip', self.kin_frame, self.last_updated)
self.last_updated, sys_knee = utils.getSkeletonTransformation(self.id, tf, 'left_knee', self.kin_frame, self.last_updated)
self.last_updated, sys_foot = utils.getSkeletonTransformation(self.id, tf, 'left_foot', self.kin_frame, self.last_updated)
else:
self.last_updated, sys_hip = utils.getSkeletonTransformation(self.id, tf, 'right_hip', self.kin_frame, self.last_updated)
self.last_updated, sys_knee = utils.getSkeletonTransformation(self.id, tf, 'right_knee', self.kin_frame, self.last_updated)
self.last_updated, sys_foot = utils.getSkeletonTransformation(self.id, tf, 'right_foot', self.kin_frame, self.last_updated)
if sys_hip is None or sys_knee is None or sys_foot is None:
return None
vect_kh = (sys_hip[0:3,3] - sys_knee[0:3,3])/ \
numpy.linalg.norm([sys_hip[0:3,3] - sys_knee[0:3,3]])
vect_fk = (sys_knee[0:3,3] - sys_foot[0:3,3])/ \
numpy.linalg.norm([sys_knee[0:3,3] - sys_foot[0:3,3]])
q2 = - numpy.arccos(utils.checkArg(numpy.asscalar(numpy.dot(vect_kh.T,vect_fk))))
q1 = numpy.asscalar(numpy.arccos(vect_kh[1])) #[0,pi]
if numpy.asscalar(numpy.arcsin(vect_kh[2])) < 0: #[-pi,pi]
q1 = -q1
return [q1, q2]
def getUpperLimbAngles(self, tf, side):
"""
Defines the joint angles of the human arm, starting from the position
of the tf generated accordin to the data coming from the kinect
@param tf tf
@param 'L' for left upper limb, 'R' for right upper limb
"""
if side == 'L':
#4: elbow (-x), #3: shoulder 3 (y), #2: shoulder 2 (z), #1: shoulder (-x) in kin frame
self.last_updated, sys_shoulder = utils.getSkeletonTransformation(self.id, tf, 'ShoulderLeft', self.refsys, self.last_updated)
self.last_updated, sys_elbow = utils.getSkeletonTransformation(self.id, tf, 'ElbowLeft', self.refsys, self.last_updated)
self.last_updated, sys_hand = utils.getSkeletonTransformation(self.id, tf, 'WristLeft', self.refsys, self.last_updated)
else:
#4: elbow (-x), #3: shoulder 3 (-y), #2: shoulder 2 (-z), #1: shoulder (-x) in kin frame
self.last_updated, sys_shoulder = utils.getSkeletonTransformation(self.id, tf, 'ShoulderRight', self.refsys, self.last_updated)
self.last_updated, sys_elbow = utils.getSkeletonTransformation(self.id, tf, 'ElbowRight', self.refsys, self.last_updated)
self.last_updated, sys_hand = utils.getSkeletonTransformation(self.id, tf, 'WristRight', self.refsys, self.last_updated)
self.last_updated, sys_shcen = utils.getSkeletonTransformation(self.id, tf, 'SpineShoulder', self.refsys, self.last_updated)
if sys_shoulder is None or sys_elbow is None or sys_hand is None or sys_shcen is None:
return None
sys_shcen_elb = numpy.dot(numpy.linalg.inv(sys_shcen), sys_elbow)
den_vect_es = numpy.linalg.norm([sys_shoulder[0:3,3] - sys_elbow[0:3,3]])
den_vect_he = numpy.linalg.norm([sys_elbow[0:3,3] - sys_hand[0:3,3]])
if numpy.abs(den_vect_es) > 0.001 and numpy.abs(den_vect_he) > 0.001:
vect_es = (sys_shoulder[0:3,3] - sys_elbow[0:3,3])/ den_vect_es
vect_he = (sys_elbow[0:3,3] - sys_hand[0:3,3])/ den_vect_he
vect_norm_es_eh = numpy.cross(vect_he, vect_es)
q4 = - numpy.arccos(numpy.asscalar(numpy.dot(vect_he.T,vect_es))) #[0,-pi]
if side == 'L':
q2 = numpy.asscalar(numpy.arcsin(utils.checkArg(-vect_es[0]))) #[-pi/2,pi/2]
else:
q2 = numpy.asscalar(numpy.arcsin(utils.checkArg(vect_es[0])))
if sys_shcen_elb[1,3] > 0: #projection on y of the SpineShoulder. if y positive, add pi/2
q2 = q2 + numpy.sign(q2)*numpy.pi/2 #[-pi,pi]
if numpy.abs(numpy.cos(q2)) > 0.1:
q1 = numpy.asscalar(numpy.arccos(utils.checkArg(vect_es[1]/numpy.cos(q2)))) #[0,pi]
if numpy.asscalar(numpy.arcsin(utils.checkArg(-vect_es[2]/numpy.cos(q2)))) < 0: #[-pi,pi]
q1 = -q1
if numpy.abs(numpy.sin(q1)) > 0.1:
q3 = numpy.asscalar(numpy.arccos(utils.checkArg(-vect_norm_es_eh[0]/numpy.cos(q2)))) #[0,pi]
comm_q3 = numpy.cos(q1)*numpy.cos(q3)*numpy.sin(q2)
if side == 'L':
val_q3 = (vect_norm_es_eh[1] + comm_q3)/numpy.sin(q1)
q3_b = numpy.asscalar(numpy.arcsin(utils.checkArg(val_q3)))
else:
val_q3 = (-vect_norm_es_eh[1] + comm_q3)/numpy.sin(q1)
q3_b = numpy.asscalar(numpy.arcsin(utils.checkArg(val_q3)))
if q3_b < 0: #[-pi,pi]
q3 = -q3
else:
q3 = None
else:
q1 = None
q3 = None
return [q1, q2, q3, q4]
else:
return None
