Gregorio Marchesini
Gregorio Marchesini

Reputation: 11

Custom Pybullet cartpole example results in weird torque control behavior

I have a problem simulating a custom pybullet example I created for simulating a cart pole system. I have created the following cartpole urdf file

<?xml version="1.0" ?>
<!-- =================================================================================== -->
<!-- |    This document was autogenerated by xacro from cartpole.urdf.xacro            | -->
<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
<!-- =================================================================================== -->
<robot name="physics">
  <!--  xacro file that defines the properties of the cartpole -->
  <!-- find useful xacro commands here : https://wiki.ros.org/urdf/Tutorials/Using%20Xacro%20to%20Clean%20Up%20a%20URDF%20File -->
  <link name="slideBar">
    <visual>
      <geometry>
        <box size="30 0.05 0.05"/>
      </geometry>
      <origin xyz="0 0 0"/>
      <material name="green">
        <color rgba="0 0.8 .8 1"/>
      </material>
    </visual>
    <inertial>
      <mass value="0"/>
      <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
    </inertial>
  </link>
  <link name="cart">
    <visual>
      <geometry>
        <box size="0.5 0.5 0.2"/>
      </geometry>
      <origin xyz="0 0 0"/>
      <material name="blue">
        <color rgba="0 0 .8 1"/>
      </material>
    </visual>
    <collision>
      <geometry>
        <box size="0.5 0.5 0.2"/>
      </geometry>
      <origin xyz="0 0 0"/>
    </collision>
    <inertial>
      <mass value="10"/>
      <inertia ixx="0.24166666666666667" ixy="0.0" ixz="0.0" iyy="0.24166666666666667" iyz="0.0" izz="0.41666666666666663"/>
    </inertial>
  </link>
  <!-- Place the pole visual standing at the origin -->
  <link name="pole">
    <visual>
      <geometry>
        <box size="0.05 0.05 2.0"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 1.22"/>
      <material name="white">
        <color rgba="1 1 1 1"/>
      </material>
    </visual>
    <inertial>
      <origin rpy="0 0 0" xyz="0 0 1.22"/>
      <mass value="0.1"/>
      <inertia ixx="pole_I" ixy="0.0" ixz="0.0" iyy="pole_I" iyz="0.0" izz="{pole_Iz}"/>
    </inertial>
    <collision>
      <geometry>
        <box size="0.05 0.05 1.0"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 0.5"/>
    </collision>
  </link>
  <!-- Ball At the top of the pole-->
  <link name="ball">
    <visual>
      <geometry>
        <sphere radius="0.2"/>
      </geometry>
      <material name="blue">
        <color rgba="0 0 .8 1"/>
      </material>
    </visual>
    <collision>
      <geometry>
        <sphere radius="2.0"/>
      </geometry>
      <origin xyz="0 0 2.22"/>
    </collision>
    <inertial>
      <mass value="1"/>
      <inertia ixx="0.016000000000000004" ixy="0.0" ixz="0.0" iyy="0.016000000000000004" iyz="0.0" izz="0.016000000000000004"/>
    </inertial>
  </link>
  <joint name="slider_to_cart" type="prismatic">
    <axis xyz="1 0 0"/>
    <origin xyz="0.0 0.0 0.0"/>
    <parent link="slideBar"/>
    <child link="cart"/>
    <limit effort="1000.0" lower="-15" upper="15" velocity="5"/>
  </joint>
  <joint name="cart_to_pole" type="continuous">
    <axis xyz="0 1 0"/>
    <origin xyz="0.0 0.0 0.0"/>
    <parent link="cart"/>
    <child link="pole"/>
  </joint>
  <joint name="pole_to_ball" type="fixed">
    <origin xyz="0.0 0.0 2.0"/>
    <parent link="pole"/>
    <child link="ball"/>
  </joint>
</robot>

And I have this simulation

import pybullet as p
import time
import pybullet_data
import os,sys

print(sys.path)

physicsClient = p.connect(p.GUI)#or p.DIRECT for non-graphical version

p.setAdditionalSearchPath(pybullet_data.getDataPath())
p.resetSimulation()
p.setGravity(0, 0, -10)
useRealTimeSim = 1

#for video recording (works best on Mac and Linux, not well on Windows)
#p.startStateLogging(p.STATE_LOGGING_VIDEO_MP4, "racecar.mp4")
p.setRealTimeSimulation(useRealTimeSim)  # either this

# p.loadURDF("plane.urdf")

# p.resetSimulation()

# current folder directory

current_dir = os.path.dirname(os.path.realpath(__file__))
urdf_file_path = os.path.join(current_dir, "cartpole.urdf")
cartpole = p.loadURDF(urdf_file_path ,[0, 0, 0])

# p.changeDynamics(cartpole, -1, linearDamping=0, angularDamping=0)

# p.changeDynamics(cartpole, 0, linearDamping=0, angularDamping=0)

# p.changeDynamics(cartpole, 1, linearDamping=0, angularDamping=0)

# p.changeDynamics(cartpole, 2, linearDamping=0, angularDamping=0)

p.resetJointState(cartpole, 1, targetValue=3.14,targetVelocity=0)
p.setJointMotorControl2(cartpole, 1, p.VELOCITY_CONTROL, force=0) # this command with a reference force of zero removes any constraint on the object
p.setJointMotorControl2(cartpole, 0, p.VELOCITY_CONTROL, force=0)

# p.setJointMotorControl2(cartpole, 2, p.VELOCITY_CONTROL, force=100)

p.setGravity(0, 0, -9.8)
p.setRealTimeSimulation(0)
counter = 1

p.enableJointForceTorqueSensor(cartpole, 1, enableSensor=True)

while (True):
#p.setJointMotorControl2(cartpole, 0, p.TORQUE_CONTROL, force=0.001)

p.setJointMotorControl2(cartpole, 1,  p.TORQUE_CONTROL, force=3)

print(p.getJointState(cartpole, 1))
time.sleep(1./240.)
p.stepSimulation()

The joint 1 is the joint associated with the revolute joint between the cart and the pole in the system. I am trying to control the joint through a torque control and what I am doing in the example it is to apply a constant torque over the joint to see if the simulation is accurate. What I see is that the pole starts oscillating even if the torque is constant. What I expected was to see the pole actually standing at an angle with respect to the vertical, where this angle is the angle at which the gravitational force would exert a toque proportional to the pole length and equal to the input torque. But What I see is that the system oscillates instead which I was not what I was expecting. Can somone spot any clear mistake in my implementation or in my reasoning?

Upvotes: 1

Views: 58

Answers (0)

Related Questions