AuroraMiddleware/bullet3
1
0
Fork 0
mirror of https://github.com/bulletphysics/bullet3 synced 2024-12-14 13:50:04 +00:00
Code Issues Releases Wiki Activity

Merge pull request #1381 from erwincoumans/master

Browse Source 
Fix pybullet.calculateInverseDynamics in the case where #dof != #join…
This commit is contained in:
erwincoumans 2017-10-13 15:19:04 -07:00 committed by GitHub
commit 0a9cf254f2
5 changed files with 490 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

182
data/TwoJointRobot_w_fixedJoints.urdf Normal file
View File

@ -0,0 +1,182 @@
<?xml version="1.0" ?>
<!-- =================================================================================== -->
<!-- | This document was autogenerated by xacro from TwoJointRobot.xacro | -->
<!-- | EDITING THIS FILE BY HAND IS NOT RECOMMENDED | -->
<!-- =================================================================================== -->
<robot name="TwoJointRobot" xmlns:xacro="http://www.ros.org/wiki/xacro">
<material name="blue">
<color rgba="0 0 0.8 1"/>
</material>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
<link name="base_link">
<visual>
<geometry>
<cylinder length="0.05" radius="0.075"/>
</geometry>
<origin rpy="0 0 0" xyz="0 0 0.025"/>
</visual>
<collision>
<geometry>
<cylinder length="0.05" radius="0.075"/>
</geometry>
<origin rpy="0 0 0" xyz="0 0 0.025"/>
</collision>
<inertial>
<mass value="0.2"/>
<inertia ixx="0.000322916666667" ixy="0.0" ixz="0.0" iyy="0.000322916666667" iyz="0.0" izz="0.0005625"/>
</inertial>
</link>
<link name="link_01_cyl">
<visual>
<geometry>
<cylinder length="0.05" radius="0.075"/>
</geometry>
<origin rpy="0 0 0" xyz="0 0 0"/>
<material name="blue"/>
</visual>
<collision>
<geometry>
<cylinder length="0.05" radius="0.075"/>
</geometry>
<origin rpy="0 0 0" xyz="0 0 0"/>
</collision>
<inertial>
<mass value="0.2"/>
<inertia ixx="0.000322916666667" ixy="0.0" ixz="0.0" iyy="0.000322916666667" iyz="0.0" izz="0.0005625"/>
</inertial>
</link>
<link name="link_12_cyl">
<visual>
<geometry>
<cylinder length="0.05" radius="0.075"/>
</geometry>
<origin rpy="0 0 0" xyz="0 0 0"/>
<material name="blue"/>
</visual>
<collision>
<geometry>
<cylinder length="0.05" radius="0.075"/>
</geometry>
<origin rpy="0 0 0" xyz="0 0 0"/>
</collision>
<inertial>
<mass value="0.2"/>
<inertia ixx="0.000322916666667" ixy="0.0" ixz="0.0" iyy="0.000322916666667" iyz="0.0" izz="0.0005625"/>
</inertial>
</link>
<link name="link_21_cyl">
<visual>
<geometry>
<cylinder length="0.05" radius="0.075"/>
</geometry>
<origin rpy="0 0 0" xyz="0 0 0"/>
<material name="white"/>
</visual>
<collision>
<geometry>
<cylinder length="0.05" radius="0.075"/>
</geometry>
<origin rpy="0 0 0" xyz="0 0 0"/>
</collision>
<inertial>
<mass value="0.2"/>
<inertia ixx="0.000322916666667" ixy="0.0" ixz="0.0" iyy="0.000322916666667" iyz="0.0" izz="0.0005625"/>
</inertial>
</link>
<link name="link_23_cyl">
<visual>
<geometry>
<cylinder length="0.05" radius="0.075"/>
</geometry>
<origin rpy="0 0 0" xyz="0 0 0"/>
<material name="white"/>
</visual>
<collision>
<geometry>
<cylinder length="0.05" radius="0.075"/>
</geometry>
<origin rpy="0 0 0" xyz="0 0 0"/>
</collision>
<inertial>
<mass value="0.2"/>
<inertia ixx="0.000322916666667" ixy="0.0" ixz="0.0" iyy="0.000322916666667" iyz="0.0" izz="0.0005625"/>
</inertial>
</link>
<link name="link_1">
<visual>
<geometry>
<box size="1.0 0.1 0.05"/>
</geometry>
<origin rpy="0 0 0" xyz="0.5 0 0"/>
<material name="blue"/>
</visual>
<collision>
<geometry>
<box size="1.0 0.1 0.05"/>
</geometry>
<origin rpy="0 0 0" xyz="0.5 0 0"/>
</collision>
<inertial>
<mass value="0.5"/>
<inertia ixx="0.00208333333333" ixy="0.0125" ixz="0.00625" iyy="0.167083333333" iyz="0.000625" izz="0.168333333333"/>
</inertial>
</link>
<link name="link_2">
<visual>
<geometry>
<box size="1.0 0.1 0.05"/>
</geometry>
<origin rpy="0 0 0" xyz="0.5 0 0"/>
<material name="white"/>
</visual>
<collision>
<geometry>
<box size="1.0 0.1 0.05"/>
</geometry>
<origin rpy="0 0 0" xyz="0.5 0 0"/>
</collision>
<inertial>
<mass value="0.5"/>
<inertia ixx="0.00208333333333" ixy="0.0125" ixz="0.00625" iyy="0.167083333333" iyz="0.000625" izz="0.168333333333"/>
</inertial>
</link>
<joint name="link_01" type="fixed">
<origin rpy="0 0 0" xyz="0 0 0"/>
<parent link="link_01_cyl"/>
<child link="link_1"/>
</joint>
<joint name="link_12" type="fixed">
<origin rpy="0 0 0" xyz="1.0 0 0"/>
<parent link="link_1"/>
<child link="link_12_cyl"/>
</joint>
<joint name="link_21" type="fixed">
<origin rpy="0 0 0" xyz="0 0 0"/>
<parent link="link_21_cyl"/>
<child link="link_2"/>
</joint>
<joint name="link_23" type="fixed">
<origin rpy="0 0 0" xyz="1.0 0 0"/>
<parent link="link_2"/>
<child link="link_23_cyl"/>
</joint>
<joint name="joint_1" type="revolute">
<axis xyz="0 0 1"/>
<limit effort="10000" lower="-3" upper="+3" velocity="5"/>
<dynamics damping="0" friction="0"/>
<origin rpy="0 0 0" xyz="0 0 0.075"/>
<parent link="base_link"/>
<child link="link_01_cyl"/>
</joint>
<joint name="joint_2" type="revolute">
<axis xyz="0 0 1"/>
<limit effort="10000" lower="-3" upper="+3" velocity="5"/>
<dynamics damping="0" friction="0"/>
<origin rpy="0 0 0" xyz="0.0 0. 0.05"/>
<parent link="link_12_cyl"/>
<child link="link_21_cyl"/>
</joint>
</robot>

