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"/>
</material>
</visual>
<collision>
<geometry>
<cylinder length="0.026" radius="0.0434"/>
</geometry>
</collision>
<inertial>
<mass value="0.25"/>
<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">
<contact>
<stiffness value="10000"/>
<damping value="10"/>
<friction_anchor/>
<stiffness value="30000.0"/>
<damping value="1000.0"/>
<spinning_friction value=".3"/>
<lateral_friction value="1"/>
</contact>
@@ -448,11 +455,12 @@
</geometry>
</collision>
<inertial>
<origin rpy="0.0 0 0" xyz="0 0 .1"/>
<mass value="0.05"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="knee_front_rightR_link" type="revolute">
<joint name="knee_front_rightR_link" type="continuous">
<axis xyz="0 1 0"/>
<parent link="upper_leg_front_rightR_link"/>
<child link="lower_leg_front_rightR_link"/>
@@ -495,8 +503,10 @@
<link name="lower_leg_front_rightL_link">
<contact>
<stiffness value="10000"/>
<damping value="10"/>
<friction_anchor/>
<stiffness value="30000.0"/>
<damping value="1000.0"/>
<spinning_friction value=".3"/>
<lateral_friction value="1"/>
</contact>
@@ -516,11 +526,12 @@
</geometry>
</collision>
<inertial>
<origin rpy="0.0 0 0" xyz="0 0 .1"/>
<mass value="0.05"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="knee_front_rightL_link" type="revolute">
<joint name="knee_front_rightL_link" type="continuous">
<axis xyz="0 1 0"/>
<parent link="upper_leg_front_rightL_link"/>
<child link="lower_leg_front_rightL_link"/>
@@ -561,8 +572,10 @@
<link name="lower_leg_front_leftR_link">
<contact>
<stiffness value="10000"/>
<damping value="10"/>
<friction_anchor/>
<stiffness value="30000.0"/>
<damping value="1000.0"/>
<spinning_friction value=".3"/>
<lateral_friction value="1"/>
</contact>
@@ -582,11 +595,12 @@
</geometry>
</collision>
<inertial>
<origin rpy="0.0 0.0 0" xyz="0 0 .1"/>
<mass value="0.05"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="knee_front_leftR_link" type="revolute">
<joint name="knee_front_leftR_link" type="continuous">
<axis xyz="0 1 0"/>
<parent link="upper_leg_front_leftR_link"/>
<child link="lower_leg_front_leftR_link"/>
@@ -628,8 +642,10 @@
<link name="lower_leg_front_leftL_link">
<contact>
<stiffness value="10000"/>
<damping value="10"/>
<friction_anchor/>
<stiffness value="30000.0"/>
<damping value="1000.0"/>
<spinning_friction value=".3"/>
<lateral_friction value="1"/>
</contact>
@@ -649,11 +665,12 @@
</geometry>
</collision>
<inertial>
<origin rpy="0.0 0.0 0" xyz="0 0 .1"/>
<mass value="0.05"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="knee_front_leftL_link" type="revolute">
<joint name="knee_front_leftL_link" type="continuous">
<axis xyz="0 1 0"/>
<parent link="upper_leg_front_leftL_link"/>
<child link="lower_leg_front_leftL_link"/>
@@ -692,8 +709,10 @@
<link name="lower_leg_back_rightR_link">
<contact>
<stiffness value="10000"/>
<damping value="10"/>
<friction_anchor/>
<stiffness value="30000.0"/>
<damping value="1000.0"/>
<spinning_friction value=".3"/>
<lateral_friction value="1"/>
</contact>
@@ -713,11 +732,12 @@
</geometry>
</collision>
<inertial>
<origin rpy="0.0 0 0" xyz="0 0 .1"/>
<mass value="0.05"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="knee_back_rightR_link" type="revolute">
<joint name="knee_back_rightR_link" type="continuous">
<axis xyz="0 1 0"/>
<parent link="upper_leg_back_rightR_link"/>
<child link="lower_leg_back_rightR_link"/>
@@ -760,8 +780,10 @@
<link name="lower_leg_back_rightL_link">
<contact>
<stiffness value="10000"/>
<damping value="10"/>
<friction_anchor/>
<stiffness value="30000.0"/>
<damping value="1000.0"/>
<spinning_friction value=".3"/>
<lateral_friction value="1"/>
</contact>
@@ -781,11 +803,12 @@
</geometry>
</collision>
<inertial>
<origin rpy="0.0 0 0" xyz="0 0 .1"/>
<mass value="0.05"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="knee_back_rightL_link" type="revolute">
<joint name="knee_back_rightL_link" type="continuous">
<axis xyz="0 1 0"/>
<parent link="upper_leg_back_rightL_link"/>
<child link="lower_leg_back_rightL_link"/>
@@ -826,8 +849,10 @@
<link name="lower_leg_back_leftR_link">
<contact>
<stiffness value="10000"/>
<damping value="10"/>
<friction_anchor/>
<stiffness value="30000.0"/>
<damping value="1000.0"/>
<spinning_friction value=".3"/>
<lateral_friction value="1"/>
</contact>
@@ -847,11 +872,12 @@
</geometry>
</collision>
<inertial>
<origin rpy="0.0 0 0" xyz="0 0 .1"/>
<mass value="0.05"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="knee_back_leftR_link" type="revolute">
<joint name="knee_back_leftR_link" type="continuous">
<axis xyz="0 1 0"/>
<parent link="upper_leg_back_leftR_link"/>
<child link="lower_leg_back_leftR_link"/>
@@ -891,8 +917,10 @@
<link name="lower_leg_back_leftL_link">
<contact>
<stiffness value="10000"/>
<damping value="10"/>
<friction_anchor/>
<stiffness value="30000.0"/>
<damping value="1000.0"/>
<spinning_friction value=".3"/>
<lateral_friction value="1"/>
</contact>
@@ -912,11 +940,12 @@
</geometry>
</collision>
<inertial>
<origin rpy="0.0 0 0" xyz="0 0 .1"/>
<mass value="0.05"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
<joint name="knee_back_leftL_link" type="revolute">
<joint name="knee_back_leftL_link" type="continuous">
<axis xyz="0 1 0"/>
<parent link="upper_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;
gDisableDeactivation = ((gDebugDrawFlags & btIDebugDraw::DBG_NoDeactivation) != 0);
}
if (key == 'j' && state)
{
gDebugDrawFlags ^= btIDebugDraw::DBG_DrawFrames;
}
if (key == 'k' && state)
{
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->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_flags = 0;
info.m_jointIndex = linkIndex;
info.m_qIndex = linkIndex+6;
info.m_qIndex = linkIndex+7;
info.m_uIndex = linkIndex+6;
//derive type from limits

85
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

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

6
examples/SharedMemory/PhysicsServerExample.cpp
View File

@@ -25,6 +25,8 @@
bool gEnablePicking=true;
bool gEnableTeleporting=true;
bool gEnableRendering= true;
bool gActivedVRRealTimeSimulation = false;
bool gEnableSyncPhysicsRendering= true;
bool gEnableUpdateDebugDrawLines = true;
static int gCamVisualizerWidth = 320;
@@ -2627,8 +2629,10 @@ void PhysicsServerExample::renderScene()
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);
}
}

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

