Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Merge pull request #1291 from erwincoumans/master

Browse Source 
improve minitaur urdf and quadruped.py test
This commit is contained in:
erwincoumans
2017-09-02 13:23:59 -07:00
committed by GitHub
parent 29271c53eb a47e342ec4
commit ee64a39c7b
14 changed files with 297 additions and 113 deletions
Download Patch File Download Diff File Expand all files Collapse all files

77
data/quadruped/minitaur.urdf
View File

@@ -267,6 +267,11 @@
<color rgba="1 1 1 1"/> <color rgba="1 1 1 1"/>
</material> </material>
</visual> </visual>
<collision>
<geometry>
<cylinder length="0.026" radius="0.0434"/>
</geometry>
</collision>
<inertial> <inertial>
<mass value="0.25"/> <mass value="0.25"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/> <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
@@ -427,8 +432,10 @@
<link name="lower_leg_front_rightR_link"> <link name="lower_leg_front_rightR_link">
<contact> <contact>
<stiffness value="10000"/> <friction_anchor/>
<damping value="10"/> <stiffness value="30000.0"/>
<damping value="1000.0"/>
<spinning_friction value=".3"/>
<lateral_friction value="1"/> <lateral_friction value="1"/>
</contact> </contact>
@@ -448,11 +455,12 @@
</geometry> </geometry>
</collision> </collision>
<inertial> <inertial>
<origin rpy="0.0 0 0" xyz="0 0 .1"/>
<mass value="0.05"/> <mass value="0.05"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/> <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial> </inertial>
</link> </link>
<joint name="knee_front_rightR_link" type="revolute"> <joint name="knee_front_rightR_link" type="continuous">
<axis xyz="0 1 0"/> <axis xyz="0 1 0"/>
<parent link="upper_leg_front_rightR_link"/> <parent link="upper_leg_front_rightR_link"/>
<child link="lower_leg_front_rightR_link"/> <child link="lower_leg_front_rightR_link"/>
@@ -495,8 +503,10 @@
<link name="lower_leg_front_rightL_link"> <link name="lower_leg_front_rightL_link">
<contact> <contact>
<stiffness value="10000"/> <friction_anchor/>
<damping value="10"/> <stiffness value="30000.0"/>
<damping value="1000.0"/>
<spinning_friction value=".3"/>
<lateral_friction value="1"/> <lateral_friction value="1"/>
</contact> </contact>
@@ -516,11 +526,12 @@
</geometry> </geometry>
</collision> </collision>
<inertial> <inertial>
<origin rpy="0.0 0 0" xyz="0 0 .1"/>
<mass value="0.05"/> <mass value="0.05"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/> <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial> </inertial>
</link> </link>
<joint name="knee_front_rightL_link" type="revolute"> <joint name="knee_front_rightL_link" type="continuous">
<axis xyz="0 1 0"/> <axis xyz="0 1 0"/>
<parent link="upper_leg_front_rightL_link"/> <parent link="upper_leg_front_rightL_link"/>
<child link="lower_leg_front_rightL_link"/> <child link="lower_leg_front_rightL_link"/>
@@ -561,8 +572,10 @@
<link name="lower_leg_front_leftR_link"> <link name="lower_leg_front_leftR_link">
<contact> <contact>
<stiffness value="10000"/> <friction_anchor/>
<damping value="10"/> <stiffness value="30000.0"/>
<damping value="1000.0"/>
<spinning_friction value=".3"/>
<lateral_friction value="1"/> <lateral_friction value="1"/>
</contact> </contact>
@@ -582,11 +595,12 @@
</geometry> </geometry>
</collision> </collision>
<inertial> <inertial>
<origin rpy="0.0 0.0 0" xyz="0 0 .1"/>
<mass value="0.05"/> <mass value="0.05"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/> <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial> </inertial>
</link> </link>
<joint name="knee_front_leftR_link" type="revolute"> <joint name="knee_front_leftR_link" type="continuous">
<axis xyz="0 1 0"/> <axis xyz="0 1 0"/>
<parent link="upper_leg_front_leftR_link"/> <parent link="upper_leg_front_leftR_link"/>
<child link="lower_leg_front_leftR_link"/> <child link="lower_leg_front_leftR_link"/>
@@ -628,8 +642,10 @@
<link name="lower_leg_front_leftL_link"> <link name="lower_leg_front_leftL_link">
<contact> <contact>
<stiffness value="10000"/> <friction_anchor/>
<damping value="10"/> <stiffness value="30000.0"/>
<damping value="1000.0"/>
<spinning_friction value=".3"/>
<lateral_friction value="1"/> <lateral_friction value="1"/>
</contact> </contact>
@@ -649,11 +665,12 @@
</geometry> </geometry>
</collision> </collision>
<inertial> <inertial>
<origin rpy="0.0 0.0 0" xyz="0 0 .1"/>
<mass value="0.05"/> <mass value="0.05"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/> <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial> </inertial>
</link> </link>
<joint name="knee_front_leftL_link" type="revolute"> <joint name="knee_front_leftL_link" type="continuous">
<axis xyz="0 1 0"/> <axis xyz="0 1 0"/>
<parent link="upper_leg_front_leftL_link"/> <parent link="upper_leg_front_leftL_link"/>
<child link="lower_leg_front_leftL_link"/> <child link="lower_leg_front_leftL_link"/>
@@ -692,8 +709,10 @@
<link name="lower_leg_back_rightR_link"> <link name="lower_leg_back_rightR_link">
<contact> <contact>
<stiffness value="10000"/> <friction_anchor/>
<damping value="10"/> <stiffness value="30000.0"/>
<damping value="1000.0"/>
<spinning_friction value=".3"/>
<lateral_friction value="1"/> <lateral_friction value="1"/>
</contact> </contact>
@@ -713,11 +732,12 @@
</geometry> </geometry>
</collision> </collision>
<inertial> <inertial>
<origin rpy="0.0 0 0" xyz="0 0 .1"/>
<mass value="0.05"/> <mass value="0.05"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/> <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial> </inertial>
</link> </link>
<joint name="knee_back_rightR_link" type="revolute"> <joint name="knee_back_rightR_link" type="continuous">
<axis xyz="0 1 0"/> <axis xyz="0 1 0"/>
<parent link="upper_leg_back_rightR_link"/> <parent link="upper_leg_back_rightR_link"/>
<child link="lower_leg_back_rightR_link"/> <child link="lower_leg_back_rightR_link"/>
@@ -760,8 +780,10 @@
<link name="lower_leg_back_rightL_link"> <link name="lower_leg_back_rightL_link">
<contact> <contact>
<stiffness value="10000"/> <friction_anchor/>
<damping value="10"/> <stiffness value="30000.0"/>
<damping value="1000.0"/>
<spinning_friction value=".3"/>
<lateral_friction value="1"/> <lateral_friction value="1"/>
</contact> </contact>
@@ -781,11 +803,12 @@
</geometry> </geometry>
</collision> </collision>
<inertial> <inertial>
<origin rpy="0.0 0 0" xyz="0 0 .1"/>
<mass value="0.05"/> <mass value="0.05"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/> <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial> </inertial>
</link> </link>
<joint name="knee_back_rightL_link" type="revolute"> <joint name="knee_back_rightL_link" type="continuous">
<axis xyz="0 1 0"/> <axis xyz="0 1 0"/>
<parent link="upper_leg_back_rightL_link"/> <parent link="upper_leg_back_rightL_link"/>
<child link="lower_leg_back_rightL_link"/> <child link="lower_leg_back_rightL_link"/>
@@ -826,8 +849,10 @@
<link name="lower_leg_back_leftR_link"> <link name="lower_leg_back_leftR_link">
<contact> <contact>
<stiffness value="10000"/> <friction_anchor/>
<damping value="10"/> <stiffness value="30000.0"/>
<damping value="1000.0"/>
<spinning_friction value=".3"/>
<lateral_friction value="1"/> <lateral_friction value="1"/>
</contact> </contact>
@@ -847,11 +872,12 @@
</geometry> </geometry>
</collision> </collision>
<inertial> <inertial>
<origin rpy="0.0 0 0" xyz="0 0 .1"/>
<mass value="0.05"/> <mass value="0.05"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/> <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial> </inertial>
</link> </link>
<joint name="knee_back_leftR_link" type="revolute"> <joint name="knee_back_leftR_link" type="continuous">
<axis xyz="0 1 0"/> <axis xyz="0 1 0"/>
<parent link="upper_leg_back_leftR_link"/> <parent link="upper_leg_back_leftR_link"/>
<child link="lower_leg_back_leftR_link"/> <child link="lower_leg_back_leftR_link"/>
@@ -891,8 +917,10 @@
<link name="lower_leg_back_leftL_link"> <link name="lower_leg_back_leftL_link">
<contact> <contact>
<stiffness value="10000"/> <friction_anchor/>
<damping value="10"/> <stiffness value="30000.0"/>
<damping value="1000.0"/>
<spinning_friction value=".3"/>
<lateral_friction value="1"/> <lateral_friction value="1"/>
</contact> </contact>
@@ -912,11 +940,12 @@
</geometry> </geometry>
</collision> </collision>
<inertial> <inertial>
<origin rpy="0.0 0 0" xyz="0 0 .1"/>
<mass value="0.05"/> <mass value="0.05"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/> <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial> </inertial>
</link> </link>
<joint name="knee_back_leftL_link" type="revolute"> <joint name="knee_back_leftL_link" type="continuous">
<axis xyz="0 1 0"/> <axis xyz="0 1 0"/>
<parent link="upper_leg_back_leftL_link"/> <parent link="upper_leg_back_leftL_link"/>
<child link="lower_leg_back_leftL_link"/> <child link="lower_leg_back_leftL_link"/>

