Add pybullet example for resetting dynamics.

This commit is contained in:
yunfeibai
2017-05-03 21:30:42 -07:00
parent c7e9a31898
commit 1841a41f2a
2 changed files with 19 additions and 0 deletions

View File

@@ -1232,6 +1232,7 @@ int b3ResetDynamicInfoSetMass(b3SharedMemoryCommandHandle commandHandle, int bod
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command->m_type == CMD_RESET_DYNAMIC_INFO);
b3Assert(mass > 0);
command->m_resetDynamicInfoArgs.m_bodyUniqueId = bodyUniqueId;
command->m_resetDynamicInfoArgs.m_linkIndex = linkIndex;
command->m_resetDynamicInfoArgs.m_mass = mass;

View File

@@ -0,0 +1,18 @@
import pybullet as p
import time
import math
p.connect(p.GUI)
p.loadURDF(fileName="plane.urdf",baseOrientation=[0.25882,0,0,0.96593])
p.loadURDF(fileName="cube.urdf",baseOrientation=[0.25882,0,0,0.96593],basePosition=[0,0,2])
p.loadURDF(fileName="cube.urdf",baseOrientation=[0,0,0,1],basePosition=[0,0,4])
p.resetDynamicInfo(bodyUniqueId=2,linkIndex=-1,mass=0.1)
#p.resetDynamicInfo(bodyUniqueId=2,linkIndex=-1,mass=100.0)
p.setGravity(0,0,-10)
p.setRealTimeSimulation(0)
t=0
while 1:
t=t+1
time.sleep(.01)
p.stepSimulation()