@@ -2,34 +2,49 @@ import pybullet as p
import time
import math
useRealTime = 0
fixedTimeStep = 0.01
toeConstraint = True
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
amplitude = 0.8
jump_amp = 0.5
maxForce = 3.5
kneeFrictionForce = 0.00
kneeFrictionForce = 0
kp = 1
kd = .1
kd = .5
maxKneeForce = 1000
physId = p.connect(p.SHARED_MEMORY)
if (physId<0):
p.connect(p.GUI)
#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.setTimeOut(4)
p.setTimeOut(4000000)
p.setGravity(0,0,0)
p.setTimeStep(fixedTimeStep)
orn = p.getQuaternionFromEuler([0,0,0.4])
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)
jointNameToId = {}
for i in range(nJoints):
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_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']
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']
knee_front_rightL_link = jointNameToId['knee_front_rightL_link']
motor_front_leftR_joint = jointNameToId['motor_front_leftR_joint']
hip_front_leftR_link = jointNameToId['hip_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']
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]
halfpi = 1.57079632679
twopi = 4*halfpi
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.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)
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=10000)
p.setJointMotorControl(quadruped,knee_front_rightL_link,p.VELOCITY_CONTROL,0,kneeFrictionForce)
p.setJointMotorControl(quadruped,knee_front_rightR_link,p.VELOCITY_CONTROL,0,kneeFrictionForce)
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)
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.changeConstraint(cid,maxForce=10000)
p.setJointMotorControl(quadruped,knee_back_rightL_link,p.VELOCITY_CONTROL,0,kneeFrictionForce)
p.setJointMotorControl(quadruped,knee_back_rightR_link,p.VELOCITY_CONTROL,0,kneeFrictionForce)
if (toeConstraint):
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.changeConstraint(cid,maxForce=maxKneeForce)
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])
p.changeConstraint(cid,maxForce=maxKneeForce)
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)
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.changeConstraint(cid,maxForce=maxKneeForce)
if (1):
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.setJointMotorControl(quadruped,knee_front_rightL_link,p.VELOCITY_CONTROL,0,kneeFrictionForce)
p.setJointMotorControl(quadruped,knee_front_rightR_link,p.VELOCITY_CONTROL,0,kneeFrictionForce)
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_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_rightR_link,p.VELOCITY_CONTROL,0,kneeFrictionForce)
p.setGravity(0,0,-10)
@@ -135,10 +182,18 @@ p.setJointMotorControl2(bodyIndex=quadruped,jointIndex=legnumbering[7],controlMo
#stand still
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)
@@ -156,7 +211,7 @@ t_end = t + 100
i=0
ref_time = time.time()
for aa in range (100):
while (1):
if (useRealTime):
t = time.time()-ref_time
else:

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