BIN
docs/pybullet_quickstartguide.pdf
View File

Binary file not shown.

5
examples/ExampleBrowser/OpenGLExampleBrowser.cpp
View File

@@ -221,6 +221,11 @@ void MyKeyboardCallback(int key, int state)
gDebugDrawFlags ^= btIDebugDraw::DBG_NoDeactivation; gDebugDrawFlags ^= btIDebugDraw::DBG_NoDeactivation;
gDisableDeactivation = ((gDebugDrawFlags & btIDebugDraw::DBG_NoDeactivation) != 0); gDisableDeactivation = ((gDebugDrawFlags & btIDebugDraw::DBG_NoDeactivation) != 0);
} }
if (key == 'j' && state)
{
gDebugDrawFlags ^= btIDebugDraw::DBG_DrawFrames;
}
if (key == 'k' && state) if (key == 'k' && state)
{ {
gDebugDrawFlags ^= btIDebugDraw::DBG_DrawConstraints; gDebugDrawFlags ^= btIDebugDraw::DBG_DrawConstraints;

2
examples/Importers/ImportURDFDemo/MyMultiBodyCreator.cpp
View File

@@ -140,7 +140,7 @@ class btGeneric6DofSpring2Constraint* MyMultiBodyCreator::createRevoluteJoint(in
dof6->setLinearUpperLimit(btVector3(0,0,0)); dof6->setLinearUpperLimit(btVector3(0,0,0));
dof6->setAngularUpperLimit(btVector3(0,0,-1)); dof6->setAngularUpperLimit(btVector3(0,0,-1));
dof6->setAngularLowerLimit(btVector3(0,0,0)); dof6->setAngularLowerLimit(btVector3(0,0,1));
} }
}; };

