Friction between body and groundplane

Post Reply
alexp
Posts: 4
Joined: Fri Aug 04, 2017 5:10 am

Friction between body and groundplane

Post by alexp »

Hi all,
I set up an ant on ground plane environment inspired by the evo neural network 3D walker example.
My problem now is, that I have no or minimal friction between the ant and the groundplane and I can not figure out how to increase it.
This results in my ML code learning to slide the ant, but not to walk.

The interesting part of init physics looks like
(I tried to set friction for the ground plane ground->setFriction(5); with no effect):

Code: Select all

createEmptyDynamicsWorld();
  
  m_dynamicsWorld->setInternalTickCallback(evaluationUpdatePreTickCallback, this, true); //true is preTick; this is necessary to have the torques appiled!
  m_dynamicsWorld->setInternalTickCallback(evaluationUpdatePostTickCallback, this, false); //false is postTick; this is necessary to have the torques appiled!


  // 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));
    btRigidBody* ground = createRigidBody(btScalar(0.),groundTransform,groundShape);
    ground->setFriction(5);
    ground->setUserPointer(GROUND_ID);
  }
  // Spawn one ant
  btVector3 offset(0,0,0);
  int i = 0;
  spawnAnt(i, offset, false);


the spawnAnt follows the example; i call the ant initializer (appended below), which adds the rigid bodies for the main body and 12 leg parts connected via hinge constraints.
In there, I use body->setDamping(0.05, 0.85) but no setFriction.
How can I achieve h higher friction between the groundplane and the ant's body parts, but not between the ant body parts themselves (or at least no higher friction for the hinge motion)?

