コード例 #1
0
    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]
コード例 #2
0
 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]
コード例 #3
0
    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