AuroraMiddleware/bullet3
1
0
Fork 0
mirror of https://github.com/bulletphysics/bullet3 synced 2024-12-13 21:30:09 +00:00
Code Issues Releases Wiki Activity

Merge remote-tracking branch 'upstream/master'

Browse Source
This commit is contained in:
yunfeibai 2017-02-07 17:21:14 -08:00
commit 8034a6f7fc
8 changed files with 214 additions and 49 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

40
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@ -444,10 +444,12 @@ int b3JointControlSetDesiredPosition(b3SharedMemoryCommandHandle commandHandle,
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
command->m_sendDesiredStateCommandArgument.m_desiredStateQ[qIndex] = value;
command->m_updateFlags |= SIM_DESIRED_STATE_HAS_Q;
command->m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[qIndex] |= SIM_DESIRED_STATE_HAS_Q;
if ((qIndex>=0) && (qIndex < MAX_DEGREE_OF_FREEDOM))
{
command->m_sendDesiredStateCommandArgument.m_desiredStateQ[qIndex] = value;
command->m_updateFlags |= SIM_DESIRED_STATE_HAS_Q;
command->m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[qIndex] |= SIM_DESIRED_STATE_HAS_Q;
}
return 0;
}
@ -455,8 +457,7 @@ int b3JointControlSetKp(b3SharedMemoryCommandHandle commandHandle, int dofIndex,
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(dofIndex>=0);
if (dofIndex>=0)
if ((dofIndex>=0) && (dofIndex < MAX_DEGREE_OF_FREEDOM))
{
command->m_sendDesiredStateCommandArgument.m_Kp[dofIndex] = value;
command->m_updateFlags |= SIM_DESIRED_STATE_HAS_KP;
@ -469,8 +470,7 @@ int b3JointControlSetKd(b3SharedMemoryCommandHandle commandHandle, int dofIndex,
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(dofIndex>=0);
if (dofIndex>=0)
if ((dofIndex>=0) && (dofIndex < MAX_DEGREE_OF_FREEDOM))
{
command->m_sendDesiredStateCommandArgument.m_Kd[dofIndex] = value;
command->m_updateFlags |= SIM_DESIRED_STATE_HAS_KD;
@ -483,8 +483,7 @@ int b3JointControlSetDesiredVelocity(b3SharedMemoryCommandHandle commandHandle,
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(dofIndex>=0);
if (dofIndex>=0)
if ((dofIndex>=0) && (dofIndex < MAX_DEGREE_OF_FREEDOM))
{
command->m_sendDesiredStateCommandArgument.m_desiredStateQdot[dofIndex] = value;
command->m_updateFlags |= SIM_DESIRED_STATE_HAS_QDOT;
@ -498,8 +497,7 @@ int b3JointControlSetMaximumForce(b3SharedMemoryCommandHandle commandHandle, int
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(dofIndex>=0);
if (dofIndex>=0)
if ((dofIndex>=0) && (dofIndex < MAX_DEGREE_OF_FREEDOM))
{
command->m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[dofIndex] = value;
command->m_updateFlags |= SIM_DESIRED_STATE_HAS_MAX_FORCE;
@ -512,8 +510,7 @@ int b3JointControlSetDesiredForceTorque(b3SharedMemoryCommandHandle commandHandl
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(dofIndex>=0);
if (dofIndex>=0)
if ((dofIndex>=0) && (dofIndex < MAX_DEGREE_OF_FREEDOM))
{
command->m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[dofIndex] = value;
command->m_updateFlags |= SIM_DESIRED_STATE_HAS_MAX_FORCE;
@ -547,13 +544,16 @@ int b3GetJointState(b3PhysicsClientHandle physClient, b3SharedMemoryStatusHandle
bool result = b3GetJointInfo(physClient, bodyIndex,jointIndex, &info);
if (result)
{
state->m_jointPosition = status->m_sendActualStateArgs.m_actualStateQ[info.m_qIndex];
state->m_jointVelocity = status->m_sendActualStateArgs.m_actualStateQdot[info.m_uIndex];
for (int ii(0); ii < 6; ++ii) {
state->m_jointForceTorque[ii] = status->m_sendActualStateArgs.m_jointReactionForces[6 * jointIndex + ii];
if ((info.m_qIndex>=0) && (info.m_uIndex>=0) && (info.m_qIndex < MAX_DEGREE_OF_FREEDOM) && (info.m_uIndex < MAX_DEGREE_OF_FREEDOM))
{
state->m_jointPosition = status->m_sendActualStateArgs.m_actualStateQ[info.m_qIndex];
state->m_jointVelocity = status->m_sendActualStateArgs.m_actualStateQdot[info.m_uIndex];
for (int ii(0); ii < 6; ++ii) {
state->m_jointForceTorque[ii] = status->m_sendActualStateArgs.m_jointReactionForces[6 * jointIndex + ii];
}
state->m_jointMotorTorque = status->m_sendActualStateArgs.m_jointMotorForce[jointIndex];
return 1;
}
state->m_jointMotorTorque = status->m_sendActualStateArgs.m_jointMotorForce[jointIndex];
return 1;
}
}
return 0;

17
examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.cpp
View File

@ -1,5 +1,6 @@
#include "SharedMemoryInProcessPhysicsC_API.h"
#include "../Utils/b3Clock.h"
#include "PhysicsClientSharedMemory.h"
#include"../ExampleBrowser/InProcessExampleBrowser.h"
@ -8,7 +9,8 @@
class InProcessPhysicsClientSharedMemoryMainThread : public PhysicsClientSharedMemory
{
btInProcessExampleBrowserMainThreadInternalData* m_data;
b3Clock m_clock;
public:
InProcessPhysicsClientSharedMemoryMainThread(int argc, char* argv[])
@ -41,9 +43,16 @@ public:
{
PhysicsClientSharedMemory::disconnectSharedMemory();
}
btUpdateInProcessExampleBrowserMainThread(m_data);
return PhysicsClientSharedMemory::processServerStatus();
unsigned long int ms = m_clock.getTimeMilliseconds();
if (ms>20)
{
m_clock.reset();
btUpdateInProcessExampleBrowserMainThread(m_data);
} else
{
//b3Clock::usleep(100);
}
return PhysicsClientSharedMemory::processServerStatus();
}

2
examples/ThirdPartyLibs/Gwen/Utility.cpp
View File

@ -16,7 +16,7 @@ using namespace Gwen;
UnicodeString Gwen::Utility::Format( const wchar_t* fmt, ... )
{
wchar_t strOut[ 4096 ];
wchar_t strOut[ 2048 ];
va_list s;
va_start( s, fmt );

83
examples/pybullet/minitaur.py Normal file
View File

@ -0,0 +1,83 @@
import pybullet as p
class Minitaur:
def __init__(self):
self.reset()
def reset(self):
self.quadruped = p.loadURDF("quadruped/quadruped.urdf",0,0,.3)
self.kp = 1
self.kd = 0.1
self.maxForce = 100
nJoints = p.getNumJoints(self.quadruped)
self.jointNameToId = {}
for i in range(nJoints):
jointInfo = p.getJointInfo(self.quadruped, i)
self.jointNameToId[jointInfo[1].decode('UTF-8')] = jointInfo[0]
self.resetPose()
for i in range(100):
p.stepSimulation()
def disableAllMotors(self):
nJoints = p.getNumJoints(self.quadruped)
for i in range(nJoints):
p.setJointMotorControl2(bodyIndex=self.quadruped, jointIndex=i, controlMode=p.VELOCITY_CONTROL, force=0)
def setMotorAngleByName(self, motorName, desiredAngle):
p.setJointMotorControl2(bodyIndex=self.quadruped, jointIndex=self.jointNameToId[motorName], controlMode=p.POSITION_CONTROL, targetPosition=desiredAngle, positionGain=self.kp, velocityGain=self.kd, force=self.maxForce)
def resetPose(self):
#right front leg
self.disableAllMotors()
p.resetJointState(self.quadruped,self.jointNameToId['motor_front_rightR_joint'],1.57)
p.resetJointState(self.quadruped,self.jointNameToId['knee_front_rightR_link'],-2.2)
p.resetJointState(self.quadruped,self.jointNameToId['motor_front_rightL_joint'],-1.57)
p.resetJointState(self.quadruped,self.jointNameToId['knee_front_rightL_link'],2.2)
p.createConstraint(self.quadruped,self.jointNameToId['knee_front_rightR_link'],self.quadruped,self.jointNameToId['knee_front_rightL_link'],p.JOINT_POINT2POINT,[0,0,0],[0,0.01,0.2],[0,-0.015,0.2])
self.setMotorAngleByName('motor_front_rightR_joint', 1.57)
self.setMotorAngleByName('motor_front_rightL_joint',-1.57)
#left front leg
p.resetJointState(self.quadruped,self.jointNameToId['motor_front_leftR_joint'],1.57)
p.resetJointState(self.quadruped,self.jointNameToId['knee_front_leftR_link'],-2.2)
p.resetJointState(self.quadruped,self.jointNameToId['motor_front_leftL_joint'],-1.57)
p.resetJointState(self.quadruped,self.jointNameToId['knee_front_leftL_link'],2.2)
p.createConstraint(self.quadruped,self.jointNameToId['knee_front_leftR_link'],self.quadruped,self.jointNameToId['knee_front_leftL_link'],p.JOINT_POINT2POINT,[0,0,0],[0,-0.01,0.2],[0,0.015,0.2])
self.setMotorAngleByName('motor_front_leftR_joint', 1.57)
self.setMotorAngleByName('motor_front_leftL_joint',-1.57)
#right back leg
p.resetJointState(self.quadruped,self.jointNameToId['motor_back_rightR_joint'],1.57)
p.resetJointState(self.quadruped,self.jointNameToId['knee_back_rightR_link'],-2.2)
p.resetJointState(self.quadruped,self.jointNameToId['motor_back_rightL_joint'],-1.57)
p.resetJointState(self.quadruped,self.jointNameToId['knee_back_rightL_link'],2.2)
p.createConstraint(self.quadruped,self.jointNameToId['knee_back_rightR_link'],self.quadruped,self.jointNameToId['knee_back_rightL_link'],p.JOINT_POINT2POINT,[0,0,0],[0,0.01,0.2],[0,-0.015,0.2])
self.setMotorAngleByName('motor_back_rightR_joint', 1.57)
self.setMotorAngleByName('motor_back_rightL_joint',-1.57)
#left back leg
p.resetJointState(self.quadruped,self.jointNameToId['motor_back_leftR_joint'],1.57)
p.resetJointState(self.quadruped,self.jointNameToId['knee_back_leftR_link'],-2.2)
p.resetJointState(self.quadruped,self.jointNameToId['motor_back_leftL_joint'],-1.57)
p.resetJointState(self.quadruped,self.jointNameToId['knee_back_leftL_link'],2.2)
p.createConstraint(self.quadruped,self.jointNameToId['knee_back_leftR_link'],self.quadruped,self.jointNameToId['knee_back_leftL_link'],p.JOINT_POINT2POINT,[0,0,0],[0,-0.01,0.2],[0,0.015,0.2])
self.setMotorAngleByName('motor_back_leftR_joint', 1.57)
self.setMotorAngleByName('motor_back_leftL_joint',-1.57)
def getBasePosition(self):
position, orientation = p.getBasePositionAndOrientation(self.quadruped)
return position
def getBaseOrientation(self):
position, orientation = p.getBasePositionAndOrientation(self.quadruped)
return orientation
def applyAction(self, motorCommands):
self.setMotorAngleByName('motor_front_rightR_joint', motorCommands[0])
self.setMotorAngleByName('motor_front_rightL_joint', motorCommands[1])
self.setMotorAngleByName('motor_front_leftR_joint', motorCommands[2])
self.setMotorAngleByName('motor_front_leftL_joint', motorCommands[3])
self.setMotorAngleByName('motor_back_rightR_joint', motorCommands[4])
self.setMotorAngleByName('motor_back_rightL_joint', motorCommands[5])
self.setMotorAngleByName('motor_back_leftR_joint', motorCommands[6])
self.setMotorAngleByName('motor_back_leftL_joint', motorCommands[7])

33
examples/pybullet/minitaur_test.py Normal file
View File

@ -0,0 +1,33 @@
import pybullet as p
from minitaur import Minitaur
import time
import math
import numpy as np
def main(unused_args):
timeStep = 0.01
c = p.connect(p.SHARED_MEMORY)
if (c<0):
c = p.connect(p.GUI)
p.resetSimulation()
p.setTimeStep(timeStep)
p.loadURDF("plane.urdf")
p.setGravity(0,0,-10)
minitaur = Minitaur()
amplitude = 0.24795664427
speed = 0.2860877729434
for i in range(1000):
a1 = math.sin(i*speed)*amplitude+1.57
a2 = math.sin(i*speed+3.14)*amplitude+1.57
joint_values = [a1, -1.57, a1, -1.57, a2, -1.57, a2, -1.57]
minitaur.applyAction(joint_values)
p.stepSimulation()
# print(minitaur.getBasePosition())
time.sleep(timeStep)
final_distance = np.linalg.norm(np.asarray(minitaur.getBasePosition()))
print(final_distance)
main(0)

69
examples/pybullet/quadruped.py
View File

@ -2,6 +2,8 @@ import pybullet as p
import time
import math
useRealTime = 0
fixedTimeStep = 0.001
physId = p.connect(p.SHARED_MEMORY)
if (physId<0):
@ -9,8 +11,10 @@ if (physId<0):
p.loadURDF("plane.urdf")
p.setGravity(0,0,-1)
p.setTimeStep(fixedTimeStep)
p.setRealTimeSimulation(0)
quadruped = p.loadURDF("quadruped/quadruped.urdf",1,-2,2)
quadruped = p.loadURDF("quadruped/quadruped.urdf",1,-2,1)
#p.getNumJoints(1)
#right front leg
p.resetJointState(quadruped,0,1.57)
@ -71,24 +75,41 @@ p.setJointMotorControl(quadruped,23,p.VELOCITY_CONTROL,0,0)
p_gain = 2
speed = 10
p_gain =1
speed = 10
amplitude = 1.3
#stand still
t_end = time.time() + 2
while time.time() < t_end:
p.stepSimulation()
p.setRealTimeSimulation(useRealTime)
t=0.0
ref_time = time.time()
t_end = t + 4
while t < t_end:
if (useRealTime==0):
t = t+fixedTimeStep
p.stepSimulation()
else:
t = time.time()-ref_time
p.setGravity(0,0,-1)
p.setGravity(0,0,-10)
jump_amp = 0.5
#jump
t_end = time.time() + 10
t = 0.0
t_end = t + 10
i=0
t=0
while time.time() < t_end:
t = time.time()
ref_time = time.time()
while t < t_end:
if (useRealTime):
t = time.time()-ref_time
else:
t = t+fixedTimeStep
p.setJointMotorControl(quadruped,0,p.POSITION_CONTROL,math.sin(t*speed)*jump_amp+1.57,p_gain)
p.setJointMotorControl(quadruped,3,p.POSITION_CONTROL,-math.sin(t*speed)*jump_amp-1.57,p_gain)
p.setJointMotorControl(quadruped,6,p.POSITION_CONTROL,math.sin(t*speed)*jump_amp+1.57,p_gain)
@ -97,15 +118,19 @@ while time.time() < t_end:
p.setJointMotorControl(quadruped,15,p.POSITION_CONTROL,-math.sin(t*speed)*jump_amp-1.57,p_gain)
p.setJointMotorControl(quadruped,18,p.POSITION_CONTROL,math.sin(t*speed)*jump_amp+1.57,p_gain)
p.setJointMotorControl(quadruped,21,p.POSITION_CONTROL,-math.sin(t*speed)*jump_amp-1.57,p_gain)
p.stepSimulation()
if (useRealTime==0):
p.stepSimulation()
#hop forward
t_end = time.time() + 30
t_end = 20
i=0
while time.time() < t_end:
t = time.time()
while t < t_end:
if (useRealTime):
t = time.time()-ref_time
else:
t = t+fixedTimeStep
p.setJointMotorControl(quadruped,0,p.POSITION_CONTROL,math.sin(t*speed)*amplitude+1.57,p_gain)
p.setJointMotorControl(quadruped,3,p.POSITION_CONTROL,-1.57,p_gain)
p.setJointMotorControl(quadruped,6,p.POSITION_CONTROL,math.sin(t*speed)*amplitude+1.57,p_gain)
@ -114,14 +139,18 @@ while time.time() < t_end:
p.setJointMotorControl(quadruped,15,p.POSITION_CONTROL,-1.57,p_gain)
p.setJointMotorControl(quadruped,18,p.POSITION_CONTROL,math.sin(t*speed+3.14)*amplitude+1.57,p_gain)
p.setJointMotorControl(quadruped,21,p.POSITION_CONTROL,-1.57,p_gain)
p.stepSimulation()
if (useRealTime==0):
p.stepSimulation()
#walk
t_end = time.time() + 120
t_end = 100
i=0
while time.time() < t_end:
t = time.time()
while t < t_end:
if (useRealTime):
t = time.time()-ref_time
else:
t = t+fixedTimeStep
p.setJointMotorControl(quadruped,0,p.POSITION_CONTROL,math.sin(t*3)*.3+1.57,1)
p.setJointMotorControl(quadruped,3,p.POSITION_CONTROL,-1.57,1)
p.setJointMotorControl(quadruped,6,p.POSITION_CONTROL,math.sin(t*3+0.5*3.14)*.3+1.57,1)

11
src/BulletCollision/CollisionDispatch/btCompoundCompoundCollisionAlgorithm.cpp
View File

@ -24,6 +24,8 @@ subject to the following restrictions:
#include "BulletCollision/CollisionDispatch/btManifoldResult.h"
#include "BulletCollision/CollisionDispatch/btCollisionObjectWrapper.h"
//USE_LOCAL_STACK will avoid most (often all) dynamic memory allocations due to resizing in processCollision and MycollideTT
#define USE_LOCAL_STACK 1
btShapePairCallback gCompoundCompoundChildShapePairCallback = 0;
@ -251,7 +253,12 @@ static inline void MycollideTT( const btDbvtNode* root0,
int depth=1;
int treshold=btDbvt::DOUBLE_STACKSIZE-4;
btAlignedObjectArray<btDbvt::sStkNN> stkStack;
#ifdef USE_LOCAL_STACK
ATTRIBUTE_ALIGNED16(btDbvt::sStkNN localStack[btDbvt::DOUBLE_STACKSIZE]);
stkStack.initializeFromBuffer(&localStack,btDbvt::DOUBLE_STACKSIZE,btDbvt::DOUBLE_STACKSIZE);
#else
stkStack.resize(btDbvt::DOUBLE_STACKSIZE);
#endif
stkStack[0]=btDbvt::sStkNN(root0,root1);
do {
btDbvt::sStkNN p=stkStack[--depth];
@ -329,6 +336,10 @@ void btCompoundCompoundCollisionAlgorithm::processCollision (const btCollisionOb
{
int i;
btManifoldArray manifoldArray;
#ifdef USE_LOCAL_STACK
btPersistentManifold localManifolds[4];
manifoldArray.initializeFromBuffer(&localManifolds,0,4);
#endif
btSimplePairArray& pairs = m_childCollisionAlgorithmCache->getOverlappingPairArray();
for (i=0;i<pairs.size();i++)
{

8
src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp
View File

@ -332,10 +332,10 @@ struct MultiBodyInplaceSolverIslandCallback : public btSimulationIslandManager::
}
}
if (m_solverInfo->m_minimumSolverBatchSize<=1)
{
m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,startConstraint,numCurConstraints,*m_solverInfo,m_debugDrawer,m_dispatcher);
} else
//if (m_solverInfo->m_minimumSolverBatchSize<=1)
//{
// m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,startConstraint,numCurConstraints,*m_solverInfo,m_debugDrawer,m_dispatcher);
//} else
{
for (i=0;i<numBodies;i++)