110
data/TwoJointRobot_wo_fixedJoints.urdf Normal file
View File

@ -0,0 +1,110 @@
<?xml version="1.0" ?>
<!-- =================================================================================== -->
<!-- | This document was autogenerated by xacro from TwoJointRobot_woCyl.xacro | -->
<!-- | EDITING THIS FILE BY HAND IS NOT RECOMMENDED | -->
<!-- =================================================================================== -->
<robot name="TwoJointRobot" xmlns:xacro="http://www.ros.org/wiki/xacro">
<material name="blue">
<color rgba="0 0 0.8 1"/>
</material>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
<link name="base_link">
<visual>
<geometry>
<cylinder length="0.05" radius="0.075"/>
</geometry>
<origin rpy="0 0 0" xyz="0 0 0.025"/>
</visual>
<collision>
<geometry>
<cylinder length="0.05" radius="0.075"/>
</geometry>
<origin rpy="0 0 0" xyz="0 0 0.025"/>
</collision>
<inertial>
<mass value="0.2"/>
<inertia ixx="0.000322916666667" ixy="0.0" ixz="0.0" iyy="0.000322916666667" iyz="0.0" izz="0.0005625"/>
</inertial>
</link>
<link name="link_23_cyl">
<visual>
<geometry>
<cylinder length="0.05" radius="0.075"/>
</geometry>
<origin rpy="0 0 0" xyz="0 0 0"/>
<material name="white"/>
</visual>
<collision>
<geometry>
<cylinder length="0.05" radius="0.075"/>
</geometry>
<origin rpy="0 0 0" xyz="0 0 0"/>
</collision>
<inertial>
<mass value="0.2"/>
<inertia ixx="0.000322916666667" ixy="0.0" ixz="0.0" iyy="0.000322916666667" iyz="0.0" izz="0.0005625"/>
</inertial>
</link>
<link name="link_1">
<visual>
<geometry>
<box size="1.0 0.1 0.05"/>
</geometry>
<origin rpy="0 0 0" xyz="0.5 0 0"/>
<material name="blue"/>
</visual>
<collision>
<geometry>
<box size="1.0 0.1 0.05"/>
</geometry>
<origin rpy="0 0 0" xyz="0.5 0 0"/>
</collision>
<inertial>
<mass value="0.5"/>
<inertia ixx="0.00208333333333" ixy="0.0125" ixz="0.00625" iyy="0.167083333333" iyz="0.000625" izz="0.168333333333"/>
</inertial>
</link>
<link name="link_2">
<visual>
<geometry>
<box size="1.0 0.1 0.05"/>
</geometry>
<origin rpy="0 0 0" xyz="0.5 0 0"/>
<material name="white"/>
</visual>
<collision>
<geometry>
<box size="1.0 0.1 0.05"/>
</geometry>
<origin rpy="0 0 0" xyz="0.5 0 0"/>
</collision>
<inertial>
<mass value="0.5"/>
<inertia ixx="0.00208333333333" ixy="0.0125" ixz="0.00625" iyy="0.167083333333" iyz="0.000625" izz="0.168333333333"/>
</inertial>
</link>
<joint name="joint_1" type="revolute">
<axis xyz="0 0 1"/>
<limit effort="10000" lower="-3.14" upper="3.14" velocity="5"/>
<dynamics damping="0" friction="0"/>
<origin rpy="0 0 0" xyz="0 0 0.075"/>
<parent link="base_link"/>
<child link="link_1"/>
</joint>
<joint name="joint_2" type="revolute">
<axis xyz="0 0 1"/>
<limit effort="10000" lower="-3.14" upper="3.14" velocity="5"/>
<dynamics damping="0" friction="0"/>
<origin rpy="0 0 0" xyz="1.0 0. 0.05"/>
<parent link="link_1"/>
<child link="link_2"/>
</joint>
<joint name="link_23" type="fixed">
<origin rpy="0 0 0" xyz="1.0 0 0"/>
<parent link="link_2"/>
<child link="link_23_cyl"/>
</joint>
</robot>

151
examples/pybullet/examples/inverse_dynamics.py Normal file
View File

@ -0,0 +1,151 @@
import pybullet as bullet
plot = True
import time
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.pause(0.01)
while (1):
bullet.stepSimulation()
time.sleep(0.01)

53
examples/pybullet/pybullet.c
View File

@ -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;
}
}

2
setup.py
View File

@ -441,7 +441,7 @@ print("-----")
setup(
name = 'pybullet',
version='1.5.7',
version='1.5.8',
description='Official Python Interface for the Bullet Physics SDK specialized for Robotics Simulation and Reinforcement Learning',
long_description='pybullet is an easy to use Python module for physics simulation, robotics and deep reinforcement learning based on the Bullet Physics SDK. With pybullet you can load articulated bodies from URDF, SDF and other file formats. pybullet provides forward dynamics simulation, inverse dynamics computation, forward and inverse kinematics and collision detection and ray intersection queries. Aside from physics simulation, pybullet supports to rendering, with a CPU renderer and OpenGL visualization and support for virtual reality headsets.',
url='https://github.com/bulletphysics/bullet3',