Fixed joint constraint pendulum problem
Posted: Wed Feb 16, 2022 2:45 pm
Hello everyone,
My aim is to create a simple mid-air suspended oscillating pendulum.
Here is the URDF code:
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 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:
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 ?
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>
Now if I use
Code: Select all
resetJointState
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()
Can anyone explain why this weird behavior ?