Code: Select all

  Ant(int index, btDynamicsWorld* ownerWorld, const btVector3& positionOffset, bool bFixed, BasicExample* pBasicExample )
        : pContainingBasicExample (pBasicExample),
          m_ownerWorld (ownerWorld)
		  
	{


		m_index = index;
		btVector3 vUp(0, 1, 0); // up in local reference frame



		//randomizeSensoryMotorWeights();

		//
		// Setup geometry
		m_shapes[0] = new btCapsuleShape(gRootBodyRadius, gRootBodyHeight); // root body capsule
		int i;
		for ( i=0; i<NUM_LEGS; i++)
		{
			m_shapes[1 + 2*i] = new btCapsuleShape(gLegRadius, gLegLength); // leg  capsule
			m_shapes[2 + 2*i] = new btCapsuleShape(gForeLegRadius, gForeLegLength); // fore leg capsule
		}



		//initialize jointTorqueValues
		initJointTorque();
		//
		// Setup rigid bodies
		btScalar rootAboveGroundHeight = gForeLegLength;
		btTransform bodyOffset; bodyOffset.setIdentity();
		bodyOffset.setOrigin(positionOffset);		

		// root body
		btVector3 localRootBodyPosition = btVector3(btScalar(0.), rootAboveGroundHeight, btScalar(0.)); // root body position in local reference frame
		btTransform transform;
		transform.setIdentity();
		transform.setOrigin(localRootBodyPosition);


		m_bodies[0] = localCreateRigidBody(btScalar(bFixed?0.:1.), bodyOffset*transform, m_shapes[0]);
		m_ownerWorld->addRigidBody(m_bodies[0]);
		m_bodyRelativeTransforms[0] = btTransform::getIdentity();
		m_bodies[0]->setUserPointer(this);
		m_bodyTouchSensorIndexMap.insert(btHashPtr(m_bodies[0]), 0);

		btHingeConstraint* hingeC;
		//btConeTwistConstraint* coneC;

		btTransform localA, localB, localC;

		// legs
		for (i = 0; i < NUM_LEGS; i++)
		{
			float footAngle = 2 * SIMD_PI * i / NUM_LEGS; // legs are uniformly distributed around the root body
			float footYUnitPosition = sin(footAngle); // y position of the leg on the unit circle
			float footXUnitPosition = cos(footAngle); // x position of the leg on the unit circle

			transform.setIdentity();
			btVector3 legCOM = btVector3(btScalar(footXUnitPosition*(gRootBodyRadius+0.5*gLegLength)), btScalar(rootAboveGroundHeight), btScalar(footYUnitPosition*(gRootBodyRadius+0.5*gLegLength)));
			transform.setOrigin(legCOM);

			// thigh
			btVector3 legDirection = (legCOM - localRootBodyPosition).normalize();
			btVector3 kneeAxis = legDirection.cross(vUp);			
			transform.setRotation(btQuaternion(kneeAxis, SIMD_HALF_PI));
			m_bodies[1+2*i] = localCreateRigidBody(btScalar(1.), bodyOffset*transform, m_shapes[1+2*i]);
			m_bodyRelativeTransforms[1+2*i] = transform;
			m_bodies[1+2*i]->setUserPointer(this);
			m_bodyTouchSensorIndexMap.insert(btHashPtr(m_bodies[1+2*i]),1+2*i);

			// shin
			transform.setIdentity();
			transform.setOrigin(btVector3(btScalar(footXUnitPosition*(gRootBodyRadius+gLegLength)), btScalar(rootAboveGroundHeight-0.5*gForeLegLength), btScalar(footYUnitPosition*(gRootBodyRadius+gLegLength))));
			m_bodies[2+2*i] = localCreateRigidBody(btScalar(1.), bodyOffset*transform, m_shapes[2+2*i]);
			m_bodyRelativeTransforms[2+2*i] = transform;
			m_bodies[2+2*i]->setUserPointer(this);
			m_bodyTouchSensorIndexMap.insert(btHashPtr(m_bodies[2+2*i]),2+2*i);

			// hip joints
			localA.setIdentity(); localB.setIdentity();
			localA.getBasis().setEulerZYX(0,-footAngle,0);	localA.setOrigin(btVector3(btScalar(footXUnitPosition*gRootBodyRadius), btScalar(0.), btScalar(footYUnitPosition*gRootBodyRadius)));
			localB = b3ReferenceFrameHelper::getTransformWorldToLocal(m_bodies[1+2*i]->getWorldTransform(), b3ReferenceFrameHelper::getTransformLocalToWorld(m_bodies[0]->getWorldTransform(),localA));
			hingeC = new btHingeConstraint(*m_bodies[0], *m_bodies[1+2*i], localA, localB);
			//hingeC->setLimit(btScalar(-0.75 * SIMD_PI_4), btScalar(SIMD_PI_8));
			
			hingeC->setLimit(btScalar(-0.75 * SIMD_PI_4), btScalar(SIMD_PI_8), 1.0f,  1.0f, 0.0f );
			//softness: ~inverse friction;bias: bias of relaxed rotation (1.0 is neutral), relaxationFactor

			//hingeC->setLimit(btScalar(-0.1), btScalar(0.1));
			m_joints[2*i] = hingeC;

			{
			  btJointFeedback* fb = new btJointFeedback();
			  m_jointFeedback.push_back(fb);
			  hingeC->setJointFeedback(fb);
			}
			


			// knee joints
			localA.setIdentity(); localB.setIdentity(); localC.setIdentity();
			localA.getBasis().setEulerZYX(0,-footAngle,0);	localA.setOrigin(btVector3(btScalar(footXUnitPosition*(gRootBodyRadius+gLegLength)), btScalar(0.), btScalar(footYUnitPosition*(gRootBodyRadius+gLegLength))));
			localB = b3ReferenceFrameHelper::getTransformWorldToLocal(m_bodies[1+2*i]->getWorldTransform(), b3ReferenceFrameHelper::getTransformLocalToWorld(m_bodies[0]->getWorldTransform(),localA));
			localC = b3ReferenceFrameHelper::getTransformWorldToLocal(m_bodies[2+2*i]->getWorldTransform(), b3ReferenceFrameHelper::getTransformLocalToWorld(m_bodies[0]->getWorldTransform(),localA));
			hingeC = new btHingeConstraint(*m_bodies[1+2*i], *m_bodies[2+2*i], localB, localC);
			//hingeC->setLimit(btScalar(-0.01), btScalar(0.01));
			hingeC->setLimit(btScalar(-SIMD_PI_8), btScalar(0.2), 1.0f,  1.0f, 0.0f );
			//softness: ~inverse friction;bias: bias of relaxed rotation (1.0 is neutral), relaxationFactor 0 means no force


			m_joints[1+2*i] = hingeC;

			m_ownerWorld->addRigidBody(m_bodies[1+2*i]); // add thigh bone

			m_ownerWorld->addConstraint(m_joints[2*i], true); // connect thigh bone with root

			if(pContainingBasicExample->detectCollisions()){ // if thigh bone causes collision, remove it again

				m_ownerWorld->removeRigidBody(m_bodies[1+2*i]);
				m_ownerWorld->removeConstraint(m_joints[2*i]); // disconnect thigh bone from root
			}
			else{

				m_ownerWorld->addRigidBody(m_bodies[2+2*i]); // add shin bone
				m_ownerWorld->addConstraint(m_joints[1+2*i], true); // connect shin bone with thigh

				if(pContainingBasicExample->detectCollisions()){ // if shin bone causes collision, remove it again
					m_ownerWorld->removeRigidBody(m_bodies[2+2*i]);
					m_ownerWorld->removeConstraint(m_joints[1+2*i]); // disconnect shin bone from thigh
				}
			}
		}

		// Setup some damping on the m_bodies
		for (i = 0; i < BODYPART_COUNT; ++i)
		{
			m_bodies[i]->setDamping(0.05, 0.85);
			m_bodies[i]->setActivationState(DISABLE_DEACTIVATION); //we have only active objects in here
			/*m_bodies[i]->setDeactivationTime(0.8);
			//m_bodies[i]->setSleepingThresholds(1.6, 2.5);
			m_bodies[i]->setSleepingThresholds(0.5f, 0.5f);
			*/

		}

        //=== init some datafields
        m_startPosition = getMainBodyPosition();
        m_workTot=0.0; //init work done

		// removeFromWorld(); // it should not yet be in the world

	}
