From cb23e6c102f13702686b3322fcfab186ccb827cf Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Fri, 13 Oct 2017 14:19:25 -0700 Subject: [PATCH 1/2] 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 --- data/TwoJointRobot_w_fixedJoints.urdf | 182 ++++++++++++++++++ data/TwoJointRobot_wo_fixedJoints.urdf | 110 +++++++++++ .../pybullet/examples/inverse_dynamics.py | 147 ++++++++++++++ examples/pybullet/pybullet.c | 53 ++++- 4 files changed, 485 insertions(+), 7 deletions(-) create mode 100644 data/TwoJointRobot_w_fixedJoints.urdf create mode 100644 data/TwoJointRobot_wo_fixedJoints.urdf create mode 100644 examples/pybullet/examples/inverse_dynamics.py diff --git a/data/TwoJointRobot_w_fixedJoints.urdf b/data/TwoJointRobot_w_fixedJoints.urdf new file mode 100644 index 000000000..4053835eb --- /dev/null +++ b/data/TwoJointRobot_w_fixedJoints.urdf @@ -0,0 +1,182 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/data/TwoJointRobot_wo_fixedJoints.urdf b/data/TwoJointRobot_wo_fixedJoints.urdf new file mode 100644 index 000000000..bd0dec215 --- /dev/null +++ b/data/TwoJointRobot_wo_fixedJoints.urdf @@ -0,0 +1,110 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/examples/pybullet/examples/inverse_dynamics.py b/examples/pybullet/examples/inverse_dynamics.py new file mode 100644 index 000000000..fecb4ad64 --- /dev/null +++ b/examples/pybullet/examples/inverse_dynamics.py @@ -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() + + \ No newline at end of file diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index b1df18ce0..508e6c461 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -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 Date: Fri, 13 Oct 2017 14:51:06 -0700 Subject: [PATCH 2/2] fix blocking plot.show() never terminating properly --- examples/pybullet/examples/inverse_dynamics.py | 10 +++++++--- setup.py | 2 +- 2 files changed, 8 insertions(+), 4 deletions(-) diff --git a/examples/pybullet/examples/inverse_dynamics.py b/examples/pybullet/examples/inverse_dynamics.py index fecb4ad64..e6af3011b 100644 --- a/examples/pybullet/examples/inverse_dynamics.py +++ b/examples/pybullet/examples/inverse_dynamics.py @@ -1,5 +1,6 @@ import pybullet as bullet plot = True +import time if (plot): import matplotlib.pyplot as plt @@ -141,7 +142,10 @@ if plot: ax_tor.plot(t, q_tor[1], '-b', lw=2, label='Torque q1') ax_tor.set_ylim(-20., 20.) ax_tor.legend() - - plt.show() - \ No newline at end of file + plt.pause(0.01) + + +while (1): + bullet.stepSimulation() + time.sleep(0.01) diff --git a/setup.py b/setup.py index 4362b92a3..04e3c4cae 100644 --- a/setup.py +++ b/setup.py @@ -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',