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
182
data/TwoJointRobot_w_fixedJoints.urdf
Normal file
182
data/TwoJointRobot_w_fixedJoints.urdf
Normal 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
110
data/TwoJointRobot_wo_fixedJoints.urdf
Normal 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>
|
||||||
|
|
||||||
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 szObPos = PySequence_Size(objPositionsQ);
|
||||||
int szObVel = PySequence_Size(objVelocitiesQdot);
|
int szObVel = PySequence_Size(objVelocitiesQdot);
|
||||||
int szObAcc = PySequence_Size(objAccelerations);
|
int szObAcc = PySequence_Size(objAccelerations);
|
||||||
int numJoints = b3GetNumJoints(sm, bodyUniqueId);
|
int nj = b3GetNumJoints(sm, bodyUniqueId);
|
||||||
if (numJoints && (szObPos == numJoints) && (szObVel == numJoints) &&
|
int j=0;
|
||||||
(szObAcc == numJoints))
|
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;
|
int i;
|
||||||
PyObject* pylist = 0;
|
PyObject* pylist = 0;
|
||||||
double* jointPositionsQ = (double*)malloc(szInBytes);
|
double* jointPositionsQ = (double*)malloc(szInBytes);
|
||||||
@@ -7234,7 +7272,7 @@ static PyObject* pybullet_calculateInverseDynamics(PyObject* self, PyObject* arg
|
|||||||
double* jointAccelerations = (double*)malloc(szInBytes);
|
double* jointAccelerations = (double*)malloc(szInBytes);
|
||||||
double* jointForcesOutput = (double*)malloc(szInBytes);
|
double* jointForcesOutput = (double*)malloc(szInBytes);
|
||||||
|
|
||||||
for (i = 0; i < numJoints; i++)
|
for (i = 0; i < dofCountOrg; i++)
|
||||||
{
|
{
|
||||||
jointPositionsQ[i] =
|
jointPositionsQ[i] =
|
||||||
pybullet_internalGetFloatFromSequence(objPositionsQ, i);
|
pybullet_internalGetFloatFromSequence(objPositionsQ, i);
|
||||||
@@ -7263,6 +7301,7 @@ static PyObject* pybullet_calculateInverseDynamics(PyObject* self, PyObject* arg
|
|||||||
b3GetStatusInverseDynamicsJointForces(statusHandle, &bodyUniqueId,
|
b3GetStatusInverseDynamicsJointForces(statusHandle, &bodyUniqueId,
|
||||||
&dofCount, 0);
|
&dofCount, 0);
|
||||||
|
|
||||||
|
|
||||||
if (dofCount)
|
if (dofCount)
|
||||||
{
|
{
|
||||||
b3GetStatusInverseDynamicsJointForces(statusHandle, 0, 0,
|
b3GetStatusInverseDynamicsJointForces(statusHandle, 0, 0,
|
||||||
@@ -7293,10 +7332,10 @@ static PyObject* pybullet_calculateInverseDynamics(PyObject* self, PyObject* arg
|
|||||||
else
|
else
|
||||||
{
|
{
|
||||||
PyErr_SetString(SpamError,
|
PyErr_SetString(SpamError,
|
||||||
"calculateInverseDynamics numJoints needs to be "
|
"calculateInverseDynamics numDofs needs to be "
|
||||||
"positive and [joint positions], [joint velocities], "
|
"positive and [joint positions], [joint velocities], "
|
||||||
"[joint accelerations] need to match the number of "
|
"[joint accelerations] need to match the number of "
|
||||||
"joints.");
|
"degrees of freedom.");
|
||||||
return NULL;
|
return NULL;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|||||||
Reference in New Issue
Block a user