2
examples/SharedMemory/PhysicsClientSharedMemory.cpp
View File

@@ -396,7 +396,7 @@ template <typename T, typename U> void addJointInfoFromConstraint(int linkIndex,
info.m_linkName = 0; info.m_linkName = 0;
info.m_flags = 0; info.m_flags = 0;
info.m_jointIndex = linkIndex; info.m_jointIndex = linkIndex;
info.m_qIndex = linkIndex+6; info.m_qIndex = linkIndex+7;
info.m_uIndex = linkIndex+6; info.m_uIndex = linkIndex+6;
//derive type from limits //derive type from limits

85
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -890,7 +890,7 @@ struct GenericRobotStateLogger : public InternalStateLogger
{ {
m_structTypes.append("f"); m_structTypes.append("f");
char jointName[256]; char jointName[256];
sprintf(jointName,"v%d",i); sprintf(jointName,"u%d",i);
structNames.push_back(jointName); structNames.push_back(jointName);
} }
@@ -900,7 +900,7 @@ struct GenericRobotStateLogger : public InternalStateLogger
{ {
m_structTypes.append("f"); m_structTypes.append("f");
char jointName[256]; char jointName[256];
sprintf(jointName,"u%d",i); sprintf(jointName,"t%d",i);
structNames.push_back(jointName); structNames.push_back(jointName);
} }
} }
@@ -4601,7 +4601,9 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
b3Printf("Using CONTROL_MODE_TORQUE"); b3Printf("Using CONTROL_MODE_TORQUE");
} }
// mb->clearForcesAndTorques(); // mb->clearForcesAndTorques();
int torqueIndex = 6; ///see addJointInfoFromConstraint
int velIndex = 6;
int posIndex = 7;
//if ((clientCmd.m_updateFlags&SIM_DESIRED_STATE_HAS_MAX_FORCE)!=0) //if ((clientCmd.m_updateFlags&SIM_DESIRED_STATE_HAS_MAX_FORCE)!=0)
{ {
for (int link=0;link<body->m_rigidBodyJoints.size();link++) for (int link=0;link<body->m_rigidBodyJoints.size();link++)
@@ -4616,14 +4618,33 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
//for (int dof=0;dof<mb->getLink(link).m_dofCount;dof++) //for (int dof=0;dof<mb->getLink(link).m_dofCount;dof++)
{ {
double torque = 0.f;
//if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[torqueIndex]&SIM_DESIRED_STATE_HAS_MAX_FORCE)!=0)
{
torque = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[torqueIndex];
btScalar qdotTarget = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQdot[torqueIndex];
btScalar qTarget = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQ[torqueIndex];
{
int torqueIndex = velIndex;
double torque = 100;
bool hasDesiredTorque = false;
if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[velIndex] & SIM_DESIRED_STATE_HAS_MAX_FORCE)!=0)
{
torque = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[velIndex];
hasDesiredTorque = true;
}
bool hasDesiredPosOrVel = false;
btScalar qdotTarget = 0.f;
if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[velIndex] & SIM_DESIRED_STATE_HAS_QDOT)!=0)
{
hasDesiredPosOrVel = true;
qdotTarget = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQdot[velIndex];
}
btScalar qTarget = 0.f;
if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[posIndex] & SIM_DESIRED_STATE_HAS_Q)!=0)
{
hasDesiredPosOrVel = true;
qTarget = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQ[posIndex];
}
con->getLinearLowerLimit(linearLowerLimit); con->getLinearLowerLimit(linearLowerLimit);
con->getLinearUpperLimit(linearUpperLimit); con->getLinearUpperLimit(linearUpperLimit);
con->getAngularLowerLimit(angularLowerLimit); con->getAngularLowerLimit(angularLowerLimit);
@@ -4650,30 +4671,39 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
{ {
case CONTROL_MODE_TORQUE: case CONTROL_MODE_TORQUE:
{ {
con->getRigidBodyA().applyTorque(torque*axisA); if (hasDesiredTorque)
con->getRigidBodyB().applyTorque(-torque*axisB); {
con->getRigidBodyA().applyTorque(torque*axisA);
con->getRigidBodyB().applyTorque(-torque*axisB);
}
break; break;
} }
case CONTROL_MODE_VELOCITY: case CONTROL_MODE_VELOCITY:
{ {
con->enableMotor(3+limitAxis,true); if (hasDesiredPosOrVel)
con->setTargetVelocity(3+limitAxis, qdotTarget); {
//this is max motor force impulse con->enableMotor(3+limitAxis,true);
btScalar torqueImpulse = torque*m_data->m_dynamicsWorld->getSolverInfo().m_timeStep; con->setTargetVelocity(3+limitAxis, qdotTarget);
con->setMaxMotorForce(3+limitAxis,torqueImpulse); //this is max motor force impulse
btScalar torqueImpulse = torque*m_data->m_dynamicsWorld->getSolverInfo().m_timeStep;
con->setMaxMotorForce(3+limitAxis,torqueImpulse);
}
break; break;
} }
case CONTROL_MODE_POSITION_VELOCITY_PD: case CONTROL_MODE_POSITION_VELOCITY_PD:
{ {
con->setServo(3+limitAxis,true); if (hasDesiredPosOrVel)
con->setServoTarget(3+limitAxis,qTarget); {
//next one is the maximum velocity to reach target position. con->setServo(3+limitAxis,true);
//the maximum velocity is limited by maxMotorForce con->setServoTarget(3+limitAxis,-qTarget);
con->setTargetVelocity(3+limitAxis, 100); //next one is the maximum velocity to reach target position.
//this is max motor force impulse //the maximum velocity is limited by maxMotorForce
btScalar torqueImpulse = torque*m_data->m_dynamicsWorld->getSolverInfo().m_timeStep; con->setTargetVelocity(3+limitAxis, 100);
con->setMaxMotorForce(3+limitAxis,torqueImpulse); //this is max motor force impulse
con->enableMotor(3+limitAxis,true); btScalar torqueImpulse = torque*m_data->m_dynamicsWorld->getSolverInfo().m_timeStep;
con->setMaxMotorForce(3+limitAxis,torqueImpulse);
con->enableMotor(3+limitAxis,true);
}
break; break;
} }
default: default:
@@ -4733,7 +4763,10 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
} }
} }
}//fi }//fi
torqueIndex++; ///see addJointInfoFromConstraint
velIndex ++;//info.m_uIndex
posIndex ++;//info.m_qIndex
} }
} }
}//fi }//fi

