Weired behavior on 4-wheel robot: constant desired velocity but draw rectangle

[yanshil]

I was working on a wheeled-robot simulation, and I notice a strange behavior that I cannot tell what cause this.

What I have done:

1. Set specific value of targetVelocity in p.setJointMotorControl2 to each wheel motor.
2. Start simulation.

Expected Behavior:

Draw a circle / straight line. At least the angle of the chassis should not change too much

Actual Behavior:

For some specific value of targetVelocity, it will draw rectangle. It means that after some steps of stable angle simulation, it suddently turns.
Also, the number of stable simulation steps are approximative.

Notes

I notice this first on an omni-wheel robot, but this can be also reproduced in simple 4 wheel vehicle where each wheel is just a cylinder.
I even tested with the racecar example, and it will also show such behavior.

I also try to enable and disable the flag p.URDF_USE_INERTIA_FROM_FILE. This shows difference on the trajectory, but this behavior is still exists. And since I exported urdf from a model software I indeed expect this flag is on.

Code and example

Here is the recorded video. Code and urdf is in this repo. Videos are recorded using the values set in hello_*.py of each model.

1. The racecar model: racecar.urdf
   A small change: I modified the right_steering_hinge_joint from continuous to fixed to make it stable.

• video: https://streamable.com/af6aor

2. The simple car model: A cuboid with 4 cylinders.

• video: https://streamable.com/yc4wfk

3. The omni-wheeled car model: A cuboid with 4 mecanum wheels.

• video: https://streamable.com/jfgyot

Omni wheel support

I notice that somebody request an omni wheel support. I also really want this. And also if it is not supported yet, can anyone tell me what is the reason that omni wheel cannot be nicely simulated in PyBullet? ( I actually can tune the parameters to get some parallel moving behavior, but I'm not sure if it functions currently as it should be.)

I suppose it is something related to how the constraints is implemented but not quite sure.

[erwincoumans]

It is confusing and a lot of code to dig through. How can you simulate a simple car (non-mecanum wheels)? What does dividing a circle by a straight line mean?

Please simplify the code to 1 file and just drive straight, with regular wheels.

See an example using the racecar here:
https://github.com/erwincoumans/pybullet_robots
Just run f10_racecar.py

[yanshil]

I have the 1 file code in the repo. The reason it looks huge is because I have 3 example in it.

For all this 3 files, I just initialize the desired velocity of each wheel, and set to that value before simulation get started. And then you will see the trajectory shown above, the rectangles.

1. racecar : https://github.com/yanshil/PybulletCarRobot/blob/master/Pybullet_racecar/hello_racecar.py
   2.cuboic with mecanum wheels:
   https://github.com/yanshil/PybulletCarRobot/blob/master/mecanum_simple/hello_mecanum.py
2. cuboic with cylinder wheels: https://github.com/yanshil/PybulletCarRobot/blob/master/simple_cuboid_4074g/hello_simple.py

---

Also for the "circle/straight", I mean, if I don't change the motor “desired velocity” during the simulation, I don't know why will the simulator give me a rectangle. So there must be something happens in the simulator that generate this pattern.

A rectangle means for the same signal, some timesteps it goes straight, and suddenly after some timesteps, it turns.

[Steven89Liu]

do you have any progress?

[yanshil]

do you have any progress?

Ah? Me? No. I dig through this problem for a few weeks and this is already a summary of the problems I met with. I think the problem might be related to how the forces and torch implemented.

Let me conclude this in one sentence:

My car will run in a rectangle even though I didn't change the preset motor desired velocity. (And in real life, I think it should go like a circle or in a straight line)

---

Also for

How can you simulate a simple car (non-mecanum wheels)?

Well, the friction force applied on the wheel(saying the cylinder) drive the car. Doesn't it? Did I make something wrong?

[Steven89Liu]

OK, i will try to reproduce it later and work with you to find a solution.

[Steven89Liu]

for cylinders case it will run in a straight line if set all the target velocity to the same value. i will check why the trajectory is rectangle later.

[yanshil]

Hi, Steven,

Is there any updates about the rectangle pattern?

[Steven89Liu]

for mecanum case, i think it is more complicated for the contact between the Roller and the ground. i am trying a test to hard cod e to make the friction only exist along the axis of the Roller.

[Steven89Liu]

for mecanum case, this is the trajectory after applying my new patch attached also which seems like what you want.
in my patch i finish two things: (this patch only for your test)

1. only remain the contact between the wheel and the ground.
2. project the friction along the axis of the Roller.
   

diff --git a/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp b/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp
index 6c1ed09..fe4943c 100644
--- a/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp
+++ b/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp
@@ -25,6 +25,9 @@ subject to the following restrictions:
 #include "BulletDynamics/Featherstone/btMultiBodySolverConstraint.h"
 #include "LinearMath/btScalar.h"
 
+
+bool isPrint=false;
+
 btScalar btMultiBodyConstraintSolver::solveSingleIteration(int iteration, btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer)
 {
 	btScalar leastSquaredResidual = btSequentialImpulseConstraintSolver::solveSingleIteration(iteration, bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer);
@@ -236,7 +239,7 @@ btScalar btMultiBodyConstraintSolver::resolveSingleConstraintRowGeneric(const bt
 	btSolverBody* bodyB = 0;
 	int ndofA = 0;
 	int ndofB = 0;
-
+ if (c.m_linkA != -1 && isPrint) printf("resolveconstraint linkA=%d linkB=%d\n", c.m_linkA, c.m_linkB);
 	if (c.m_multiBodyA)
 	{
 		ndofA = c.m_multiBodyA->getNumDofs() + 6;
@@ -260,9 +263,10 @@ btScalar btMultiBodyConstraintSolver::resolveSingleConstraintRowGeneric(const bt
 		bodyB = &m_tmpSolverBodyPool[c.m_solverBodyIdB];
 		deltaVelBDotn += c.m_contactNormal2.dot(bodyB->internalGetDeltaLinearVelocity()) + c.m_relpos2CrossNormal.dot(bodyB->internalGetDeltaAngularVelocity());
 	}
-
+if (c.m_linkA != -1 && isPrint) printf("  rhs=%f, applied=%f,cfm=%f delta=%f, deltaVelADotn=%f, a_delta=%f, deltaVelBDotn=%f,b_delta=%f,", c.m_rhs, c.m_appliedImpulse,c.m_cfm, deltaImpulse, deltaVelADotn, deltaVelADotn * c.m_jacDiagABInv,  deltaVelBDotn, deltaVelBDotn * c.m_jacDiagABInv);
 	deltaImpulse -= deltaVelADotn * c.m_jacDiagABInv;  //m_jacDiagABInv = 1./denom
 	deltaImpulse -= deltaVelBDotn * c.m_jacDiagABInv;
+if (c.m_linkA != -1 && isPrint) printf("deltaImpulse = %f, ", deltaImpulse);
 	const btScalar sum = btScalar(c.m_appliedImpulse) + deltaImpulse;
 
 	if (sum < c.m_lowerLimit)
@@ -280,6 +284,7 @@ btScalar btMultiBodyConstraintSolver::resolveSingleConstraintRowGeneric(const bt
 		c.m_appliedImpulse = sum;
 	}
 
+if (c.m_linkA != -1 && isPrint) printf("f_deltaImpulse = %f %f %f\n", deltaImpulse, c.m_lowerLimit, c.m_upperLimit);
 	if (c.m_multiBodyA)
 	{
 		applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex], deltaImpulse, c.m_deltaVelAindex, ndofA);
@@ -1280,6 +1285,31 @@ void btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold*
 		{
 			btScalar relaxation;
 
+                        const btMultiBody* mbRoller;
+                        int link;
+                        bool flag = false;
+                        btVector3 revoluteAxisInWorld;
+                        if (mbA && (fcA->m_link != -1))
+                        {
+                             mbRoller = mbA;
+                             link = fcA->m_link;
+                        }
+                        else
+                        {
+                             mbRoller = mbB;
+                             link = fcB->m_link;
+                        }
+                        if (!(fcA->m_link == -1 && fcB->m_link == -1))
+                        {
+                            flag = true;
+                            revoluteAxisInWorld = quatRotate(mbRoller->getLink(link).m_cachedWorldTransform.getRotation(), mbRoller->getLink(link).m_axes[0].m_topVec);
+                            if (strstr(mbRoller->getLink(link).m_linkName, "Wheel") == NULL) return;
+                            printf("link=%d fcA->m_link=%d fcB->m_link=%d mbA=%p mbB=%p\n", link, fcA->m_link, fcB->m_link, mbA, mbB);
+                            printf("link name=%s, joint name=%s\n", mbRoller->getLink(link).m_linkName, mbRoller->getLink(link).m_jointName);
+                            printf("    roll axis: %f:%f:%f\n", revoluteAxisInWorld[0], revoluteAxisInWorld[1], revoluteAxisInWorld[2]);
+                        }
+
+
 			int frictionIndex = m_multiBodyNormalContactConstraints.size();
 
 			btMultiBodySolverConstraint& solverConstraint = m_multiBodyNormalContactConstraints.expandNonInitializing();
@@ -1300,6 +1330,7 @@ void btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold*
 
 			solverConstraint.m_originalContactPoint = &cp;
 
+
 			bool isFriction = false;
 			setupMultiBodyContactConstraint(solverConstraint, cp.m_normalWorldOnB, cp.m_appliedImpulse, cp, infoGlobal, relaxation, isFriction);
 
@@ -1326,10 +1357,16 @@ void btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold*
 			///In that case, you can set the target relative motion in each friction direction (cp.m_contactMotion1 and cp.m_contactMotion2)
 			///this will give a conveyor belt effect
 			///
-
+#if 0
 			btPlaneSpace1(cp.m_normalWorldOnB, cp.m_lateralFrictionDir1, cp.m_lateralFrictionDir2);
 			cp.m_lateralFrictionDir1.normalize();
 			cp.m_lateralFrictionDir2.normalize();
+#endif
+                        cp.m_lateralFrictionDir1 = revoluteAxisInWorld;
+                        cp.m_lateralFrictionDir1[2] = 0;
+                        cp.m_lateralFrictionDir2 = cp.m_lateralFrictionDir1.cross(cp.m_normalWorldOnB);
+                        cp.m_lateralFrictionDir1.normalize();
+                        cp.m_lateralFrictionDir2.normalize();
 
 			if (rollingFriction > 0)
 			{

[yanshil]

Thank you sooooo much!

Since it is the first time I try to build from source to use PyBullet, it takes me some time to walk through the process.

However, I got an error while I try to apply the patch using git apply patch.diff, where I simply copy your code block to get this file.

The last two line of the attached code block indeed looks strange to me.

(bulletTest) yanshimsi@Bach:/mnt/d/GitHub/PybulletCarRobot/bullet3$ git apply ../patch.diff
error: corrupt patch at line 101

L100 if (rollingFriction > 0)
L101 {

[Steven89Liu]

please ignore my old patch, now i find another more reasonable solution:

1. disable all the motor constraints of the Roller by this p.setJointMotorControl2(objUid, linkIndex, p.VELOCITY_CONTROL, force=0) which is a tedious work, so i just change the maxMotorImpulse to 0 when create the default motor constraint in the function PhysicsServerCommandProcessor::createJointMotors() and then rebuilt the engine.
2. disable the cone frition by this p.setPhysicsEngineParameter(enableConeFriction=0).

[yanshil]

That indeed sounds more reasonable.
I am digging into these two settings to see how they effect the rectangle pattern.

I did some test and it looks like, if I got some rectangles, they usually is in a small scale that actually the vehicle is turning almost in the same place. (For example: https://github.com/yanshil/PybulletCarRobot/blob/master/src/test_outputs/simple_cuboid/disableConeFriction_initMaxPulseZero.png)

Currently I didn't find other patterns that is far away from my expectations. Thanks again!