avikde
commented
6 years ago BTW I'm setting all the joints to velocity mode, with max force 0 after importing (as suggested in #1026)
Closed avikde closed 6 years ago
avikde
commented
6 years ago BTW I'm setting all the joints to velocity mode, with max force 0 after importing (as suggested in #1026)
erwincoumans
commented
6 years ago It would be easier if you quickly prototype this using pybullet in Python, and once that works, re-implement it using the C-API: this is likely about 10 lines of Python. It is very easy to get the indexing wrong in C (m_uIndex/m_qIndex, joint index etc). Alternatively, provide a simple reproduction case that compiles out-of-the-box (with some main function etc, because scanning C code for mistakes is very time consuming. Also provide the TwoJointRobot_wo_fixedJoints.urdf, or use any of the URDF files in Bullet/data.
It is not recommended to call any reset/b3CreatePoseCommandSetBaseLinearVelocity during the simulation, why is it needed?
setting all the joints to velocity mode, with max force 0
Yes, you need to unlock the joints after loading the URDF, even if you use torque control afterwards.
There are plenty of pybullet examples using torque control. For example, see https://github.com/bulletphysics/bullet3/blob/master/examples/pybullet/examples/inverse_dynamics.py with graphs. Thanks!
avikde
commented
6 years ago Thanks for the very quick response!
#include <map>
#include <string>
#include <chrono>
#include <thread>
#include "SharedMemory/PhysicsClientC_API.h"
#include "Bullet3Common/b3Vector3.h"
#include "Bullet3Common/b3Quaternion.h"
extern const int CONTROL_RATE;
const int CONTROL_RATE = 250;
// Bullet globals
b3PhysicsClientHandle kPhysClient = 0;
const b3Scalar FIXED_TIMESTEP = 1.0/((b3Scalar)CONTROL_RATE);
// temp vars used a lot
b3SharedMemoryCommandHandle command;
b3SharedMemoryStatusHandle statusHandle;
int statusType, ret;
b3JointInfo jointInfo;
b3JointSensorState state;
// test
int twojoint;
std::map< std::string, int > jointNameToId;
int main(int argc, char* argv[]) {
kPhysClient = b3CreateInProcessPhysicsServerAndConnect(argc, argv);
if (!kPhysClient)
return -1;
// visualizer
command = b3InitConfigureOpenGLVisualizer(kPhysClient);
b3ConfigureOpenGLVisualizerSetVisualizationFlags(command, COV_ENABLE_GUI, 0);
b3SubmitClientCommandAndWaitStatus(kPhysClient, command);
b3ConfigureOpenGLVisualizerSetVisualizationFlags(command, COV_ENABLE_SHADOWS, 0);
b3SubmitClientCommandAndWaitStatus(kPhysClient, command);
b3SetTimeOut(kPhysClient, 10);
//syncBodies is only needed when connecting to an existing physics server that has already some bodies
command = b3InitSyncBodyInfoCommand(kPhysClient);
statusHandle = b3SubmitClientCommandAndWaitStatus(kPhysClient, command);
statusType = b3GetStatusType(statusHandle);
// set fixed time step
command = b3InitPhysicsParamCommand(kPhysClient);
ret = b3PhysicsParamSetTimeStep(command, FIXED_TIMESTEP);
statusHandle = b3SubmitClientCommandAndWaitStatus(kPhysClient, command);
ret = b3PhysicsParamSetRealTimeSimulation(command, false);
statusHandle = b3SubmitClientCommandAndWaitStatus(kPhysClient, command);
b3Assert(b3GetStatusType(statusHandle) == CMD_CLIENT_COMMAND_COMPLETED);
// load test
command = b3LoadUrdfCommandInit(kPhysClient, "TwoJointRobot_wo_fixedJoints.urdf");
b3LoadUrdfCommandSetStartPosition(command, -2, 0, 1);
// q.setEulerZYX(0, 0, 0);
// b3LoadUrdfCommandSetStartOrientation(command, q[0], q[1], q[2], q[3]);
b3LoadUrdfCommandSetUseMultiBody(command, true);
statusHandle = b3SubmitClientCommandAndWaitStatus(kPhysClient, command);
statusType = b3GetStatusType(statusHandle);
b3Assert(statusType == CMD_URDF_LOADING_COMPLETED);
if (statusType == CMD_URDF_LOADING_COMPLETED) {
twojoint = b3GetStatusBodyIndex(statusHandle);
}
int numJoints = b3GetNumJoints(kPhysClient, twojoint);
printf("twojoint numjoints = %d\n", numJoints);
// Loop through all joints
for (int i=0; i<numJoints; ++i) {
b3GetJointInfo(kPhysClient, twojoint, i, &jointInfo);
if (jointInfo.m_jointName[0]) {
jointNameToId[std::string(jointInfo.m_jointName)] = i;
} else {
continue;
}
// max force = 0
command = b3JointControlCommandInit2(kPhysClient, twojoint, CONTROL_MODE_VELOCITY);
b3JointControlSetMaximumForce(command, jointInfo.m_uIndex, 0);
statusHandle = b3SubmitClientCommandAndWaitStatus(kPhysClient, command);
}
// loop
unsigned long dtus1 = (unsigned long) 1000000.0*FIXED_TIMESTEP;
double simTimeUS1 = 0;
double zer[3] = {0,0,0};
double q[2], v[2];
while (b3CanSubmitCommand(kPhysClient)) {
simTimeUS1 += dtus1;
// reset base velocity
// apply some torque
b3GetJointInfo(kPhysClient, twojoint, jointNameToId["joint_2"], &jointInfo);
// command = b3JointControlCommandInit2(kPhysClient, twojoint, CONTROL_MODE_VELOCITY);
// b3JointControlSetDesiredVelocity(command, jointInfo.m_uIndex, 1);
command = b3JointControlCommandInit2(kPhysClient, twojoint, CONTROL_MODE_TORQUE);
b3JointControlSetDesiredForceTorque(command, jointInfo.m_uIndex, 200);
// b3JointControlSetMaximumForce(command, jointInfo.m_uIndex, 100);
statusHandle = b3SubmitClientCommandAndWaitStatus(kPhysClient, command);
// get joint values
command = b3RequestActualStateCommandInit(kPhysClient, twojoint);
statusHandle = b3SubmitClientCommandAndWaitStatus(kPhysClient, command);
b3GetJointState(kPhysClient, statusHandle, jointNameToId["joint_1"], &state);
q[0] = state.m_jointPosition;
v[0] = state.m_jointVelocity;
b3GetJointState(kPhysClient, statusHandle, jointNameToId["joint_2"], &state);
q[1] = state.m_jointPosition;
v[1] = state.m_jointVelocity;
statusHandle = b3SubmitClientCommandAndWaitStatus(kPhysClient, b3InitStepSimulationCommand(kPhysClient));
// debugging output
printf("%.3f\t%.3f\t%.3f\t%.3f\t%.3f\n", 0.000001*simTimeUS1, q[0], q[1], v[0], v[1]);
std::this_thread::sleep_for(std::chrono::microseconds(dtus1));
}
b3DisconnectSharedMemory(kPhysClient);
return 0;
}
BTW my goal is just to compare to (e.g.) this simple MATLAB (EDIT: R2017A) code
robot = importrobot('../../../bullet3/data/TwoJointRobot_wo_fixedJoints.urdf');
X0 = zeros(4,1);
[t,X] = ode45(@(t, X) myDyn(t, X, robot), [0,5], X0);
subplot(211)
plot(t, X(:,1:2))
ylabel('pos')
subplot(212)
plot(t, X(:,3:4))
ylabel('vel')
function Xd = myDyn(t, X, robot)
q = X(1:2);
qd = X(3:4);
qdd = forwardDynamics(robot, q, qd, [0;1]);
Xd = [qd;qdd];
end
I'll try the pybullet suggestion but I don't have it installed yet so it will take me a little while -- besides I'm sure my problem is just that I'm not using the API correctly in the first place.
erwincoumans
commented
6 years ago The C-API is very error-prone and verbose, mainly for pybullet and C++ binding.
You can 'fix' the base of the body using
b3LoadUrdfCommandSetUseFixedBase(command,true);
You need to use b3JointControlSetDesiredVelocity together with b3JointControlSetMaximumForce, otherwise the command has no effect. We could remove the if (hasDesiredVelocity) check around line 4492 to make your version work. For now, just add some target velocity:
command = b3JointControlCommandInit2(kPhysClient, twojoint, CONTROL_MODE_VELOCITY);
b3JointControlSetDesiredVelocity(command,jointInfo.m_uIndex,0);
b3JointControlSetMaximumForce(command, jointInfo.m_uIndex, 0);
Also, you may want to disable the default linear and angular damping:
//disable default linear/angular damping
b3SharedMemoryCommandHandle command = b3InitChangeDynamicsInfo(kPhysClient);
double linearDamping = 0;
double angularDamping = 0;
b3ChangeDynamicsInfoSetLinearDamping(command,twojoint, linearDamping);
b3ChangeDynamicsInfoSetAngularDamping(command,twojoint,angularDamping);
statusHandle = b3SubmitClientCommandAndWaitStatus(kPhysClient, command);
Here is a working version. Note that the links move very fast and seem to hit some joint limit soon.
#include <map>
#include <string>
#include <chrono>
#include <thread>
#include "SharedMemory/PhysicsClientC_API.h"
#include "Bullet3Common/b3Vector3.h"
#include "Bullet3Common/b3Quaternion.h"
extern const int CONTROL_RATE;
const int CONTROL_RATE = 250;
// Bullet globals
b3PhysicsClientHandle kPhysClient = 0;
const b3Scalar FIXED_TIMESTEP = 1.0/((b3Scalar)CONTROL_RATE);
// temp vars used a lot
b3SharedMemoryCommandHandle command;
b3SharedMemoryStatusHandle statusHandle;
int statusType, ret;
b3JointInfo jointInfo;
b3JointSensorState state;
// test
int twojoint;
std::map< std::string, int > jointNameToId;
int main(int argc, char* argv[]) {
kPhysClient = b3CreateInProcessPhysicsServerAndConnectMainThread(argc, argv);
if (!kPhysClient)
return -1;
// visualizer
command = b3InitConfigureOpenGLVisualizer(kPhysClient);
b3ConfigureOpenGLVisualizerSetVisualizationFlags(command, COV_ENABLE_GUI, 0);
b3SubmitClientCommandAndWaitStatus(kPhysClient, command);
b3ConfigureOpenGLVisualizerSetVisualizationFlags(command, COV_ENABLE_SHADOWS, 0);
b3SubmitClientCommandAndWaitStatus(kPhysClient, command);
b3SetTimeOut(kPhysClient, 10);
//syncBodies is only needed when connecting to an existing physics server that has already some bodies
command = b3InitSyncBodyInfoCommand(kPhysClient);
statusHandle = b3SubmitClientCommandAndWaitStatus(kPhysClient, command);
statusType = b3GetStatusType(statusHandle);
// set fixed time step
command = b3InitPhysicsParamCommand(kPhysClient);
ret = b3PhysicsParamSetTimeStep(command, FIXED_TIMESTEP);
statusHandle = b3SubmitClientCommandAndWaitStatus(kPhysClient, command);
ret = b3PhysicsParamSetRealTimeSimulation(command, false);
statusHandle = b3SubmitClientCommandAndWaitStatus(kPhysClient, command);
b3Assert(b3GetStatusType(statusHandle) == CMD_CLIENT_COMMAND_COMPLETED);
// load test
command = b3LoadUrdfCommandInit(kPhysClient, "TwoJointRobot_wo_fixedJoints.urdf");
b3LoadUrdfCommandSetStartPosition(command, -2, 0, 1);
b3LoadUrdfCommandSetUseFixedBase(command,true);
// q.setEulerZYX(0, 0, 0);
// b3LoadUrdfCommandSetStartOrientation(command, q[0], q[1], q[2], q[3]);
b3LoadUrdfCommandSetUseMultiBody(command, true);
statusHandle = b3SubmitClientCommandAndWaitStatus(kPhysClient, command);
statusType = b3GetStatusType(statusHandle);
b3Assert(statusType == CMD_URDF_LOADING_COMPLETED);
if (statusType == CMD_URDF_LOADING_COMPLETED) {
twojoint = b3GetStatusBodyIndex(statusHandle);
}
//disable default linear/angular damping
b3SharedMemoryCommandHandle command = b3InitChangeDynamicsInfo(kPhysClient);
double linearDamping = 0;
double angularDamping = 0;
b3ChangeDynamicsInfoSetLinearDamping(command,twojoint, linearDamping);
b3ChangeDynamicsInfoSetAngularDamping(command,twojoint,angularDamping);
statusHandle = b3SubmitClientCommandAndWaitStatus(kPhysClient, command);
int numJoints = b3GetNumJoints(kPhysClient, twojoint);
printf("twojoint numjoints = %d\n", numJoints);
// Loop through all joints
for (int i=0; i<numJoints; ++i) {
// max force = 0
command = b3JointControlCommandInit2(kPhysClient, twojoint, CONTROL_MODE_VELOCITY);
b3JointControlSetDesiredVelocity(command,jointInfo.m_uIndex,0);
b3JointControlSetMaximumForce(command, jointInfo.m_uIndex, 0);
statusHandle = b3SubmitClientCommandAndWaitStatus(kPhysClient, command);
b3GetJointInfo(kPhysClient, twojoint, i, &jointInfo);
if (jointInfo.m_jointName[0]) {
jointNameToId[std::string(jointInfo.m_jointName)] = i;
} else {
continue;
}
}
// loop
unsigned long dtus1 = (unsigned long) 1000000.0*FIXED_TIMESTEP;
double simTimeUS1 = 0;
double zer[3] = {0,0,0};
double q[2], v[2];
while (b3CanSubmitCommand(kPhysClient)) {
simTimeUS1 += dtus1;
// reset base velocity
// apply some torque
b3GetJointInfo(kPhysClient, twojoint, jointNameToId["joint_2"], &jointInfo);
// command = b3JointControlCommandInit2(kPhysClient, twojoint, CONTROL_MODE_VELOCITY);
// b3JointControlSetDesiredVelocity(command, jointInfo.m_uIndex, 1);
command = b3JointControlCommandInit2(kPhysClient, twojoint, CONTROL_MODE_TORQUE);
b3JointControlSetDesiredForceTorque(command, jointInfo.m_uIndex, 200);
// b3JointControlSetMaximumForce(command, jointInfo.m_uIndex, 100);
statusHandle = b3SubmitClientCommandAndWaitStatus(kPhysClient, command);
statusHandle = b3SubmitClientCommandAndWaitStatus(kPhysClient, b3InitStepSimulationCommand(kPhysClient));
// get joint values
command = b3RequestActualStateCommandInit(kPhysClient, twojoint);
statusHandle = b3SubmitClientCommandAndWaitStatus(kPhysClient, command);
b3GetJointState(kPhysClient, statusHandle, jointNameToId["joint_1"], &state);
q[0] = state.m_jointPosition;
v[0] = state.m_jointVelocity;
b3GetJointState(kPhysClient, statusHandle, jointNameToId["joint_2"], &state);
q[1] = state.m_jointPosition;
v[1] = state.m_jointVelocity;
statusHandle = b3SubmitClientCommandAndWaitStatus(kPhysClient, b3InitStepSimulationCommand(kPhysClient));
// debugging output
printf("%.3f\t%.3f\t%.3f\t%.3f\t%.3f\n", 0.000001*simTimeUS1, q[0], q[1], v[0], v[1]);
std::this_thread::sleep_for(std::chrono::microseconds(dtus1));
}
b3DisconnectSharedMemory(kPhysClient);
return 0;
}
That resolves my misuse of the C API, thanks!
I'm now trying to calibrate different simulation parameters (which I'm slowly learning about) so I understand their effect. I ran some simulations and compared to a MATLAB simulation with MaxStep 0.1ms (to try and create a "ground truth" for this simple example).
I varied:
Sorry the plot is a little crowded, but the 'mi' plots are matlab, joint i, 'bi' are from bullet. In general my conclusion is that the timestep is not having a large effect (the 250, 1000 plots almost coincide). The linear/angular damping has some effect, so disabling them seems right. However, there is still some discrepancy with MATLAB. Would you suggest tuning any other parameters?
EDIT: my simulation code is the same as yours, with the last while loop containing this:
while (b3CanSubmitCommand(kPhysClient)) {
simTimeS += 0.000001*dtus1;
// apply some torque
b3GetJointInfo(kPhysClient, twojoint, jointNameToId2["joint_2"], &jointInfo);
command = b3JointControlCommandInit2(kPhysClient, twojoint, CONTROL_MODE_TORQUE);
b3JointControlSetDesiredForceTorque(command, jointInfo.m_uIndex, 0.5 * sin(10*simTimeS));
statusHandle = b3SubmitClientCommandAndWaitStatus(kPhysClient, command);
// get joint values
command = b3RequestActualStateCommandInit(kPhysClient, twojoint);
statusHandle = b3SubmitClientCommandAndWaitStatus(kPhysClient, command);
b3GetJointState(kPhysClient, statusHandle, jointNameToId2["joint_1"], &state);
q[0] = state.m_jointPosition;
v[0] = state.m_jointVelocity;
b3GetJointState(kPhysClient, statusHandle, jointNameToId2["joint_2"], &state);
q[1] = state.m_jointPosition;
v[1] = state.m_jointVelocity;
statusHandle = b3SubmitClientCommandAndWaitStatus(kPhysClient, b3InitStepSimulationCommand(kPhysClient));
// debugging output
printf("%.3f\t%.3f\t%.3f\t%.3f\t%.3f\n", simTimeS, q[0], q[1], v[0], v[1]);
std::this_thread::sleep_for(std::chrono::microseconds(dtus1));
}It is hard to tell where the differences come from. Note that you call 'stepSimulation' after reading the joint values, so there could be 1 frame delay difference between Bullet and MATLAB.
Also, we can implement the calculateForwardDynamics, taking joint angles/velocities as argument, instead of stepping the simulation and changing the state (similar to calculateInverseDynamics).
Bullet loadURDF command ignores the inertia tensor by default, since it is often a bogus value. Instead, it recomputes the inertia based on mass and volume of the collision shape. You can force using the inertia from the URDF file using the flag URDF_USE_INERTIA_FROM_FILE. Even if you use the C-API, it helps reading the pybullet.org documentation, since pybullet uses the same C-API.
int flags = URDF_USE_INERTIA_FROM_FILE;
b3LoadUrdfCommandSetFlags(command,flags);
Thanks, that eliminated that problem!
Next, I will try to make a similar test with a hybrid system (with contacts) and perhaps have similar stupid questions about sim parameters.
Even if you use the C-API, it helps reading the pybullet.org documentation, since pybullet uses the same C-API.
I will do that more. I might start adding doxygen comments to PhysicsClientC_API.h (from the similar functions in the pybullet guide, as well as what I'm learning from these issues or otherwise) so that at least I have a reference for the C API. I can send a PR after it is done in case you want to host on github pages, or I can do it with all the pointers/references to this github repo.
erwincoumans
commented
6 years ago Great! Would it be possible to share the matlab script and way/description to add/generate the Bullet curves? There is also a Pendulum demo and test here: https://github.com/bulletphysics/bullet3/blob/master/examples/MultiBody/Pendulum.cpp It uses the a sPendulumGold version as comparison using this Python script:
#!/usr/bin/env python
from __future__ import print_function
import numpy as np
from scipy import integrate
import pylab as pl
import math
import argparse
theta_pos = 0
dtheta_pos = 1
def pendulum_system(Y, t):
m = 10 # [kg]
g = 9.81 # [m/s^2]
l = 0.5 # [m]
tau = 20 # [Nm]
I = m * pow(l, 2) # inertia
return [Y[dtheta_pos], (tau - m*g*l*math.sin(Y[theta_pos])) / I]
def main():
parser = argparse.ArgumentParser()
parser.add_argument('--plot', action='store_true', default=False, help='Plot results')
args = parser.parse_args()
t = np.arange(0, 2, 0.001) # [s]
theta0 = 0
dtheta0 = 0
theta = integrate.odeint(pendulum_system, [theta0, dtheta0], t) # [rad]
print("timestamp;theta;")
for i in range(theta.shape[0]):
print("%f;%f;" % (t[i], theta[i,theta_pos]))
if args.plot:
pl.plot(t, theta[:,theta_pos], '+')
pl.show()
if __name__ == '__main__':
main()
I'm having some trouble using CONTROL_MODE_TORQUE to command torques to joints with the C API. I'm trying to create a fairly minimal example that can be used to "calibrate" the simulation to ground truth (using a MATLAB/Mathematica simulation). It looks like
TwoJointRobot_wo_fixedJoints.urdfhas all the inertial values populated and can be used for this.My code:
Does this usage look correct? When I use the two lines with
CONTROL_MODE_VELOCITYthe behavior seems as expected (more or less), but when substituting theCONTROL_MODE_TORQUElines there is no motion at all. BTW it seems likeb3JointControlSetDesiredForceTorqueandb3JointControlSetMaximumForcedo exactly the same thing, so I'm just using the former inCONTROL_MODE_TORQUE.