Created
June 18, 2020 17:46
-
-
Save avikde/89a85fccb0643e66e1cba4563b1a824c to your computer and use it in GitHub Desktop.
Pybullet example two-link robot applyExternalForce unexpected behavior
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
import pybullet as p | |
import pybullet_data | |
import time | |
import numpy as np | |
SIM_TIMESTEP = 1 | |
# Init sim | |
physicsClient = p.connect(p.GUI)#or p.DIRECT for non-graphical version | |
# Set up the visualizer | |
p.configureDebugVisualizer(p.COV_ENABLE_WIREFRAME, 0) | |
p.configureDebugVisualizer(p.COV_ENABLE_SHADOWS, 0) | |
p.configureDebugVisualizer(p.COV_ENABLE_GUI, 1) | |
p.setRealTimeSimulation(0) | |
p.setTimeStep(SIM_TIMESTEP) | |
# load robot | |
startPos = [0,0,0] | |
startOrientation = p.getQuaternionFromEuler([0,0,0]) | |
bid = p.loadURDF("test2link.urdf", startPos, startOrientation, useFixedBase=True) | |
# springy control for both joints | |
# p.resetJointState(bid, 0, 0.5, 0) | |
p.resetJointState(bid, 1, -0.5, 0) | |
p.setJointMotorControlArray(bid, [0,1], p.POSITION_CONTROL, targetPositions=[0,0], positionGains=[0.01,1], velocityGains=[0.1,0.99]) | |
idff = p.addUserDebugParameter("Test force", -0.01, 0.01, 0) | |
while True: | |
try: | |
# Apply force to last link | |
pend = [0,1.1,0.1] | |
Fend = [p.readUserDebugParameter(idff),0,0] | |
p.applyExternalForce(bid, 1, Fend, pend, p.WORLD_FRAME) | |
p.addUserDebugLine(pend, np.array(pend) + 100*np.array(Fend), lineColorRGB=[1,0,0],lifeTime=0.1,lineWidth=2) | |
p.stepSimulation() | |
time.sleep(SIM_TIMESTEP * 1e-3) | |
except: | |
raise |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
<?xml version="1.0" ?> | |
<robot name="test2link"> | |
<!-- Body --> | |
<link name="body"> | |
<visual> | |
<origin rpy="0 0 0" xyz="0 0 0"/> | |
<geometry> | |
<box size="0.1 0.1 1"/> | |
</geometry> | |
</visual> | |
<inertial> | |
<mass value="1"/> | |
<inertia ixx="0.1" ixy="0" ixz="0" iyy="0.1" iyz="0" izz="0.1"/> | |
</inertial> | |
</link> | |
<link name="link1"> | |
<visual> | |
<origin rpy="0 0 0" xyz="0 0.5 0"/> | |
<geometry> | |
<box size="0.1 1 0.1"/> | |
</geometry> | |
</visual> | |
<inertial> | |
<mass value="1"/> | |
<inertia ixx="0.1" ixy="0" ixz="0" iyy="0.1" iyz="0" izz="0.1"/> | |
</inertial> | |
</link> | |
<link name="link2"> | |
<visual> | |
<origin rpy="0 0 0" xyz="0 0 -0.25"/> | |
<geometry> | |
<box size="0.1 0.1 0.5"/> | |
</geometry> | |
</visual> | |
<inertial> | |
<mass value="10"/> | |
<inertia ixx="0.1" ixy="0" ixz="0" iyy="0.1" iyz="0" izz="0.1"/> | |
</inertial> | |
</link> | |
<joint name="0" type="continuous"> | |
<parent link="body"/> | |
<child link="link1"/> | |
<axis xyz="0 0 1"/> | |
<origin rpy="0 0 0" xyz="0.0 0.1 0.5"/> | |
</joint> | |
<joint name="1" type="continuous"> | |
<parent link="link1"/> | |
<child link="link2"/> | |
<axis xyz="0 1 0"/> | |
<origin rpy="0 0 0" xyz="0 1 -0.1"/> | |
</joint> | |
</robot> |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment