Calibrate the e-puck dynamics3d model

[allsey87]

This issue is for tracking the progress in calibrating the e-puck dynamics3d model as discussed in #146.

[allsey87]

Hey @karthiks1701, I don't think you need to change those offsets/joint rotations. These values are the same as those in the pipuck model from a different repository which works fine.

const btTransform  CDynamics3DPiPuckModel::m_cLeftWheelOffset(btQuaternion(btVector3(-1,0,0), SIMD_HALF_PI), btVector3(0.0, 0.02125, -0.0255));
const btTransform  CDynamics3DPiPuckModel::m_cRightWheelOffset(btQuaternion(btVector3(1,0,0), SIMD_HALF_PI), btVector3(0.0, 0.02125, 0.0255));

I don't want to get into details about all the coordinate systems here, but in a nutshell these transforms are moving the wheel from the center of the model to the sides. Rotating the left wheel 90 degrees around the X axis, and the right wheel -90 degrees around the X axis.

The most important thing here is the following lines of code from dynamics3d_epuck_model.cpp:

m_ptrLeftMotor->setVelocityTarget(m_cWheeledEntity.GetWheelVelocities()[0]);
m_ptrRightMotor->setVelocityTarget(m_cWheeledEntity.GetWheelVelocities()[1]);

We need to match the units between setVelocityTarget and GetWheelVelocities. I suspect one is in cm/s while the other is in radians/sec (or something like that).

Once the units above are correct, I think the values that you need to play with to approximate the dynamics2d model are the following:

const btScalar     CDynamics3DEPuckModel::m_fWheelMotorMaxImpulse(0.5);
const btScalar     CDynamics3DEPuckModel::m_fWheelFriction(0.1);

[karthiks1701]

Yes maybe the units of the two functions are not matching, A simple check of having the same velocities for both the wheels didn't make it move in a straight line ..... To my knowledge both are in cm/sec.

[allsey87]

Hmmm, I think the units of btMultiBodyJointMotor::setVelocityTarget are angular not linear. Regarding the robot not moving in a straight line, have you undone any changes you made to the other constants?

[karthiks1701]

btMultiBodyJointMotor::setVelocityTarget, makes sense to have its units in radians/sec. For the other question, I was mostly playing with the impulses and friction. I did try changing the masses of the wheels and body, which didn't have much effect.(but i don't think that will cause any problem?) So I think I didn't touch other constants(other than the rotation change). I just verified that the other constants are the same as in the repository

[allsey87]

So, what is your current equation for translating the speeds between CWheelEntity::GetWheelVelocities and btMultiBodyJointMotor::setVelocityTarget?

[karthiks1701]

should I do something like CWheelEntity::GetWheelVelocities /(radius_wheel) and then pass it to btMultiBodyJointMotor::setVelocityTarget? If so, it is just a matter of a constant which is changing right?

[jharwell]

I tried making the equations between the pipuck and the epuck match exactly, and changed all the constants in the epuck entity definition to match those of the pipuck, and it still doesn't work :-(. The only substantial difference at this point between the two dynamics3d models and entity definitions (as far as two wheeled motion anyway) that I can see is that the pipuck entity using a CPiPuckDifferentialDriveEntity and the epuck uses CWheeledEntity.

