Joints are not moving

[slr_555]

Hi, so I'm working through the Ludobots tutorials, currently I am on adding joints to my robot https://www.reddit.com/r/ludobots/wiki/core06.

I can connect two leg components together, and they lock in position but they are rigidly fixed at their starting position at 90deg. Also it seems like the x and z cood for the hinge are switched from the world frame?

Here is my code, and help would be great.

RagdollDemo.h

Code: Select all

/*


#ifndef RAGDOLLDEMO_H
#define RAGDOLLDEMO_H


#include "GlutDemoApplication.h"
#include "LinearMath/btAlignedObjectArray.h"
#include "btBulletDynamicsCommon.h"
#include "BulletDynamics/ConstraintSolver/btHingeConstraint.h"

class btBroadphaseInterface;
class btCollisionShape;
class btOverlappingPairCache;
class btCollisionDispatcher;
class btConstraintSolver;
struct btCollisionAlgorithmCreateFunc;
class btDefaultCollisionConfiguration;

class RagdollDemo : public GlutDemoApplication
{

	btAlignedObjectArray<class RagDoll*> m_ragdolls;

	//keep the collision shapes, for deletion/cleanup
	btAlignedObjectArray<btCollisionShape*>	m_collisionShapes;

	btBroadphaseInterface*	m_broadphase;

	btCollisionDispatcher*	m_dispatcher;

	btConstraintSolver*	m_solver;

	btDefaultCollisionConfiguration* m_collisionConfiguration;

	btRigidBody*	body[9]; // one main body, 4x2 leg segments 
	btCollisionShape*	geom[9];
	btHingeConstraint* joints[8];
	bool pause = false;
	bool oneStep;

	double M_PI = 3.14159265358979323846;
	double M_PI_2 = 1.57079632679489661923;
	double M_PI_4 = 0.785398163397448309616;

public:
	void initPhysics();

	void exitPhysics();

	virtual ~RagdollDemo()
	{
		exitPhysics();
	}

	void spawnRagdoll(const btVector3& startOffset);

	virtual void clientMoveAndDisplay();

	virtual void displayCallback();

	virtual void keyboardCallback(unsigned char key, int x, int y);

	static DemoApplication* Create()
	{
		RagdollDemo* demo = new RagdollDemo();
		demo->myinit();
		demo->initPhysics();
		return demo;
	}


	void CreateBox(int index, double x, double y, double z, double length, double width, double height)
	{
		geom[index] = new btBoxShape(btVector3(width, height, length));

		//btDefaultMotionState* motionstate = new btDefaultMotionState();
		btDefaultMotionState* motionstate = new btDefaultMotionState(btTransform(
			btQuaternion(0, 0, 0, 1),
			btVector3(x, y, z)
		));

		btRigidBody::btRigidBodyConstructionInfo rigidBodyCI(
			1.0,                  // mass, in kg. 0 -> Static object, will never move.
			motionstate,
			geom[index],  // collision shape of body
			btVector3(0, 0, 0)    // local inertia
		);

		body[index] = new btRigidBody(rigidBodyCI);

		m_dynamicsWorld->addRigidBody(body[index]);

	}

	void CreateCylinder(int index, double x, double y, double z, double diameter, double sideLength, double angle)
	{
		geom[index] = new btCylinderShape(btVector3(diameter, sideLength, diameter));

		//btDefaultMotionState* motionstate = new btDefaultMotionState();
		btDefaultMotionState* motionstate = new btDefaultMotionState(btTransform(
			btQuaternion(btVector3(0, 0, 1), angle),
			btVector3(x, y, z)
		));

		btRigidBody::btRigidBodyConstructionInfo rigidBodyCI(
			1.0,                  // mass, in kg. 0 -> Static object, will never move.
			motionstate,
			geom[index],  // collision shape of body
			btVector3(0, 0, 0.5)    // local inertia
		);

		//    glRotatef(90, 1, 0, 0);

		body[index] = new btRigidBody(rigidBodyCI);

		m_dynamicsWorld->addRigidBody(body[index]);
	}


	void CreateCylinder2(int index, double x, double y, double z, double diameter, double sideLength, double angle)
	{
		geom[index] = new btCylinderShape(btVector3(diameter, sideLength, diameter));

		//btDefaultMotionState* motionstate = new btDefaultMotionState();
		btDefaultMotionState* motionstate = new btDefaultMotionState(btTransform(
			btQuaternion(btVector3(1, 0, 0), angle),
			btVector3(x, y, z)
		));

		btRigidBody::btRigidBodyConstructionInfo rigidBodyCI(
			1.0,                  // mass, in kg. 0 -> Static object, will never move.
			motionstate,
			geom[index],  // collision shape of body
			btVector3(0.5, 0, 0)    // local inertia
		);

		//glRotatef(90, 1, 0, 0);

		body[index] = new btRigidBody(rigidBodyCI);

		m_dynamicsWorld->addRigidBody(body[index]);
	}

	// takes a point p in world coordinates, and returns the coordinates of this point relative to the body stored in body[index].
	btVector3 PointWorldToLocal(int index, btVector3 &p) {
		btTransform local1 = body[index]->getCenterOfMassTransform().inverse();
		return local1 * p;
	}

	//takes an axis a in world coordinates and returns the coordinates of this point relative to the body stored in body[index].
	btVector3 AxisWorldToLocal(int index, btVector3 &a) {
		btTransform local1 = body[index]->getCenterOfMassTransform().inverse();
		btVector3 zero(0, 0, 0);
		local1.setOrigin(zero);
		return local1 * a;
	}

	btVector3 flipZY(btVector3 &input) {
		btScalar temp;
		temp = input[1];
		input[1] = input[2];
		input[2] = temp;
		return input;
	}

	void CreateHinge(int index, int body1, int body2, 
		double x, double y, double z, double ax, double ay, double az) {

		btVector3 p(x, y, z);
		btVector3 a(ax, ay, az);

		btVector3 p1 = PointWorldToLocal(body1, p);
		btVector3 p2 = PointWorldToLocal(body2, p);
		btVector3 a1 = AxisWorldToLocal(body1, a);
		btVector3 a2 = AxisWorldToLocal(body2, a);

		//btVector3 p1 = PointWorldToLocal(body1, flipZY(p));
		////btVector3 p2 = PointWorldToLocal(body2, flipZY(p));
		//btVector3 a1 = AxisWorldToLocal(body1, flipZY(a));
		//btVector3 a2 = AxisWorldToLocal(body2, flipZY(a));
		//
		joints[index] = new btHingeConstraint(*body[body1], *body[body2],
			p1, p2,
			a1, a2, false);
		//joints[index]->setLimit(btScalar(-M_PI_2), btScalar(M_PI_2));
		
		m_dynamicsWorld->addConstraint(joints[index], true);

	}
	void DestroyHinge(int index) {
		m_dynamicsWorld->removeConstraint(joints[index]);
	}

	void DeleteObject(int index) {
		m_dynamicsWorld->removeRigidBody(body[index]);
		delete body[index]->getMotionState();
		delete body[index];
		delete geom[index];
	}
	
};


#endif

RagdollDemo.cpp

Code: Select all

/*


#define CONSTRAINT_DEBUG_SIZE 0.2f


#include "btBulletDynamicsCommon.h"
#include "GlutStuff.h"
#include "GL_ShapeDrawer.h"

#include "LinearMath/btIDebugDraw.h"

#include "GLDebugDrawer.h"
#include "RagdollDemo.h"



// Enrico: Shouldn't these three variables be real constants and not defines?

#ifndef M_PI
#define M_PI       3.14159265358979323846
#endif

#ifndef M_PI_2
#define M_PI_2     1.57079632679489661923
#endif

#ifndef M_PI_4
#define M_PI_4     0.785398163397448309616
#endif

class RagDoll
{
	enum
	{
		BODYPART_COUNT = 10
	};

	enum
	{
		JOINT_PELVIS_SPINE = 0,
		JOINT_SPINE_HEAD,

		JOINT_LEFT_HIP,
		JOINT_LEFT_KNEE,

		JOINT_RIGHT_HIP,
		JOINT_RIGHT_KNEE,

		JOINT_LEFT_SHOULDER,
		JOINT_LEFT_ELBOW,

		JOINT_RIGHT_SHOULDER,
		JOINT_RIGHT_ELBOW,

		JOINT_COUNT
	};

	btDynamicsWorld* m_ownerWorld;
	btCollisionShape* geom[9]; //m_shapes[BODYPART_COUNT];
	btRigidBody* body[9]; //m_bodies[BODYPART_COUNT];
	btTypedConstraint* m_joints[JOINT_COUNT];

	btRigidBody* localCreateRigidBody (btScalar mass, const btTransform& startTransform, btCollisionShape* shape)
	{
		bool isDynamic = (mass != 0.f);

		btVector3 localInertia(0,0,0);
		if (isDynamic)
			shape->calculateLocalInertia(mass,localInertia);

		btDefaultMotionState* myMotionState = new btDefaultMotionState(startTransform);
		
		btRigidBody::btRigidBodyConstructionInfo rbInfo(mass,myMotionState,shape,localInertia);
		btRigidBody* body = new btRigidBody(rbInfo);

		m_ownerWorld->addRigidBody(body);

		return body;
	}

public:
	RagDoll (btDynamicsWorld* ownerWorld, const btVector3& positionOffset)
		: m_ownerWorld (ownerWorld)
	{

//
		// Setup some damping on the m_bodies
		for (int i = 0; i < BODYPART_COUNT; ++i)
		{
			body[i]->setDamping(0.05, 0.85);
			body[i]->setDeactivationTime(0.8);
			body[i]->setSleepingThresholds(1.6, 2.5);
		}

		// Now setup the constraints
		/*btHingeConstraint* hingeC;
		btConeTwistConstraint* coneC;

		btTransform localA, localB;*/

	}

	virtual	~RagDoll ()
	{
		int i;

		// Remove all constraints
		for ( i = 0; i < JOINT_COUNT; ++i)
		{
			m_ownerWorld->removeConstraint(m_joints[i]);
			delete m_joints[i]; m_joints[i] = 0;
		}

		// Remove all bodies and shapes
		for ( i = 0; i < BODYPART_COUNT; ++i)
		{
			m_ownerWorld->removeRigidBody(body[i]);
			
			delete body[i]->getMotionState();

			delete body[i]; body[i] = 0;
			delete geom[i]; geom[i] = 0;
		}
	}

};




