bulletphysics / bullet3

Bullet Physics SDK: real-time collision detection and multi-physics simulation for VR, games, visual effects, robotics, machine learning etc.
http://bulletphysics.org
Other
12.54k stars 2.87k forks source link

Using the function calculateInverseKinematics, it is not possible to control the joint limit and kinematic solution chain #4642

Open Yaohu117 opened 2 months ago

Yaohu117 commented 2 months ago

I hope to use the function calculateInverseKinematics to solve the inverse kinematics. My URDF model has 31 joints, and I am trying to solve the inverse of the right arm separately, that is, the joints with indices 12-18. I use lowerLimits, upperLimits, jointRanges, and restPoses to limit the body from moving, but still consider two body joints in the planning. How can I solve this problem.please help me. Here is some of my code:

class ym_ikFast:
    def __init__(self, whicharm):
        # index test 1
        self.lowerLimits = [-0.35,-0.35,-0.35,-3.14,-3.14,
                            -1.05,-1.57,-1.57,-0.09,-1.57,-1.05,-1.05,
                            -1.05,-0.09,-1.57,-1.40,-1.57,-1.05,-1.05,
                            -3.14,-3.14,-3.14,-3.14,-3.14,-3.14,-3.14,-3.14,-3.14,-3.14,-3.14,-3.14]
        self.upperLimits = [0.35,0.35,0.35,3.14,3.14,
                            1.05,0.09,1.57,1.40,1.57,1.05,1.05, 
                            1.05,1.57,1.57,0.09,1.57,1.05,1.05,
                            3.14,3.14,3.14,3.14,3.14,3.14,3.14,3.14,3.14,3.14,3.14,3.14]
        self.jointRanges = [0.7,0.7,0.7,6.28,6.28,
                            2.1,1.66,3.14,1.49,3.14,2.1,2.1,
                            2.1,1.66,3.14,1.49,3.14,2.1,2.1,
                            6.28,6.28,6.28,6.28,6.28,6.28,6.28,6.28,6.28,6.28,6.28,6.28]
        self.restPoses = [0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0]
        self.jointDamping = [0.00001, 0.00001, 0.00001, 0.00001, 0.00001, 0.00001, 0.00001, 0.00001, 0.00001, 0.00001,
                             0.00001, 0.00001, 0.00001, 0.00001, 0.00001, 0.00001, 0.00001, 0.00001, 0.00001, 0.00001,
                             0.00001, 0.00001, 0.00001, 0.00001, 0.00001, 0.00001, 0.00001, 0.00001, 0.00001, 0.00001, 0.00001]
        self.endEffectorLinkIndex = whicharm # right_arm:18   left_arm:11
        '''
            0:dummy_joint
            1:Body_Joint1   2:Body_Joint2
            3:Neck_Joint1   4:Neck_Joint2
            5~11:Left_Arm_Joint1 ~ Left_Arm_Joint7
            12~18:Right_Arm_Joint1 ~ Right_Arm_Joint7
            19~30:Leg Joint
        '''

        self.physicsClient = None
        self.robotId = None
        # self.current_pos = None
        # self.current_orie = None
        self.end_link_ = None

        # 指定关节限制
        self.lowerLimits[0] = self.upperLimits[0] = self.restPoses[0] = 0
        self.lowerLimits[1] = self.upperLimits[1] = self.restPoses[1] = -0.0013177308185664677
        self.lowerLimits[2] = self.upperLimits[2] = self.restPoses[2] = -0.001793627978015234
        self.lowerLimits[3] = self.upperLimits[3] = self.restPoses[3] = -0.001793627978015234
        self.jointRanges[0] = self.jointRanges[1] = self.jointRanges[2] = 0

2024-07-23 19-49-20 的屏幕截图 The small coordinate in the picture is the position that I need to solve for at the end of my right hand.

Yaohu117 commented 2 months ago

I tried using my left arm instead (index 5-11), and the same problem persisted: 2024-07-24 11-26-46 的屏幕截图 This is my limit parameter, so from this perspective, only the joints with indices 5-11 should be active, But it also planned for two joints on the body. 2024-07-24 11-30-48 的屏幕截图