Fixed joint constraint pendulum problem

[francevezzi]

Hello everyone,

My aim is to create a simple mid-air suspended oscillating pendulum.
Here is the URDF code:

Code: Select all

<?xml version="1.0" ?>

<robot name="pendolo">

 <link name="base">
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <box size="0.001 0.001 0.001"/>
      </geometry>
    </visual>
    <!-- added so pybullet doesn't complain and set mass 1 diagintertia 1-->
    <inertial>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <mass value="0.00001"/>
      <inertia ixx="1e-5" ixy="0" ixz="0" iyy="1e-5" iyz="0" izz="1e-5"/>
    </inertial>
  </link>
  <joint name="floating_base" type="fixed">
    <origin rpy="0 0 0" xyz="0 0 0"/>
    <parent link="base"/>
    <child link="supporto"/>
  </joint>
  <link name="supporto">
      <inertial>
          <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
          <mass value="0.001"/>
          <inertia ixx="0.0001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.0001"/>
      </inertial>
      <visual name="">
          <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
          <geometry>
              <box size="0.0 0.0 0.0"/>
          </geometry>
          <material name="">
              <color rgba="1.0 0.0 0.0 1.0"/>
              <texture filename=""/>
          </material>
      </visual>
      <collision>
        <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
          <geometry>
              <box size="0.0001 0.0001 0.0001"/>
          </geometry>
      </collision>
  </link>  
        <joint name="pendulum_joint" type="revolute">
        <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
        <parent link="supporto"/>
        <child link="sbarra"/>
        <axis xyz="0.0 1.0 0.0"/>
        <limit lower="-10.0" upper="10.0" effort="0.0" velocity="0.0"/>
    </joint>
  <link name="sbarra">
      <inertial>
          <origin xyz="0.0 0.0 -0.5" rpy="0.0 0.0 0.0"/>
          <mass value="1.0"/>
          <inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001"/>
      </inertial>
      <visual name="">
          <origin xyz="0.0 0.0 -0.5" rpy="0.0 0.0 0.0"/>
          <geometry>
              <box size="0.1 0.1 1"/>
          </geometry>
          <material name="">
              <color rgba="1.0 0.0 0.0 1.0"/>
              <texture filename=""/>
          </material>
      </visual>
      <collision>
        <origin xyz="0.0 0.0 -0.5" rpy="0.0 0.0 0.0"/>
          <geometry>
              <box size="0.1 0.1 1"/>
          </geometry>
      </collision>
  </link>
  </robot>

If I use a constraint to fix the joint base to be in a certain position it works and the pendulum is suspended exactly where I want.
Now if I use

Code: Select all

 resetJointState

to change pendulum initial angle it starts to oscillate in an unstable way, even with no gravity. Besides the pendulum rotate on further axis.
Here is the code to reproduce the issue:

Code: Select all

import pybullet
import time
import pybullet_data
import pybullet_utils.bullet_client as bc
import numpy as np

p = bc.BulletClient(connection_mode=pybullet.GUI)
p.setAdditionalSearchPath(pybullet_data.getDataPath())
# p.setGravity(0,0,-10)
planeId = p.loadURDF("plane.urdf")
startPos = [0,0,1.5]
startOrientation = p.getQuaternionFromEuler([0,0,0])
pendId = p.loadURDF("pendulum.urdf", startPos, startOrientation)
p.resetJointState(pendId, 1, -np.pi/4, 0)
p.createConstraint(pendId, -1, -1, -1,
                   p.JOINT_FIXED,
                   [0,0,0],
                   [0,0,0],
                   [0,0,2],
                   childFrameOrientation=[0,0,0,1])
for i in range (100000):
    p.stepSimulation()
    time.sleep(1./240.)

p.disconnect()

Besides, if the position of the constrain is [0,0,1.5] exactly as the initial one of the pendulum the system behaves as desired.

Can anyone explain why this weird behavior ?