S1L3nCe
Posts: 50
Joined: Thu Mar 24, 2016 10:22 am
Location: France

Re: Friction between body and groundplane

Post by S1L3nCe »

Hi,
The friction value that you set on a rigid body object in Bullet isn't a "real" physical property (and isn't what's normally called the coefficient of friction) - it's just subsequently used to derive a friction coefficient for the resulting contact. The friction coefficient for the contact, when multiplied by the normal contact force, gives the maximum frictional force at that contact, as Basroil wrote
http://bulletphysics.org/Bullet/phpBB3/ ... hp?t=10985

Moreover, avoid using huge values for friction as explained here: http://bulletphysics.org/Bullet/phpBB3/ ... php?t=6783
alexp
Posts: 4
Joined: Fri Aug 04, 2017 5:10 am

Re: Friction between body and groundplane

Post by alexp »

Hi,
thanks for the hint. I corrected the setting to 1.0 and found out that friction is ok.
It seems that the joint constraint / limits let the ant drift.

see the following video
https://streamable.com/6t179
I have a constant torque of -5 on one of the joint connected to the main body (the leg pointing exactly to the right in the video).
The torque is so small that it is not sufficient to lift the leg as can be seen from this video:
https://streamable.com/gxsug

However, the ant continuously drifts to the left, so it seems that there is some strange effect (numeric noise?).
How can I get rid of this?

The torque is applied as follows:

Code: Select all

void applyHingeTorque(btHingeConstraint* pHinge, btScalar torque){

    /*
    btRigidBody& bodyA = pHinge->getRigidBodyA();
    btTransform trA = bodyA.getWorldTransform();
    btVector3 hingeAxisInWorld = trA.getBasis()*pHinge->getFrameOffsetA().getBasis().getColumn(2);
    pHinge->getRigidBodyA().applyTorque(-hingeAxisInWorld*torque);
    pHinge->getRigidBodyB().applyTorque(hingeAxisInWorld*torque);
    */

    btVector3 hingeAxisLocal = pHinge->getAFrame().getBasis().getColumn(2); // z-axis of constraint frame
    btVector3 hingeAxisWorld = pHinge->getRigidBodyA().getWorldTransform().getBasis() * hingeAxisLocal;
    btVector3 hingeTorque = torque * hingeAxisWorld;
    pHinge->getRigidBodyA().applyTorque(hingeTorque);
    pHinge->getRigidBodyB().applyTorque(-hingeTorque);

  }//applyHingeTorque
in the pre-tick callback for every joint (in this case 0.0 for all other joints)

Alex

edit:
I found that in the example browser API: TestHingeTorque example, the joint constraints behave strange: grabbing the sphere, I can twist it around and even bend the joint between the two bars.
The corresponding example in MultiBody: Test JointTorque is as stiff as expected.
-> is my problem related to this 'non- stiffness'?
alexp
Posts: 4
Joined: Fri Aug 04, 2017 5:10 am

Re: Friction between body and groundplane

Post by alexp »

I can confirm the cause of the problem is the stange behaviour of the constraints.
I e.g. found that having a limit on a joint, and applying a torque, the joint is rotated well beyond the limit.
In the following 10-20 timesteps, the angle drifts back towards the limit. The first timestep when the angle is below the limit, the torque is re-applied, leading to an angle again well beyond the limit.

I find this astonishing. I did some moleculardynamics some years ago. There, I implemented the constraints such, that they are exactly fulfilled in each timestep. So I would expect that if I set an angular limit, the angle never! will be beyond that limit.
Did I miss some special configuration in order to achieve 'hard' constraints?

Alex
User avatar
drleviathan
Posts: 849
Joined: Tue Sep 30, 2014 6:03 pm
Location: San Francisco

Re: Friction between body and groundplane

Post by drleviathan »

I think the btSequentialImpulseConstraintSolver performs a single pass each frame when solving the constraints, although there is at least one other solver you can choose from (btNNCGConstraintSolver.cpp) and I don't have any experience with that one.

What I've found with btSequentialImpulseConstraintSolver is that if you have a high number of constraints in a network of rigidbodies then the accuracy of the solver goes down --> the constraints get soft. I noticed this at about 8 constraints in a collection at substep = 1/60 sec. I hear it recovers accuracy when you reduce the substep time (1/240 sec or lower), but I haven't experimented very much with that so I can't tell you how much it helps or how far you need to go down. I have also heard that as you drop your timestep lower eventually you need to be using double precision or things start to go unstable or just stop working -- dunno where the transition starts but as I recall it was definitely a problem for someone at substep = 1/1000 sec.
Post Reply