I was able to get things moving by multiplying the target velocity (CWheeledEntity::GetWheelVelocities / (radius_wheel) by 100, which I think is due to the wheeled entity velocities having units of cm/s, and bullet expecting m/s.

This seems really inconsistent to me: most physics engines and human intuition expect things in m/s. Is there a good reason that the velocities managed by CWheeledEntity are in cm/s ?

[karthiks1701]

Even if things move, doesn't it kind of oscillate?.

[jharwell]

Yes I can't get it to do anything other than turn in a circle in place.

[allsey87]

@jharwell as discussed in allsey87/argos3-srocs#23, did you try setting one of the velocities to negative?

This seems really inconsistent to me: most physics engines and human intuition expect things in m/s. Is there a good reason that the velocities managed by CWheeledEntity are in cm/s ?

I strongly agree with this. As a standard in the SRoCS repository, all units for all sensors and actuators in the control interface are in SI units. I also do not negate a motors velocity because it is facing in a different direction. This is because it is not clear to me where this negation should go in the code (both for the simulator and the real robot) and I think it may lead to confusion with other joints.

In my opinion, the units used in simulator or real robot implementation can be an implementation detail, but the interfaces for all robots should be in SI units.

[jharwell]

Yes setting one of the velocities to negative fixed it!

I think this "quirk" also absolutely needs to be documented, as it was not clear to either me or @karthiks1701 that there were differences between how to use the robot interfaces in 2D vs 3D, given current documentation (either in the ARGoS code, examples, or argos -q that I could find.

My understanding of how to move from 2D -> 3D dynamics in ARGoS is that all you need to do is change the physics engine type in the .argos file (this is what I've seen on the forum and on other issues for this repository). However, at least for some robots, you need to be aware that setting wheel speeds to +X,+X in 2D causes the robot to move forward, while in 3D setting wheel speeds to +X,+X makes it spin in a circle. This is true at least for the epuck robot included with ARGoS and the pipuck robot in the SRoCS repository; I don't know about other robots that have 3D models.

I'm happy to create a documentation patch for this, adding necessary bits to the affected robot models and expanding the physics engine docs in ARGoS to call this out. @allsey87 is having the motors pointed in opposite directions for differential drive robots required to get them to work in the Bullit engine, or is it a per-robot design decision (perhaps to better model the real robot) ?

Additionally, @ilpincy Would you be open to merging an interface patch for ARGoS switching all of the units for sensors and actuators from whatever they are to m/s, radians/s, etc. ? I can update all the docs as well. Personally, I think it would greatly improve ARGoS usability and intuitiveness for those trying out ARGoS/unfamiliar with its inner workings. I'm fairly experienced with ARGoS, but even I recently had to spend half an hour tracking down why my algorithm using the colored blob camera wasn't working, which ultimately turned out to be because I thought the distances it returned were in meters, but they were in centimeters.

[karthiks1701]

I think when I tried to change the wheel constants, I tried to change the motor directions to rotate in the same directions. It is better but still has problems with respect to sticking to the assigned directions. Isn't this same as setting one of the wheels as negative?. Also as @jharwell asked does it require the 3D physics engine to have opposite rotating directions?

[allsey87]

@jharwell the simulation models that I am working on have real robot counterparts for which I need a consistent control interface. Since it is usually the case that the motors on most differential drive robots point in opposite directions, having one speed being negative to move forward is the do-nothing-special solution.

I could change this, but I would like @ilpincy to weigh in on this and on the use of SI units in the control interface before I change anything.

[ilpincy]

Additionally, @ilpincy Would you be open to merging an interface patch for ARGoS switching all of the units for sensors and actuators from whatever they are to m/s, radians/s, etc. ?

The interface already uses radians, so that should not be an issue. The choice of using cm instead of meters in the control interface was made early on in the design. The reason was that most robots have fairly good performance with float, but quite bad with double. To ensure numerical stability and to fit the scale at which robots operate, we decided to work with cm. Changing this would break literally every controller ever made with ARGoS, so I don't think it's a feasible option.

[ilpincy]

My understanding of how to move from 2D -> 3D dynamics in ARGoS is that all you need to do is change the physics engine type in the .argos file (this is what I've seen on the forum and on other issues for this repository). However, at least for some robots, you need to be aware that setting wheel speeds to +X,+X in 2D causes the robot to move forward, while in 3D setting wheel speeds to +X,+X makes it spin in a circle. This is true at least for the epuck robot included with ARGoS and the pipuck robot in the SRoCS repository; I don't know about other robots that have 3D models.

The control interface of the robots should be fully compatible. Which physics engine a user picks should have no consequences on the controller. If now we require the speeds to be +X,-X in the 3D engine, that's a bug.

[ilpincy]

@jharwell the simulation models that I am working on have real robot counterparts for which I need a consistent control interface. Since it is usually the case that the motors on most differential drive robots point in opposite directions, having one speed being negative to move forward is the do-nothing-special solution.

When I made the ARGoS control interface of the foot-bot, I made it so both speeds being positive corresponded to the forward direction, because that's the intuitive way to think about it (both wheels share the same axle in the differential drive model!). The negative sign was passed in the call to the native robot API, but not visible from the ARGoS control interface.

[allsey87]

The reason was that most robots have fairly good performance with float, but quite bad with double. To ensure numerical stability and to fit the scale at which robots operate, we decided to work with cm.

I understand not changing this for historical reasons, although the logic here was wrong. Floating-point numbers can represent and operate on 2340, 2.34, 0.00234 with equal precision/stability.

The control interface of the robots should be fully compatible. Which physics engine a user picks should have no consequences on the controller. If now we require the speeds to be +X,-X in the 3D engine, that's a bug.

When I made the ARGoS control interface of the foot-bot, I made it so both speeds being positive corresponded to the forward direction, because that's the intuitive way to think about it (both wheels share the same axle in the differential drive model!). The negative sign was passed in the call to the native robot API, but not visible from the ARGoS control interface.

I am ok with changing this. I agree that it is more intuitive that two positive values make the robot go forward, but I feel that that initial intuition is wrong and causes more problems when you start working with more complicated robots. That been said, I understand that robots with lots of joints etc. is outside of the scope of ARGoS, so I will change this.

[jharwell]

@ilpincy , for SI units in robot controllers, could we do something like ARGOS_CONTROL_INTERFACE_USE_SI_UNITS as a cmake define, have it be disabled by default, preserving the current behavior, but allowing those who work primarily in simulation and/or have robots which have sufficient floating point accuracy to work with double to apply a constant to convert cm/s to m/s ?

@allsey87 I understand your point about wanting to preserve realism/how the actual robot model works as closely as possible. For my own two cents, as long as the current behavior is documented in the 3D model of a robot, and a section like "Considerations in 2D vs. 3D" is added to the results of argos3 -q <...> for all affected robots, I think the current behavior is OK. Perhaps this could be made another cmake define, allowing the user building ARGoS to choose between intuition and model real robots as closely as possible ?

[ilpincy]

Compiling ARGoS conditionally to support breaking changes would create the issue of sharing work between people. I don't think it's a good idea to change the units, but if we had to do it, it should be a flag in the .argos file. Still, this would make the controller code not shareable. Honestly, I am not willing to support this kind of breaking changes. I have never received complaints about this, and I worry about breaking existing work.

As for the wheeled robot model: the focus is not realism with respect to the hardware, but the principle of least surprise. The most direct way to control a wheeled robot is with the differential drive model. That is the mental model ARGoS follows, so that should be the implementation in the 3D dynamics engine. As for manipulators, I am open to discuss which model we want to follow (I don't know much about manipulators, so I would follow your lead).

[jharwell]

Ah that makes sense--I had not thought about the sharing aspect.

[allsey87]

I agree with the point about not using a compilation flag to modify how the control interface works. While I have pretty strong opinions about how X should be done, I want to emphasise that I am actually quite flexible regarding the final decision. At the end of the day, ARGoS has accumulated quite a few users and I agree that we should optimise for making their lives as easy as possible.

I think the way forward is to document each robots control interface, including any units or quirks. Also, I have nothing against changing the epucks model's wheels so that two postive values makes it drive forwards :)