@@ -5,7 +5,8 @@ cid = p.connect(p.SHARED_MEMORY)
if (cid<0):
p.connect(p.GUI)
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("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)]
@@ -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)):
p.resetJointState(ob,jointIndex,jointPositions[jointIndex])
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1)
p.setGravity(0.000000,0.000000,0.000000)
p.setGravity(0,0,-10)

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

@@ -132,3 +132,7 @@ register(
max_episode_steps=1000,
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(
name = 'pybullet',
version='1.3.0',
version='1.3.1',
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.',
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
if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawFrames)
{
getDebugDrawer()->drawTransform(worldTransform,1);
getDebugDrawer()->drawTransform(worldTransform,.1);
}
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)
{
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);
btScalar targetvelocity = error<0 ? -limot->m_targetVelocity : limot->m_targetVelocity;
btScalar tag_vel = -targetvelocity;
@@ -739,13 +754,13 @@ int btGeneric6DofSpring2Constraint::get_limit_motor_info2(
btScalar hiLimit;
if(limot->m_loLimit > limot->m_hiLimit)
{
lowLimit = error > 0 ? limot->m_servoTarget : -SIMD_INFINITY;
hiLimit = error < 0 ? limot->m_servoTarget : SIMD_INFINITY;
lowLimit = error > 0 ? curServoTarget : -SIMD_INFINITY;
hiLimit = error < 0 ? curServoTarget : SIMD_INFINITY;
}
else
{
lowLimit = error > 0 && limot->m_servoTarget>limot->m_loLimit ? limot->m_servoTarget : limot->m_loLimit;
hiLimit = error < 0 && limot->m_servoTarget<limot->m_hiLimit ? limot->m_servoTarget : limot->m_hiLimit;
lowLimit = error > 0 && curServoTarget>limot->m_loLimit ? curServoTarget : limot->m_loLimit;
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);
}
@@ -998,13 +1013,49 @@ void btGeneric6DofSpring2Constraint::setTargetVelocity(int index, btScalar veloc
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));
if (index<3)
m_linearLimits.m_servoTarget[index] = target;
{
m_linearLimits.m_servoTarget[index] = targetOrg;
}
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;
}
}
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"
#define DEFAULT_DEBUGDRAW_SIZE btScalar(0.3f)
#define DEFAULT_DEBUGDRAW_SIZE btScalar(0.05f)
btTypedConstraint::btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA)
:btTypedObject(type),