void RagdollDemo::initPhysics()
{
	// Setup the basic world

	setTexturing(true);
	setShadows(true);

	setCameraDistance(btScalar(5.));

	m_collisionConfiguration = new btDefaultCollisionConfiguration();

	m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration);

	btVector3 worldAabbMin(-10000,-10000,-10000);
	btVector3 worldAabbMax(10000,10000,10000);
	m_broadphase = new btAxisSweep3 (worldAabbMin, worldAabbMax);

	m_solver = new btSequentialImpulseConstraintSolver;

	m_dynamicsWorld = new btDiscreteDynamicsWorld(m_dispatcher,m_broadphase,m_solver,m_collisionConfiguration);
	//m_dynamicsWorld->getDispatchInfo().m_useConvexConservativeDistanceUtil = true;
	//m_dynamicsWorld->getDispatchInfo().m_convexConservativeDistanceThreshold = 0.01f;



	// Setup a big ground box
	{
		btCollisionShape* groundShape = new btBoxShape(btVector3(btScalar(200.),btScalar(10.),btScalar(200.)));
		m_collisionShapes.push_back(groundShape);
		btTransform groundTransform;
		groundTransform.setIdentity();
		groundTransform.setOrigin(btVector3(0,-10,0));

#define CREATE_GROUND_COLLISION_OBJECT 1
#ifdef CREATE_GROUND_COLLISION_OBJECT
		btCollisionObject* fixedGround = new btCollisionObject();
		fixedGround->setCollisionShape(groundShape);
		fixedGround->setWorldTransform(groundTransform);
		m_dynamicsWorld->addCollisionObject(fixedGround);
#else
		localCreateRigidBody(btScalar(0.),groundTransform,groundShape);
#endif //CREATE_GROUND_COLLISION_OBJECT

	}

	// Spawn one ragdoll
	btVector3 startOffset(1,0.5,0);
	//spawnRagdoll(startOffset);
	startOffset.setValue(-1,0.5,0);
	//spawnRagdoll(startOffset);
	CreateBox(0, 0, 2, 0, 1, 1, 0.2); // Create the box
	
	CreateCylinder(1, 2, 2, 0, 0.2, 1, -1.57); // Create Leg 1
	CreateCylinder(2, -2., 2., 0., 0.2, 1., -1.57); // Create Leg 2
	CreateCylinder2(3, 0, 2., 2., 0.2, 1., -1.57); // Create Leg 3
	CreateCylinder2(4, 0, 2., -2., 0.2, 1., -1.57); // Create Leg 4

	CreateCylinder(5, 3., 1, 0, 0.2, 1, 0); // Create Leg 5
	CreateCylinder(6, -3., 1, 0., 0.2, 1., 0); // Create Leg 6
	CreateCylinder2(7, 0, 1, 3., 0.2, 1., 0); // Create Leg 7
	CreateCylinder2(8, 0, 1, -3., 0.2, 1., 0); // Create Leg 8

	CreateHinge(0, 4, 8, -1.5, 1.0, 0, 0, 0, 1);
	clientResetScene();
}

