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>
|
||||
|
||||
Reference in New Issue
Block a user