Fix pybullet.calculateInverseDynamics in the case where #dof != #joints (fixed joints). We still don't handle spherical joints in pybullet (even though the underlying inverse dynamics and forward dynamics support it)
Also, add a inverse_dynamics.py example, with URDF files. Thanks to Michiel Lutter for the report and repro case! This fixes issue #1379 https://github.com/bulletphysics/bullet3/issues/1379
This commit is contained in:
erwincoumans
147
examples/pybullet/examples/inverse_dynamics.py
Normal file
147
examples/pybullet/examples/inverse_dynamics.py
Normal file
@@ -0,0 +1,147 @@
|
||||
import pybullet as bullet
|
||||
plot = True
|
||||
|
||||
if (plot):
|
||||
import matplotlib.pyplot as plt
|
||||
import math
|
||||
verbose = False
|
||||
|
||||
# Parameters:
|
||||
robot_base = [0., 0., 0.]
|
||||
robot_orientation = [0., 0., 0., 1.]
|
||||
delta_t = 0.0001
|
||||
|
||||
# Initialize Bullet Simulator
|
||||
id_simulator = bullet.connect(bullet.GUI) # or bullet.DIRECT for non-graphical version
|
||||
bullet.setTimeStep(delta_t)
|
||||
|
||||
# Switch between URDF with/without FIXED joints
|
||||
with_fixed_joints = True
|
||||
|
||||
|
||||
if with_fixed_joints:
|
||||
id_revolute_joints = [0, 3]
|
||||
id_robot = bullet.loadURDF("TwoJointRobot_w_fixedJoints.urdf",
|
||||
robot_base, robot_orientation, useFixedBase=True)
|
||||
else:
|
||||
id_revolute_joints = [0, 1]
|
||||
id_robot = bullet.loadURDF("TwoJointRobot_wo_fixedJoints.urdf",
|
||||
robot_base, robot_orientation, useFixedBase=True)
|
||||
|
||||
|
||||
jointTypeNames = ["JOINT_REVOLUTE", "JOINT_PRISMATIC","JOINT_SPHERICAL","JOINT_PLANAR","JOINT_FIXED","JOINT_POINT2POINT","JOINT_GEAR"]
|
||||
|
||||
# Disable the motors for torque control:
|
||||
bullet.setJointMotorControlArray(id_robot, id_revolute_joints, bullet.VELOCITY_CONTROL, forces=[0.0, 0.0])
|
||||
|
||||
# Target Positions:
|
||||
start = 0.0
|
||||
end = 1.0
|
||||
|
||||
steps = int((end-start)/delta_t)
|
||||
t = [0]*steps
|
||||
q_pos_desired = [[0.]* steps,[0.]* steps]
|
||||
q_vel_desired = [[0.]* steps,[0.]* steps]
|
||||
q_acc_desired = [[0.]* steps,[0.]* steps]
|
||||
|
||||
for s in range(steps):
|
||||
t[s] = start+s*delta_t
|
||||
q_pos_desired[0][s] = 1./(2.*math.pi) * math.sin(2. * math.pi * t[s]) - t[s]
|
||||
q_pos_desired[1][s] = -1./(2.*math.pi) * (math.cos(2. * math.pi * t[s]) - 1.0)
|
||||
|
||||
q_vel_desired[0][s] = math.cos(2. * math.pi * t[s]) - 1.
|
||||
q_vel_desired[1][s] = math.sin(2. * math.pi * t[s])
|
||||
|
||||
q_acc_desired[0][s] = -2. * math.pi * math.sin(2. * math.pi * t[s])
|
||||
q_acc_desired[1][s] = 2. * math.pi * math.cos(2. * math.pi * t[s])
|
||||
|
||||
|
||||
q_pos = [[0.]* steps,[0.]* steps]
|
||||
q_vel = [[0.]* steps,[0.]* steps]
|
||||
q_tor = [[0.]* steps,[0.]* steps]
|
||||
|
||||
# Do Torque Control:
|
||||
for i in range(len(t)):
|
||||
|
||||
# Read Sensor States:
|
||||
joint_states = bullet.getJointStates(id_robot, id_revolute_joints)
|
||||
|
||||
q_pos[0][i] = joint_states[0][0]
|
||||
a = joint_states[1][0]
|
||||
if (verbose):
|
||||
print("joint_states[1][0]")
|
||||
print(joint_states[1][0])
|
||||
q_pos[1][i] = a
|
||||
|
||||
q_vel[0][i] = joint_states[0][1]
|
||||
q_vel[1][i] = joint_states[1][1]
|
||||
|
||||
# Computing the torque from inverse dynamics:
|
||||
obj_pos = [q_pos[0][i], q_pos[1][i]]
|
||||
obj_vel = [q_vel[0][i], q_vel[1][i]]
|
||||
obj_acc = [q_acc_desired[0][i], q_acc_desired[1][i]]
|
||||
|
||||
if (verbose):
|
||||
print("calculateInverseDynamics")
|
||||
print("id_robot")
|
||||
print(id_robot)
|
||||
print("obj_pos")
|
||||
print(obj_pos)
|
||||
print("obj_vel")
|
||||
print(obj_vel)
|
||||
print("obj_acc")
|
||||
print(obj_acc)
|
||||
|
||||
torque = bullet.calculateInverseDynamics(id_robot, obj_pos, obj_vel, obj_acc)
|
||||
q_tor[0][i] = torque[0]
|
||||
q_tor[1][i] = torque[1]
|
||||
if (verbose):
|
||||
print("torque=")
|
||||
print(torque)
|
||||
|
||||
# Set the Joint Torques:
|
||||
bullet.setJointMotorControlArray(id_robot, id_revolute_joints, bullet.TORQUE_CONTROL, forces=[torque[0], torque[1]])
|
||||
|
||||
# Step Simulation
|
||||
bullet.stepSimulation()
|
||||
|
||||
# Plot the Position, Velocity and Acceleration:
|
||||
if plot:
|
||||
figure = plt.figure(figsize=[15, 4.5])
|
||||
figure.subplots_adjust(left=0.05, bottom=0.11, right=0.97, top=0.9, wspace=0.4, hspace=0.55)
|
||||
|
||||
ax_pos = figure.add_subplot(141)
|
||||
ax_pos.set_title("Joint Position")
|
||||
ax_pos.plot(t, q_pos_desired[0], '--r', lw=4, label='Desired q0')
|
||||
ax_pos.plot(t, q_pos_desired[1], '--b', lw=4, label='Desired q1')
|
||||
ax_pos.plot(t, q_pos[0], '-r', lw=1, label='Measured q0')
|
||||
ax_pos.plot(t, q_pos[1], '-b', lw=1, label='Measured q1')
|
||||
ax_pos.set_ylim(-1., 1.)
|
||||
ax_pos.legend()
|
||||
|
||||
ax_vel = figure.add_subplot(142)
|
||||
ax_vel.set_title("Joint Velocity")
|
||||
ax_vel.plot(t, q_vel_desired[0], '--r', lw=4, label='Desired q0')
|
||||
ax_vel.plot(t, q_vel_desired[1], '--b', lw=4, label='Desired q1')
|
||||
ax_vel.plot(t, q_vel[0], '-r', lw=1, label='Measured q0')
|
||||
ax_vel.plot(t, q_vel[1], '-b', lw=1, label='Measured q1')
|
||||
ax_vel.set_ylim(-2., 2.)
|
||||
ax_vel.legend()
|
||||
|
||||
ax_acc = figure.add_subplot(143)
|
||||
ax_acc.set_title("Joint Acceleration")
|
||||
ax_acc.plot(t, q_acc_desired[0], '--r', lw=4, label='Desired q0')
|
||||
ax_acc.plot(t, q_acc_desired[1], '--b', lw=4, label='Desired q1')
|
||||
ax_acc.set_ylim(-10., 10.)
|
||||
ax_acc.legend()
|
||||
|
||||
ax_tor = figure.add_subplot(144)
|
||||
ax_tor.set_title("Executed Torque")
|
||||
ax_tor.plot(t, q_tor[0], '-r', lw=2, label='Torque q0')
|
||||
ax_tor.plot(t, q_tor[1], '-b', lw=2, label='Torque q1')
|
||||
ax_tor.set_ylim(-20., 20.)
|
||||
ax_tor.legend()
|
||||
|
||||
plt.show()
|
||||
|
||||
|
||||
@@ -7222,11 +7222,49 @@ static PyObject* pybullet_calculateInverseDynamics(PyObject* self, PyObject* arg
|
||||
int szObPos = PySequence_Size(objPositionsQ);
|
||||
int szObVel = PySequence_Size(objVelocitiesQdot);
|
||||
int szObAcc = PySequence_Size(objAccelerations);
|
||||
int numJoints = b3GetNumJoints(sm, bodyUniqueId);
|
||||
if (numJoints && (szObPos == numJoints) && (szObVel == numJoints) &&
|
||||
(szObAcc == numJoints))
|
||||
int nj = b3GetNumJoints(sm, bodyUniqueId);
|
||||
int j=0;
|
||||
int dofCountOrg = 0;
|
||||
for (j=0;j<nj;j++)
|
||||
{
|
||||
int szInBytes = sizeof(double) * numJoints;
|
||||
struct b3JointInfo info;
|
||||
b3GetJointInfo(sm, bodyUniqueId, j, &info);
|
||||
switch (info.m_jointType)
|
||||
{
|
||||
case eRevoluteType:
|
||||
{
|
||||
dofCountOrg+=1;
|
||||
break;
|
||||
}
|
||||
case ePrismaticType:
|
||||
{
|
||||
dofCountOrg+=1;
|
||||
break;
|
||||
}
|
||||
case eSphericalType:
|
||||
{
|
||||
PyErr_SetString(SpamError,
|
||||
"Spherirical joints are not supported in the pybullet binding");
|
||||
return NULL;
|
||||
}
|
||||
case ePlanarType:
|
||||
{
|
||||
PyErr_SetString(SpamError,
|
||||
"Planar joints are not supported in the pybullet binding");
|
||||
return NULL;
|
||||
}
|
||||
default:
|
||||
{
|
||||
//fixed joint has 0-dof and at the moment, we don't deal with planar, spherical etc
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
if (dofCountOrg && (szObPos == dofCountOrg) && (szObVel == dofCountOrg) &&
|
||||
(szObAcc == dofCountOrg))
|
||||
{
|
||||
int szInBytes = sizeof(double) * dofCountOrg;
|
||||
int i;
|
||||
PyObject* pylist = 0;
|
||||
double* jointPositionsQ = (double*)malloc(szInBytes);
|
||||
@@ -7234,7 +7272,7 @@ static PyObject* pybullet_calculateInverseDynamics(PyObject* self, PyObject* arg
|
||||
double* jointAccelerations = (double*)malloc(szInBytes);
|
||||
double* jointForcesOutput = (double*)malloc(szInBytes);
|
||||
|
||||
for (i = 0; i < numJoints; i++)
|
||||
for (i = 0; i < dofCountOrg; i++)
|
||||
{
|
||||
jointPositionsQ[i] =
|
||||
pybullet_internalGetFloatFromSequence(objPositionsQ, i);
|
||||
@@ -7263,6 +7301,7 @@ static PyObject* pybullet_calculateInverseDynamics(PyObject* self, PyObject* arg
|
||||
b3GetStatusInverseDynamicsJointForces(statusHandle, &bodyUniqueId,
|
||||
&dofCount, 0);
|
||||
|
||||
|
||||
if (dofCount)
|
||||
{
|
||||
b3GetStatusInverseDynamicsJointForces(statusHandle, 0, 0,
|
||||
@@ -7293,10 +7332,10 @@ static PyObject* pybullet_calculateInverseDynamics(PyObject* self, PyObject* arg
|
||||
else
|
||||
{
|
||||
PyErr_SetString(SpamError,
|
||||
"calculateInverseDynamics numJoints needs to be "
|
||||
"calculateInverseDynamics numDofs needs to be "
|
||||
"positive and [joint positions], [joint velocities], "
|
||||
"[joint accelerations] need to match the number of "
|
||||
"joints.");
|
||||
"degrees of freedom.");
|
||||
return NULL;
|
||||
}
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user