Arm not affected by gravity

[nathaniel]

Hi all, a beginner here.

I've created a basic URDF file containing a box with an "arm" attached to it by a revolute joint. The quick start guide says I can disable the default motor by setting the joint to a velocity controller with a force of 0. So my expectation is that on running the below code, the robot should fall from its starting point and its arm should flop to the ground:

Code: Select all

import pybullet as p
import time
import pybullet_data

physicsClient = p.connect(p.GUI)#or p.DIRECT for non-graphical version
p.setAdditionalSearchPath(pybullet_data.getDataPath())
p.setGravity(0,0,-10)
planeId = p.loadURDF("plane.urdf")
startPos = [0,0,1]
startOrientation = p.getQuaternionFromEuler([0,0,0])

robot = p.loadURDF("./testbot.urdf",startPos, startOrientation)

for i in range(p.getNumJoints(robot)):
	p.setJointMotorControl2(robot, i, p.VELOCITY_CONTROL, force=0)

for i in range (10000):
	p.stepSimulation()
	time.sleep(1./480.)
p.disconnect()

However, instead the arm stays rigidly in place, not moving relative to the box when the robot hits the floor. At first I thought that disabling the motor wasn't working, but I found I can move the arm using the mouse. It behaves quite strangely, and it seems like it's not being affected by gravity - if I give it a small amount of motion upwards it will continue moving at a constant angular velocity until it hits its limits.

Am I doing something wrong here?

Here are the contents of testbot.urdf:

Code: Select all

<?xml version="1.0"?>
<robot name="testbot">
  <link name="body">
    <visual>
      <geometry>
        <box size="1 0.7 0.4" />
      </geometry>
      <origin xyz="0 0 0" rpy="0 0 0" />
      <material name="default_material">
        <color rgba="0.8 0.8 0.8 1.0" />
      </material>
    </visual>
    <collision>
      <geometry>
        <box size="1 0.7 0.4" />
      </geometry>
      <origin xyz="0 0 0" rpy="0 0 0" />
    </collision>
    <inertial>
      <mass value="10" />
      <inertia ixx="0.5416666666666666" ixy="0" ixz="0" iyy="0.9666666666666668" iyz="0" izz="1.2416666666666667" />
    </inertial>
  </link>
  <link name="arm1">
    <visual>
      <geometry>
        <box size="0.55 0.2 0.1" />
      </geometry>
      <origin xyz="0.25 0.0 0.0" rpy="0 0 0" />
      <material name="default_material">
        <color rgba="0.8 0.8 0.8 1.0" />
      </material>
    </visual>
    <collision>
      <geometry>
        <box size="0.55 0.2 0.1" />
      </geometry>
      <origin xyz="0.25 0.0 0.0" rpy="0 0 0" />
    </collision>
    <inertial>
      <mass value="2" />
      <inertia ixx="0.008333333333333335" ixy="0" ixz="0" iyy="0.05208333333333334" iyz="0" izz="0.05708333333333334" />
    </inertial>
  </link>
  <joint name="shoulder1" type="revolute">
    <parent link="body" />
    <child link="arm1" />
    <axis xyz="0 1 0" />
    <limit lower="-2.199114857512855" upper="2.199114857512855" effort="1000.0" velocity="1.0" />
    <origin xyz="0.5 0 0" rpy="0 0 0" />
  </joint>
</robot>

[nathaniel]

Ah, I've solved this I think. It seems the <inertial> tag also needs an origin. I've added one with the same values as the origin in the <collision> and <visual> tags, and it now seems to behave as I would expect.

However, I'm now unsure whether my moments of inertia are correct. The documentation at http://wiki.ros.org/urdf/XML/link discusses it, but it's pretty terse and I'm not sure if I've understood correctly what it means. Do I understand correctly that if my link is a basic shape like box, cylinder etc. whose centre of gravity is at the origin, then I can just calculate my moment of inertia around the origin, and then copy the <origin> tag from <collision> into <inertial> and it should work?

Anyway, for completeness, here's the probably-corrected testbot.urdf file:

Code: Select all

<?xml version="1.0"?>
<robot name="testbot">
  <link name="body">
    <visual>
      <geometry>
        <box size="1 0.7 0.4" />
      </geometry>
      <origin xyz="0 0 0" rpy="0 0 0" />
      <material name="default_material">
        <color rgba="0.8 0.8 0.8 1.0" />
      </material>
    </visual>
    <collision>
      <geometry>
        <box size="1 0.7 0.4" />
      </geometry>
      <origin xyz="0 0 0" rpy="0 0 0" />
    </collision>
    <inertial>
      <mass value="10" />
      <origin xyz="0 0 0" rpy="0 0 0" />
      <inertia ixx="0.5416666666666666" ixy="0" ixz="0" iyy="0.9666666666666668" iyz="0" izz="1.2416666666666667" />
    </inertial>
  </link>
  <link name="arm1">
    <visual>
      <geometry>
        <box size="0.5 0.2 0.1" />
      </geometry>
      <origin xyz="0.27 0.0 0.0" rpy="0 0 0" />
      <material name="default_material">
        <color rgba="0.8 0.8 0.8 1.0" />
      </material>
    </visual>
    <collision>
      <geometry>
        <box size="0.5 0.2 0.1" />
      </geometry>
      <origin xyz="0.27 0.0 0.0" rpy="0 0 0" />
    </collision>
    <inertial>
      <mass value="2" />
      <origin xyz="0.27 0.0 0.0" rpy="0 0 0" />
      <inertia ixx="0.008333333333333335" ixy="0" ixz="0" iyy="0.043333333333333335" iyz="0" izz="0.04833333333333334" />
    </inertial>
  </link>
  <joint name="shoulder1" type="revolute">
    <parent link="body" />
    <child link="arm1" />
    <axis xyz="0 1 0" />
    <limit lower="-2.199114857512855" upper="2.199114857512855" effort="1000.0" velocity="1.0" />
    <origin xyz="0.5 0 0" rpy="0 0 0" />
  </joint>
</robot>