Why cant I fix the arm on the shoulder joint?

[ipoirot]

This a question about Constraint. I want to simulate a robotic arm called PUMA 560 with Bullet.

Code: Select all

#define M_PI       3.14159265358979323846
#define M_PI_2     1.57079632679489661923
#define M_PI_4     0.785398163397448309616
#define M_PI_8     0.5 * M_PI_4


#define BASE_HEIGHT (1.0f)	
#define BASE_WIDTH (0.06f)    
#define BASE_MASS (300.0f)  

#define SHOULDER_LENGTH (0.15f) 
#define SHOULDER_WIDTH (0.03f)    
#define SHOULDER_MASS (12.5f)     

#define ARM_LENGTH (0.43f)	
#define ARM_WIDTH (0.04f)    
#define ARM_MASS (22.0f)     

#define FOREARM_LENGTH (0.43f)
#define FOREARM_WIDTH (0.03f) 
#define FOREARM_MASS (4.0f)  	
btCollisionShape* m_shapesT[4];
btRigidBody* m_bodiesT[4];
btTypedConstraint* m_jointsT[3];

Code: Select all

		btVector3 vBase = btVector3(btScalar(0.), btScalar(BASE_HEIGHT/2), btScalar(0.));
		transform.setIdentity();
		transform.setOrigin(vBase);
		m_bodiesT[0] = localCreateRigidBody(btScalar(0.),transform,m_shapesT[0]);

		transform.setIdentity();
		btVector3 vShoulderOrigin = btVector3(btScalar(0.),btScalar(BASE_HEIGHT),btScalar(SHOULDER_LENGTH/2));
		transform.setOrigin(vShoulderOrigin);

		//Shoulder
		btVector3 vToShoulder = (vShoulderOrigin-vBase).normalize();
		btVector3 vAxisX(1.0,0.0,0.0);
		transform.setRotation(btQuaternion(vAxisX,M_PI_2));
		m_bodiesT[1] = localCreateRigidBody(btScalar(SHOULDER_MASS),transform,m_shapesT[1]);

		//Arm
		btVector3 vArmOrigin = btVector3(btScalar(ARM_LENGTH/2),btScalar(BASE_HEIGHT),btScalar(SHOULDER_LENGTH));
		transform.setOrigin(vArmOrigin);
		btVector3 vAxisZ(0.0,0.0,1.0);
		transform.setRotation(btQuaternion(vAxisZ,M_PI_2));
		m_bodiesT[2] = localCreateRigidBody(btScalar(ARM_MASS),transform,m_shapesT[2]);

Code: Select all

		//joint
		btHingeConstraint* hingeC;
		btTransform localA,localB,localC,localA_backup;

		//build Waist Joint
		localA.setIdentity();
		localB.setIdentity();
		localA.getBasis().setEulerZYX(M_PI_2,0,0);
		localA.setOrigin(btVector3(btScalar(/*BASE_WIDTH*/0.),btScalar(BASE_HEIGHT),btScalar(0.)));
		localB = m_bodiesT[1]->getWorldTransform().inverse()*m_bodiesT[0]->getWorldTransform()*localA;

		hingeC = new btHingeConstraint(*m_bodiesT[0],*m_bodiesT[1],localA,localB);
		float waist_hi = 1;
		float waist_lo = -1;
		hingeC->setLimit(btScalar(waist_lo),btScalar(waist_hi));
		m_jointsT[0] = hingeC;
		m_ownerWorld->addConstraint(m_jointsT[0],true);

		btHinge2Constraint* hingeD;
		hingeD = new btHinge2Constraint(*m_bodiesT[1],*m_bodiesT[2],btVector3(0.0, BASE_HEIGHT, SHOULDER_LENGTH),m_bodiesT[0]->getWorldTransform().getBasis().getColumn(2),m_bodiesT[1]->getWorldTransform().getBasis().getColumn(0));
		m_jointsT[1] = hingeD;
		m_ownerWorld->addConstraint(m_jointsT[1],true);

Just copy these codes into the AppMotorDemo. The problem was that the arm cant be fixed on the shoulder joint. How can I resolve this problem?
Thanks very much...

[Wenlei_W]

In order to lock the hinge joint. You need to set: lowLimit = highLimit.