6
examples/SharedMemory/PhysicsServerExample.cpp
View File

@@ -25,6 +25,8 @@
bool gEnablePicking=true; bool gEnablePicking=true;
bool gEnableTeleporting=true; bool gEnableTeleporting=true;
bool gEnableRendering= true; bool gEnableRendering= true;
bool gActivedVRRealTimeSimulation = false;
bool gEnableSyncPhysicsRendering= true; bool gEnableSyncPhysicsRendering= true;
bool gEnableUpdateDebugDrawLines = true; bool gEnableUpdateDebugDrawLines = true;
static int gCamVisualizerWidth = 320; static int gCamVisualizerWidth = 320;
@@ -2627,8 +2629,10 @@ void PhysicsServerExample::renderScene()
if (m_guiHelper->getAppInterface()->m_renderer->getActiveCamera()->isVRCamera()) if (m_guiHelper->getAppInterface()->m_renderer->getActiveCamera()->isVRCamera())
{ {
if (!m_physicsServer.isRealTimeSimulationEnabled()) if (!m_physicsServer.isRealTimeSimulationEnabled() && !gActivedVRRealTimeSimulation)
{ {
//only activate real-time simulation once (for backward compatibility)
gActivedVRRealTimeSimulation = true;
m_physicsServer.enableRealTimeSimulation(1); m_physicsServer.enableRealTimeSimulation(1);
} }
} }

155
examples/pybullet/examples/quadruped.py
View File