void RagdollDemo::spawnRagdoll(const btVector3& startOffset)
{
	RagDoll* ragDoll = new RagDoll (m_dynamicsWorld, startOffset);
	m_ragdolls.push_back(ragDoll);
}	

void RagdollDemo::clientMoveAndDisplay()
{
	
	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); 

	//simple dynamics world doesn't handle fixed-time-stepping
	float ms = getDeltaTimeMicroseconds();

	float minFPS = 1000000.f/60.f;
	if (ms > minFPS)
		ms = minFPS;

	if (m_dynamicsWorld)
	{
		if (!pause) {

			m_dynamicsWorld->stepSimulation(ms / 1000000.f);

		}
		
		//optional but useful: debug drawing
		m_dynamicsWorld->debugDrawWorld();


	}

	renderme(); 

	glFlush();

	glutSwapBuffers();
}

void RagdollDemo::displayCallback()
{
	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); 

	renderme();

	//optional but useful: debug drawing
	if (m_dynamicsWorld)
		m_dynamicsWorld->debugDrawWorld();

	glFlush();
	glutSwapBuffers();
}

void RagdollDemo::keyboardCallback(unsigned char key, int x, int y)
{
	switch (key)
	{
	case 'e':
		{
		btVector3 startOffset(0,2,0);
		spawnRagdoll(startOffset);
		break;
		}
	default:
		DemoApplication::keyboardCallback(key, x, y);
	}
	
	switch (key)
	{
	case 'p':
	{
		if (pause == false) {
			pause = true;
		}
		else {
			pause = false;
		}

	}
		
	default:
		DemoApplication::keyboardCallback(key, x, y);
	}
	
}



void	RagdollDemo::exitPhysics()
{

	int i;

	for (i=0;i<m_ragdolls.size();i++)
	{
		RagDoll* doll = m_ragdolls[i];
		delete doll;
	}

	//cleanup in the reverse order of creation/initialization

	//remove the rigidbodies from the dynamics world and delete them

	DeleteObject(0); 
	DeleteObject(1);
	DeleteObject(2);
	DeleteObject(3);
	DeleteObject(4);
	DeleteObject(5);
	DeleteObject(6);
	DeleteObject(7);
	DeleteObject(8);
	
	DestroyHinge(0);
	
	for (i=m_dynamicsWorld->getNumCollisionObjects()-1; i>=0 ;i--)
	{
		btCollisionObject* obj = m_dynamicsWorld->getCollisionObjectArray()[i];
		btRigidBody* body = btRigidBody::upcast(obj);
		if (body && body->getMotionState())
		{
			delete body->getMotionState();
		}
		m_dynamicsWorld->removeCollisionObject( obj );
		delete obj;
	}

	//delete collision shapes
	for (int j=0;j<m_collisionShapes.size();j++)
	{
		btCollisionShape* shape = m_collisionShapes[j];
		delete shape;
	}

	//delete dynamics world
	delete m_dynamicsWorld;

	//delete solver
	delete m_solver;

	//delete broadphase
	delete m_broadphase;

	//delete dispatcher
	delete m_dispatcher;

	delete m_collisionConfiguration;

	
}

[benelot]

Hi,

Can you first clean up your code a bit? At least when asking for help here, you might first want to remove large commented sections. I found your section creating a hinge, and I see that you do not set the limits of the hinge. I do not remember the default values of the limits, but it might be the cause of your lock. Give it a try.

[Erwin Coumans]

Ludobots is a nice resource, but a bit out-of-date.

The latest version of Bullet has much better support for robots:

1) You can setup a robot directly by loading it from a URDF file or SDF file
2) btMultiBody with child links as an alternative to btRigidBody+constraints

Please update to latest version of Bullet and check out the Example Browser. Your challenge becomes how to edit the URDF file to create a robot, instead of having to dig into C++ code to setup the robot.

[benelot]

You could also look into the 3DNNWalker example I created. It contains the implemenentation of a similar creature to the ragdoll in ludobots.