Reputation: 11
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