@@ -2,34 +2,49 @@ import pybullet as p
import time import time
import math import math
useRealTime = 0 toeConstraint = True
fixedTimeStep = 0.01 useMaximalCoordinates = False
useRealTime = 1
#the fixedTimeStep and numSolverIterations are the most important parameters to trade-off quality versus performance
fixedTimeStep = 1./100
numSolverIterations = 50
if (useMaximalCoordinates):
fixedTimeStep = 1./500
numSolverIterations = 200
speed = 10 speed = 10
amplitude = 0.8 amplitude = 0.8
jump_amp = 0.5 jump_amp = 0.5
maxForce = 3.5 maxForce = 3.5
kneeFrictionForce = 0.00 kneeFrictionForce = 0
kp = 1 kp = 1
kd = .1 kd = .5
maxKneeForce = 1000
physId = p.connect(p.SHARED_MEMORY) physId = p.connect(p.SHARED_MEMORY)
if (physId<0): if (physId<0):
p.connect(p.GUI) p.connect(p.GUI)
#p.resetSimulation() #p.resetSimulation()
p.loadURDF("plane.urdf",0,0,0)
p.setPhysicsEngineParameter(numSolverIterations=50) angle = 0 # pick in range 0..0.2 radians
orn = p.getQuaternionFromEuler([0,angle,0])
p.loadURDF("plane.urdf",[0,0,0],orn)
p.setPhysicsEngineParameter(numSolverIterations=numSolverIterations)
p.startStateLogging(p.STATE_LOGGING_GENERIC_ROBOT, "genericlogdata.bin", maxLogDof = 16, logFlags = p.STATE_LOG_JOINT_TORQUES) p.startStateLogging(p.STATE_LOGGING_GENERIC_ROBOT, "genericlogdata.bin", maxLogDof = 16, logFlags = p.STATE_LOG_JOINT_TORQUES)
p.setTimeOut(4) p.setTimeOut(4000000)
p.setGravity(0,0,0) p.setGravity(0,0,0)
p.setTimeStep(fixedTimeStep) p.setTimeStep(fixedTimeStep)
orn = p.getQuaternionFromEuler([0,0,0.4]) orn = p.getQuaternionFromEuler([0,0,0.4])
p.setRealTimeSimulation(0) p.setRealTimeSimulation(0)
quadruped = p.loadURDF("quadruped/minitaur.urdf",[1,0,0.2],orn,useFixedBase=False) quadruped = p.loadURDF("quadruped/minitaur.urdf",[1,-1,.3],orn,useFixedBase=False, useMaximalCoordinates=useMaximalCoordinates)
nJoints = p.getNumJoints(quadruped) nJoints = p.getNumJoints(quadruped)
jointNameToId = {} jointNameToId = {}
for i in range(nJoints): for i in range(nJoints):
jointInfo = p.getJointInfo(quadruped, i) jointInfo = p.getJointInfo(quadruped, i)
@@ -37,11 +52,11 @@ for i in range(nJoints):
motor_front_rightR_joint = jointNameToId['motor_front_rightR_joint'] motor_front_rightR_joint = jointNameToId['motor_front_rightR_joint']
motor_front_rightL_joint = jointNameToId['motor_front_rightL_joint']
knee_front_rightL_link = jointNameToId['knee_front_rightL_link']
hip_front_rightR_link = jointNameToId['hip_front_rightR_link'] hip_front_rightR_link = jointNameToId['hip_front_rightR_link']
knee_front_rightR_link = jointNameToId['knee_front_rightR_link'] knee_front_rightR_link = jointNameToId['knee_front_rightR_link']
motor_front_rightL_joint = jointNameToId['motor_front_rightL_joint']
motor_front_rightL_link = jointNameToId['motor_front_rightL_link'] motor_front_rightL_link = jointNameToId['motor_front_rightL_link']
knee_front_rightL_link = jointNameToId['knee_front_rightL_link']
motor_front_leftR_joint = jointNameToId['motor_front_leftR_joint'] motor_front_leftR_joint = jointNameToId['motor_front_leftR_joint']
hip_front_leftR_link = jointNameToId['hip_front_leftR_link'] hip_front_leftR_link = jointNameToId['hip_front_leftR_link']
knee_front_leftR_link = jointNameToId['knee_front_leftR_link'] knee_front_leftR_link = jointNameToId['knee_front_leftR_link']
@@ -61,51 +76,83 @@ motor_back_leftL_joint = jointNameToId['motor_back_leftL_joint']
motor_back_leftL_link = jointNameToId['motor_back_leftL_link'] motor_back_leftL_link = jointNameToId['motor_back_leftL_link']
knee_back_leftL_link = jointNameToId['knee_back_leftL_link'] knee_back_leftL_link = jointNameToId['knee_back_leftL_link']
#fixtorso = p.createConstraint(-1,-1,quadruped,-1,p.JOINT_FIXED,[0,0,0],[0,0,0],[0,0,0])
motordir=[-1,-1,-1,-1,1,1,1,1] motordir=[-1,-1,-1,-1,1,1,1,1]
halfpi = 1.57079632679 halfpi = 1.57079632679
twopi = 4*halfpi
kneeangle = -2.1834 kneeangle = -2.1834
p.resetJointState(quadruped,motor_front_leftL_joint,motordir[0]*halfpi)
p.resetJointState(quadruped,knee_front_leftL_link,motordir[0]*kneeangle)
p.resetJointState(quadruped,motor_front_leftR_joint,motordir[1]*halfpi)
p.resetJointState(quadruped,knee_front_leftR_link,motordir[1]*kneeangle)
cid = p.createConstraint(quadruped,knee_front_leftR_link,quadruped,knee_front_leftL_link,p.JOINT_POINT2POINT,[0,0,0],[0,0.005,0.2],[0,0.01,0.2])
p.changeConstraint(cid,maxForce=10000)
p.setJointMotorControl(quadruped,knee_front_leftL_link,p.VELOCITY_CONTROL,0,kneeFrictionForce)
p.setJointMotorControl(quadruped,knee_front_leftR_link,p.VELOCITY_CONTROL,0,kneeFrictionForce)
p.resetJointState(quadruped,motor_back_leftL_joint,motordir[2]*halfpi)
p.resetJointState(quadruped,knee_back_leftL_link,motordir[2]*kneeangle)
p.resetJointState(quadruped,motor_back_leftR_joint,motordir[3]*halfpi)
p.resetJointState(quadruped,knee_back_leftR_link,motordir[3]*kneeangle)
cid = p.createConstraint(quadruped,knee_back_leftR_link,quadruped,knee_back_leftL_link,p.JOINT_POINT2POINT,[0,0,0],[0,0.005,0.2],[0,0.01,0.2])
p.changeConstraint(cid,maxForce=10000)
p.setJointMotorControl(quadruped,knee_back_leftL_link,p.VELOCITY_CONTROL,0,kneeFrictionForce)
p.setJointMotorControl(quadruped,knee_back_leftR_link,p.VELOCITY_CONTROL,0,kneeFrictionForce)
if (useMaximalCoordinates):
steps = 400
for aa in range (steps):
p.setJointMotorControl2(quadruped,motor_front_leftL_joint,p.POSITION_CONTROL,motordir[0]*halfpi*float(aa)/steps)
p.setJointMotorControl2(quadruped,motor_front_leftR_joint,p.POSITION_CONTROL,motordir[1]*halfpi*float(aa)/steps)
p.setJointMotorControl2(quadruped,motor_back_leftL_joint,p.POSITION_CONTROL,motordir[2]*halfpi*float(aa)/steps)
p.setJointMotorControl2(quadruped,motor_back_leftR_joint,p.POSITION_CONTROL,motordir[3]*halfpi*float(aa)/steps)
p.setJointMotorControl2(quadruped,motor_front_rightL_joint,p.POSITION_CONTROL,motordir[4]*halfpi*float(aa)/steps)
p.setJointMotorControl2(quadruped,motor_front_rightR_joint,p.POSITION_CONTROL,motordir[5]*halfpi*float(aa)/steps)
p.setJointMotorControl2(quadruped,motor_back_rightL_joint,p.POSITION_CONTROL,motordir[6]*halfpi*float(aa)/steps)
p.setJointMotorControl2(quadruped,motor_back_rightR_joint,p.POSITION_CONTROL,motordir[7]*halfpi*float(aa)/steps)
p.setJointMotorControl2(quadruped,knee_front_leftL_link,p.POSITION_CONTROL,motordir[0]*(kneeangle+twopi)*float(aa)/steps)
p.setJointMotorControl2(quadruped,knee_front_leftR_link,p.POSITION_CONTROL,motordir[1]*kneeangle*float(aa)/steps)
p.setJointMotorControl2(quadruped,knee_back_leftL_link,p.POSITION_CONTROL,motordir[2]*kneeangle*float(aa)/steps)
p.setJointMotorControl2(quadruped,knee_back_leftR_link,p.POSITION_CONTROL,motordir[3]*(kneeangle+twopi)*float(aa)/steps)
p.setJointMotorControl2(quadruped,knee_front_rightL_link,p.POSITION_CONTROL,motordir[4]*(kneeangle)*float(aa)/steps)
p.setJointMotorControl2(quadruped,knee_front_rightR_link,p.POSITION_CONTROL,motordir[5]*(kneeangle+twopi)*float(aa)/steps)
p.setJointMotorControl2(quadruped,knee_back_rightL_link,p.POSITION_CONTROL,motordir[6]*(kneeangle+twopi)*float(aa)/steps)
p.setJointMotorControl2(quadruped,knee_back_rightR_link,p.POSITION_CONTROL,motordir[7]*kneeangle*float(aa)/steps)
p.stepSimulation()
#time.sleep(fixedTimeStep)
else:
p.resetJointState(quadruped,motor_front_leftL_joint,motordir[0]*halfpi)
p.resetJointState(quadruped,knee_front_leftL_link,motordir[0]*kneeangle)
p.resetJointState(quadruped,motor_front_leftR_joint,motordir[1]*halfpi)
p.resetJointState(quadruped,knee_front_leftR_link,motordir[1]*kneeangle)
p.resetJointState(quadruped,motor_back_leftL_joint,motordir[2]*halfpi)
p.resetJointState(quadruped,knee_back_leftL_link,motordir[2]*kneeangle)
p.resetJointState(quadruped,motor_back_leftR_joint,motordir[3]*halfpi)
p.resetJointState(quadruped,knee_back_leftR_link,motordir[3]*kneeangle)
p.resetJointState(quadruped,motor_front_rightL_joint,motordir[4]*halfpi)
p.resetJointState(quadruped,knee_front_rightL_link,motordir[4]*kneeangle)
p.resetJointState(quadruped,motor_front_rightR_joint,motordir[5]*halfpi)
p.resetJointState(quadruped,knee_front_rightR_link,motordir[5]*kneeangle)
p.resetJointState(quadruped,motor_back_rightL_joint,motordir[6]*halfpi)
p.resetJointState(quadruped,knee_back_rightL_link,motordir[6]*kneeangle)
p.resetJointState(quadruped,motor_back_rightR_joint,motordir[7]*halfpi)
p.resetJointState(quadruped,knee_back_rightR_link,motordir[7]*kneeangle)
#p.getNumJoints(1) #p.getNumJoints(1)
p.resetJointState(quadruped,motor_front_rightL_joint,motordir[4]*halfpi) if (toeConstraint):
p.resetJointState(quadruped,knee_front_rightL_link,motordir[4]*kneeangle) cid = p.createConstraint(quadruped,knee_front_leftR_link,quadruped,knee_front_leftL_link,p.JOINT_POINT2POINT,[0,0,0],[0,0.005,0.1],[0,0.01,0.1])
p.resetJointState(quadruped,motor_front_rightR_joint,motordir[5]*halfpi) p.changeConstraint(cid,maxForce=maxKneeForce)
p.resetJointState(quadruped,knee_front_rightR_link,motordir[5]*kneeangle) cid = p.createConstraint(quadruped,knee_front_rightR_link,quadruped,knee_front_rightL_link,p.JOINT_POINT2POINT,[0,0,0],[0,0.005,0.1],[0,0.01,0.1])
cid = p.createConstraint(quadruped,knee_front_rightR_link,quadruped,knee_front_rightL_link,p.JOINT_POINT2POINT,[0,0,0],[0,0.005,0.2],[0,0.01,0.2]) p.changeConstraint(cid,maxForce=maxKneeForce)
p.changeConstraint(cid,maxForce=10000) cid = p.createConstraint(quadruped,knee_back_leftR_link,quadruped,knee_back_leftL_link,p.JOINT_POINT2POINT,[0,0,0],[0,0.005,0.1],[0,0.01,0.1])
p.changeConstraint(cid,maxForce=maxKneeForce)
p.setJointMotorControl(quadruped,knee_front_rightL_link,p.VELOCITY_CONTROL,0,kneeFrictionForce) cid = p.createConstraint(quadruped,knee_back_rightR_link,quadruped,knee_back_rightL_link,p.JOINT_POINT2POINT,[0,0,0],[0,0.005,0.1],[0,0.01,0.1])
p.setJointMotorControl(quadruped,knee_front_rightR_link,p.VELOCITY_CONTROL,0,kneeFrictionForce) p.changeConstraint(cid,maxForce=maxKneeForce)
p.resetJointState(quadruped,motor_back_rightL_joint,motordir[6]*halfpi) if (1):
p.resetJointState(quadruped,knee_back_rightL_link,motordir[6]*kneeangle) p.setJointMotorControl(quadruped,knee_front_leftL_link,p.VELOCITY_CONTROL,0,kneeFrictionForce)
p.resetJointState(quadruped,motor_back_rightR_joint,motordir[7]*halfpi) p.setJointMotorControl(quadruped,knee_front_leftR_link,p.VELOCITY_CONTROL,0,kneeFrictionForce)
p.resetJointState(quadruped,knee_back_rightR_link,motordir[7]*kneeangle) p.setJointMotorControl(quadruped,knee_front_rightL_link,p.VELOCITY_CONTROL,0,kneeFrictionForce)
cid = p.createConstraint(quadruped,knee_back_rightR_link,quadruped,knee_back_rightL_link,p.JOINT_POINT2POINT,[0,0,0],[0,0.005,0.2],[0,0.01,0.2]) p.setJointMotorControl(quadruped,knee_front_rightR_link,p.VELOCITY_CONTROL,0,kneeFrictionForce)
p.changeConstraint(cid,maxForce=10000) p.setJointMotorControl(quadruped,knee_back_leftL_link,p.VELOCITY_CONTROL,0,kneeFrictionForce)
p.setJointMotorControl(quadruped,knee_back_leftR_link,p.VELOCITY_CONTROL,0,kneeFrictionForce)
p.setJointMotorControl(quadruped,knee_back_rightL_link,p.VELOCITY_CONTROL,0,kneeFrictionForce) p.setJointMotorControl(quadruped,knee_back_leftL_link,p.VELOCITY_CONTROL,0,kneeFrictionForce)
p.setJointMotorControl(quadruped,knee_back_rightR_link,p.VELOCITY_CONTROL,0,kneeFrictionForce) p.setJointMotorControl(quadruped,knee_back_leftR_link,p.VELOCITY_CONTROL,0,kneeFrictionForce)
p.setJointMotorControl(quadruped,knee_back_rightL_link,p.VELOCITY_CONTROL,0,kneeFrictionForce)
p.setJointMotorControl(quadruped,knee_back_rightR_link,p.VELOCITY_CONTROL,0,kneeFrictionForce)
p.setGravity(0,0,-10) p.setGravity(0,0,-10)
@@ -135,10 +182,18 @@ p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[7],controlMo
#stand still #stand still
p.setRealTimeSimulation(useRealTime) p.setRealTimeSimulation(useRealTime)
#while (True):
# time.sleep(0.01)
#p.stepSimulation()
t = 0.0
t_end = t + 15
ref_time = time.time()
while (t<t_end):
if (useRealTime):
t = time.time()-ref_time
else:
t = t+fixedTimeStep
if (useRealTime==0):
p.stepSimulation()
time.sleep(fixedTimeStep)
print("quadruped Id = ") print("quadruped Id = ")
print(quadruped) print(quadruped)
@@ -156,7 +211,7 @@ t_end = t + 100
i=0 i=0
ref_time = time.time() ref_time = time.time()
for aa in range (100): while (1):
if (useRealTime): if (useRealTime):
t = time.time()-ref_time t = time.time()-ref_time
else: else:

5
examples/pybullet/examples/vr_kuka_setup.py
View File

@@ -5,7 +5,8 @@ cid = p.connect(p.SHARED_MEMORY)
if (cid<0): if (cid<0):
p.connect(p.GUI) p.connect(p.GUI)
p.resetSimulation() p.resetSimulation()
#disable rendering during loading makes it much faster
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0)
objects = [p.loadURDF("plane.urdf", 0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,1.000000)] objects = [p.loadURDF("plane.urdf", 0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,1.000000)]
objects = [p.loadURDF("samurai.urdf", 0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,1.000000)] objects = [p.loadURDF("samurai.urdf", 0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,1.000000)]
objects = [p.loadURDF("pr2_gripper.urdf", 0.500000,0.300006,0.700000,-0.000000,-0.000000,-0.000031,1.000000)] objects = [p.loadURDF("pr2_gripper.urdf", 0.500000,0.300006,0.700000,-0.000000,-0.000000,-0.000031,1.000000)]
@@ -74,6 +75,8 @@ jointPositions=[ 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.0
for jointIndex in range (p.getNumJoints(ob)): for jointIndex in range (p.getNumJoints(ob)):
p.resetJointState(ob,jointIndex,jointPositions[jointIndex]) p.resetJointState(ob,jointIndex,jointPositions[jointIndex])
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1)
p.setGravity(0.000000,0.000000,0.000000) p.setGravity(0.000000,0.000000,0.000000)
p.setGravity(0,0,-10) p.setGravity(0,0,-10)

4
examples/pybullet/gym/pybullet_envs/__init__.py
View File

@@ -132,3 +132,7 @@ register(
max_episode_steps=1000, max_episode_steps=1000,
reward_threshold=2500.0 reward_threshold=2500.0
) )
def getList():
btenvs = ['- ' + spec.id for spec in gym.envs.registry.all() if spec.id.find('Bullet')>=0])
return btenvs

2
setup.py
View File

@@ -440,7 +440,7 @@ print("-----")
setup( setup(
name = 'pybullet', name = 'pybullet',
version='1.3.0', version='1.3.1',
description='Official Python Interface for the Bullet Physics SDK Robotics Simulator', description='Official Python Interface for the Bullet Physics SDK Robotics Simulator',
long_description='pybullet is an easy to use Python module for physics simulation, robotics and machine learning based on the Bullet Physics SDK. With pybullet you can load articulated bodies from URDF, SDF and other file formats. pybullet provides forward dynamics simulation, inverse dynamics computation, forward and inverse kinematics and collision detection and ray intersection queries. Aside from physics simulation, pybullet supports to rendering, with a CPU renderer and OpenGL visualization and support for virtual reality headsets.', long_description='pybullet is an easy to use Python module for physics simulation, robotics and machine learning based on the Bullet Physics SDK. With pybullet you can load articulated bodies from URDF, SDF and other file formats. pybullet provides forward dynamics simulation, inverse dynamics computation, forward and inverse kinematics and collision detection and ray intersection queries. Aside from physics simulation, pybullet supports to rendering, with a CPU renderer and OpenGL visualization and support for virtual reality headsets.',
url='https://github.com/bulletphysics/bullet3', url='https://github.com/bulletphysics/bullet3',

2
src/BulletCollision/CollisionDispatch/btCollisionWorld.cpp
View File

@@ -1333,7 +1333,7 @@ void btCollisionWorld::debugDrawObject(const btTransform& worldTransform, const
// Draw a small simplex at the center of the object // Draw a small simplex at the center of the object
if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawFrames) if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawFrames)
{ {
getDebugDrawer()->drawTransform(worldTransform,1); getDebugDrawer()->drawTransform(worldTransform,.1);
} }
if (shape->getShapeType() == COMPOUND_SHAPE_PROXYTYPE) if (shape->getShapeType() == COMPOUND_SHAPE_PROXYTYPE)

63
src/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.cpp
View File

@@ -729,6 +729,21 @@ int btGeneric6DofSpring2Constraint::get_limit_motor_info2(
if (limot->m_enableMotor && limot->m_servoMotor) if (limot->m_enableMotor && limot->m_servoMotor)
{ {
btScalar error = limot->m_currentPosition - limot->m_servoTarget; btScalar error = limot->m_currentPosition - limot->m_servoTarget;
btScalar curServoTarget = limot->m_servoTarget;
if (rotational)
{
if (error > SIMD_PI)
{
error -= SIMD_2_PI;
curServoTarget +=SIMD_2_PI;
}
if (error < -SIMD_PI)
{
error += SIMD_2_PI;
curServoTarget -=SIMD_2_PI;
}
}
calculateJacobi(limot,transA,transB,info,srow,ax1,rotational,rotAllowed); calculateJacobi(limot,transA,transB,info,srow,ax1,rotational,rotAllowed);
btScalar targetvelocity = error<0 ? -limot->m_targetVelocity : limot->m_targetVelocity; btScalar targetvelocity = error<0 ? -limot->m_targetVelocity : limot->m_targetVelocity;
btScalar tag_vel = -targetvelocity; btScalar tag_vel = -targetvelocity;
@@ -739,13 +754,13 @@ int btGeneric6DofSpring2Constraint::get_limit_motor_info2(
btScalar hiLimit; btScalar hiLimit;
if(limot->m_loLimit > limot->m_hiLimit) if(limot->m_loLimit > limot->m_hiLimit)
{ {
lowLimit = error > 0 ? limot->m_servoTarget : -SIMD_INFINITY; lowLimit = error > 0 ? curServoTarget : -SIMD_INFINITY;
hiLimit = error < 0 ? limot->m_servoTarget : SIMD_INFINITY; hiLimit = error < 0 ? curServoTarget : SIMD_INFINITY;
} }
else else
{ {
lowLimit = error > 0 && limot->m_servoTarget>limot->m_loLimit ? limot->m_servoTarget : limot->m_loLimit; lowLimit = error > 0 && curServoTarget>limot->m_loLimit ? curServoTarget : limot->m_loLimit;
hiLimit = error < 0 && limot->m_servoTarget<limot->m_hiLimit ? limot->m_servoTarget : limot->m_hiLimit; hiLimit = error < 0 && curServoTarget<limot->m_hiLimit ? curServoTarget : limot->m_hiLimit;
} }
mot_fact = getMotorFactor(limot->m_currentPosition, lowLimit, hiLimit, tag_vel, info->fps * limot->m_motorERP); mot_fact = getMotorFactor(limot->m_currentPosition, lowLimit, hiLimit, tag_vel, info->fps * limot->m_motorERP);
} }
@@ -998,13 +1013,49 @@ void btGeneric6DofSpring2Constraint::setTargetVelocity(int index, btScalar veloc
m_angularLimits[index - 3].m_targetVelocity = velocity; m_angularLimits[index - 3].m_targetVelocity = velocity;
} }
void btGeneric6DofSpring2Constraint::setServoTarget(int index, btScalar target)
void btGeneric6DofSpring2Constraint::setServoTarget(int index, btScalar targetOrg)
{ {
btAssert((index >= 0) && (index < 6)); btAssert((index >= 0) && (index < 6));
if (index<3) if (index<3)
m_linearLimits.m_servoTarget[index] = target; {
m_linearLimits.m_servoTarget[index] = targetOrg;
}
else else
{
//wrap between -PI and PI, see also
//https://stackoverflow.com/questions/4633177/c-how-to-wrap-a-float-to-the-interval-pi-pi
btScalar target = targetOrg+SIMD_PI;
if (1)
{
btScalar m = target - SIMD_2_PI * floor(target/SIMD_2_PI);
// handle boundary cases resulted from floating-point cut off:
{
if (m>=SIMD_2_PI)
{
target = 0;
} else
{
if (m<0 )
{
if (SIMD_2_PI+m == SIMD_2_PI)
target = 0;
else
target = SIMD_2_PI+m;
}
else
{
target = m;
}
}
}
target -= SIMD_PI;
}
m_angularLimits[index - 3].m_servoTarget = target; m_angularLimits[index - 3].m_servoTarget = target;
}
} }
void btGeneric6DofSpring2Constraint::setMaxMotorForce(int index, btScalar force) void btGeneric6DofSpring2Constraint::setMaxMotorForce(int index, btScalar force)

2
src/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp
View File

@@ -19,7 +19,7 @@ subject to the following restrictions:
#include "LinearMath/btSerializer.h" #include "LinearMath/btSerializer.h"
#define DEFAULT_DEBUGDRAW_SIZE btScalar(0.3f) #define DEFAULT_DEBUGDRAW_SIZE btScalar(0.05f)
btTypedConstraint::btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA) btTypedConstraint::btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA)
:btTypedObject(type), :btTypedObject(type),