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

Merge branch 'master' of https://github.com/bulletphysics/bullet3

Browse Source
This commit is contained in:
YunfeiBai
2016-09-28 13:54:18 -07:00
parent a967163665 cf046093ab
commit 032156fbf2
70 changed files with 28977 additions and 749 deletions
Download Patch File Download Diff File Expand all files Collapse all files

5
examples/Importers/ImportURDFDemo/URDF2Bullet.cpp
View File

@@ -225,7 +225,8 @@ void ConvertURDF2BulletInternal(
{
btVector3 color = selectColor2();
btVector4 color = selectColor2();
u2b.getLinkColor(urdfLinkIndex,color);
/*
if (visual->material.get())
{
@@ -255,7 +256,7 @@ void ConvertURDF2BulletInternal(
btRigidBody* body = creation.allocateRigidBody(urdfLinkIndex, mass, localInertiaDiagonal, inertialFrameInWorldSpace, compoundShape);
linkRigidBody = body;
world1->addRigidBody(body, bodyCollisionFilterGroup, bodyCollisionFilterMask);
world1->addRigidBody(body);
compoundShape->setUserIndex(graphicsIndex);

9
examples/SharedMemory/IKTrajectoryHelper.cpp
View File

@@ -43,7 +43,7 @@ bool IKTrajectoryHelper::computeIK(const double endEffectorTargetPosition[3],
const double endEffectorWorldPosition[3],
const double endEffectorWorldOrientation[4],
const double* q_current, int numQ,int endEffectorIndex,
double* q_new, int ikMethod, const double* linear_jacobian, const double* angular_jacobian, int jacobian_size, double dampIk)
double* q_new, int ikMethod, const double* linear_jacobian, const double* angular_jacobian, int jacobian_size, const double dampIk[6])
{
bool useAngularPart = (ikMethod==IK2_VEL_DLS_WITH_ORIENTATION) ? true : false;
@@ -69,7 +69,7 @@ bool IKTrajectoryHelper::computeIK(const double endEffectorTargetPosition[3],
VectorRn deltaS(3);
for (int i = 0; i < 3; ++i)
{
deltaS.Set(i,dampIk*(endEffectorTargetPosition[i]-endEffectorWorldPosition[i]));
deltaS.Set(i,dampIk[i]*(endEffectorTargetPosition[i]-endEffectorWorldPosition[i]));
}
// Set one end effector world orientation from Bullet
@@ -79,13 +79,12 @@ bool IKTrajectoryHelper::computeIK(const double endEffectorTargetPosition[3],
btQuaternion deltaQ = endQ*startQ.inverse();
float angle = deltaQ.getAngle();
btVector3 axis = deltaQ.getAxis();
float angleDot = angle*dampIk;
float angleDot = angle;
btVector3 angularVel = angleDot*axis.normalize();
for (int i = 0; i < 3; ++i)
{
deltaR.Set(i,angularVel[i]);
deltaR.Set(i,dampIk[i+3]*angularVel[i]);
}
deltaR[2] = 0.0;
{

13
examples/SharedMemory/IKTrajectoryHelper.h
View File

@@ -21,13 +21,12 @@ public:
IKTrajectoryHelper();
virtual ~IKTrajectoryHelper();
bool computeIK(const double endEffectorTargetPosition[3],
const double endEffectorTargetOrientation[4],
const double endEffectorWorldPosition[3],
const double endEffectorWorldOrientation[4],
const double* q_old, int numQ,int endEffectorIndex,
double* q_new, int ikMethod, const double* linear_jacobian, const double* angular_jacobian, int jacobian_size, double dampIk=1.);
bool computeIK(const double endEffectorTargetPosition[3],
const double endEffectorTargetOrientation[4],
const double endEffectorWorldPosition[3],
const double endEffectorWorldOrientation[4],
const double* q_old, int numQ, int endEffectorIndex,
double* q_new, int ikMethod, const double* linear_jacobian, const double* angular_jacobian, int jacobian_size, const double dampIk[6]);
};
#endif //IK_TRAJECTORY_HELPER_H

10
examples/SharedMemory/PhysicsClient.h
View File

@@ -24,9 +24,15 @@ public:
virtual bool submitClientCommand(const struct SharedMemoryCommand& command) = 0;
virtual int getNumJoints(int bodyIndex) const = 0;
virtual int getNumBodies() const = 0;
virtual bool getJointInfo(int bodyIndex, int jointIndex, struct b3JointInfo& info) const = 0;
virtual int getBodyUniqueId(int serialIndex) const = 0;
virtual bool getBodyInfo(int bodyUniqueId, struct b3BodyInfo& info) const = 0;
virtual int getNumJoints(int bodyUniqueId) const = 0;
virtual bool getJointInfo(int bodyUniqueId, int jointIndex, struct b3JointInfo& info) const = 0;
virtual void setSharedMemoryKey(int key) = 0;

23
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@@ -739,6 +739,29 @@ b3SharedMemoryStatusHandle b3SubmitClientCommandAndWaitStatus(b3PhysicsClientHan
}
///return the total number of bodies in the simulation
int b3GetNumBodies(b3PhysicsClientHandle physClient)
{
PhysicsClient* cl = (PhysicsClient* ) physClient;
return cl->getNumBodies();
}
/// return the body unique id, given the index in range [0 , b3GetNumBodies() )
int b3GetBodyUniqueId(b3PhysicsClientHandle physClient, int serialIndex)
{
PhysicsClient* cl = (PhysicsClient* ) physClient;
return cl->getBodyUniqueId(serialIndex);
}
///given a body unique id, return the body information. See b3BodyInfo in SharedMemoryPublic.h
int b3GetBodyInfo(b3PhysicsClientHandle physClient, int bodyUniqueId, struct b3BodyInfo* info)
{
PhysicsClient* cl = (PhysicsClient* ) physClient;
return cl->getBodyInfo(bodyUniqueId,*info);
}
int b3GetNumJoints(b3PhysicsClientHandle physClient, int bodyId)
{
PhysicsClient* cl = (PhysicsClient* ) physClient;

9
examples/SharedMemory/PhysicsClientC_API.h
View File

@@ -53,6 +53,15 @@ int b3GetStatusActualState(b3SharedMemoryStatusHandle statusHandle,
const double* actualStateQdot[],
const double* jointReactionForces[]);
///return the total number of bodies in the simulation
int b3GetNumBodies(b3PhysicsClientHandle physClient);
/// return the body unique id, given the index in range [0 , b3GetNumBodies() )
int b3GetBodyUniqueId(b3PhysicsClientHandle physClient, int serialIndex);
///given a body unique id, return the body information. See b3BodyInfo in SharedMemoryPublic.h
int b3GetBodyInfo(b3PhysicsClientHandle physClient, int bodyUniqueId, struct b3BodyInfo* info);
///give a unique body index (after loading the body) return the number of joints.
int b3GetNumJoints(b3PhysicsClientHandle physClient, int bodyIndex);

2
examples/SharedMemory/PhysicsClientExample.cpp
View File

@@ -624,7 +624,7 @@ void PhysicsClientExample::initPhysics()
{
MyCallback(CMD_LOAD_URDF, true, this);
MyCallback(CMD_STEP_FORWARD_SIMULATION,true,this);
MyCallback(CMD_STEP_FORWARD_SIMULATION,true,this);
MyCallback(CMD_RESET_SIMULATION,true,this);
}

45
examples/SharedMemory/PhysicsClientSharedMemory.cpp
View File

@@ -18,6 +18,7 @@
struct BodyJointInfoCache
{
std::string m_baseName;
btAlignedObjectArray<b3JointInfo> m_jointInfo;
};
@@ -74,10 +75,37 @@ struct PhysicsClientSharedMemoryInternalData {
int PhysicsClientSharedMemory::getNumJoints(int bodyIndex) const
int PhysicsClientSharedMemory::getNumBodies() const
{
BodyJointInfoCache** bodyJointsPtr = m_data->m_bodyJointMap[bodyIndex];
return m_data->m_bodyJointMap.size();
}
int PhysicsClientSharedMemory::getBodyUniqueId(int serialIndex) const
{
if ((serialIndex >= 0) && (serialIndex < getNumBodies()))
{
return m_data->m_bodyJointMap.getKeyAtIndex(serialIndex).getUid1();
}
return -1;
}
bool PhysicsClientSharedMemory::getBodyInfo(int bodyUniqueId, struct b3BodyInfo& info) const
{
BodyJointInfoCache** bodyJointsPtr = m_data->m_bodyJointMap[bodyUniqueId];
if (bodyJointsPtr && *bodyJointsPtr)
{
BodyJointInfoCache* bodyJoints = *bodyJointsPtr;
info.m_baseName = bodyJoints->m_baseName.c_str();
return true;
}
return false;
}
int PhysicsClientSharedMemory::getNumJoints(int bodyUniqueId) const
{
BodyJointInfoCache** bodyJointsPtr = m_data->m_bodyJointMap[bodyUniqueId];
if (bodyJointsPtr && *bodyJointsPtr)
{
BodyJointInfoCache* bodyJoints = *bodyJointsPtr;
@@ -88,9 +116,9 @@ int PhysicsClientSharedMemory::getNumJoints(int bodyIndex) const
}
bool PhysicsClientSharedMemory::getJointInfo(int bodyIndex, int jointIndex, b3JointInfo& info) const
bool PhysicsClientSharedMemory::getJointInfo(int bodyUniqueId, int jointIndex, b3JointInfo& info) const
{
BodyJointInfoCache** bodyJointsPtr = m_data->m_bodyJointMap[bodyIndex];
BodyJointInfoCache** bodyJointsPtr = m_data->m_bodyJointMap[bodyUniqueId];
if (bodyJointsPtr && *bodyJointsPtr)
{
BodyJointInfoCache* bodyJoints = *bodyJointsPtr;
@@ -196,12 +224,13 @@ void PhysicsClientSharedMemory::processBodyJointInfo(int bodyUniqueId, const Sha
Bullet::btMultiBodyDoubleData* mb =
(Bullet::btMultiBodyDoubleData*)bf.m_multiBodies[i];
bodyJoints->m_baseName = mb->m_baseName;
addJointInfoFromMultiBodyData(mb,bodyJoints, m_data->m_verboseOutput);
} else
{
Bullet::btMultiBodyFloatData* mb =
(Bullet::btMultiBodyFloatData*)bf.m_multiBodies[i];
bodyJoints->m_baseName = mb->m_baseName;
addJointInfoFromMultiBodyData(mb,bodyJoints, m_data->m_verboseOutput);
}
}
@@ -272,10 +301,10 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
serverCmd.m_dataStreamArguments.m_streamChunkLength);
bf.setFileDNAisMemoryDNA();
bf.parse(false);
int bodyIndex = serverCmd.m_dataStreamArguments.m_bodyUniqueId;
int bodyUniqueId = serverCmd.m_dataStreamArguments.m_bodyUniqueId;
BodyJointInfoCache* bodyJoints = new BodyJointInfoCache;
m_data->m_bodyJointMap.insert(bodyIndex,bodyJoints);
m_data->m_bodyJointMap.insert(bodyUniqueId,bodyJoints);
for (int i = 0; i < bf.m_multiBodies.size(); i++) {

10
examples/SharedMemory/PhysicsClientSharedMemory.h
View File

@@ -34,9 +34,15 @@ public:
virtual bool submitClientCommand(const struct SharedMemoryCommand& command);
virtual int getNumJoints(int bodyIndex) const;
virtual int getNumBodies() const;
virtual bool getJointInfo(int bodyIndex, int jointIndex, struct b3JointInfo& info) const;
virtual int getBodyUniqueId(int serialIndex) const;
virtual bool getBodyInfo(int bodyUniqueId, struct b3BodyInfo& info) const;
virtual int getNumJoints(int bodyUniqueId) const;
virtual bool getJointInfo(int bodyUniqueId, int jointIndex, struct b3JointInfo& info) const;
virtual void setSharedMemoryKey(int key);

40
examples/SharedMemory/PhysicsDirect.cpp
View File

@@ -9,10 +9,11 @@
#include "../../Extras/Serialize/BulletFileLoader/btBulletFile.h"
#include "../../Extras/Serialize/BulletFileLoader/autogenerated/bullet.h"
#include "BodyJointInfoUtility.h"
#include <string>
struct BodyJointInfoCache2
{
std::string m_baseName;
btAlignedObjectArray<b3JointInfo> m_jointInfo;
};
@@ -329,8 +330,7 @@ void PhysicsDirect::processBodyJointInfo(int bodyUniqueId, const SharedMemorySta
serverCmd.m_dataStreamArguments.m_streamChunkLength);
bf.setFileDNAisMemoryDNA();
bf.parse(false);
BodyJointInfoCache2* bodyJoints = new BodyJointInfoCache2;
m_data->m_bodyJointMap.insert(bodyUniqueId,bodyJoints);
@@ -341,13 +341,15 @@ void PhysicsDirect::processBodyJointInfo(int bodyUniqueId, const SharedMemorySta
{
Bullet::btMultiBodyDoubleData* mb =
(Bullet::btMultiBodyDoubleData*)bf.m_multiBodies[i];
bodyJoints->m_baseName = mb->m_baseName;
addJointInfoFromMultiBodyData(mb,bodyJoints, m_data->m_verboseOutput);
} else
{
Bullet::btMultiBodyFloatData* mb =
(Bullet::btMultiBodyFloatData*)bf.m_multiBodies[i];
bodyJoints->m_baseName = mb->m_baseName;
addJointInfoFromMultiBodyData(mb,bodyJoints, m_data->m_verboseOutput);
}
}
@@ -458,6 +460,34 @@ bool PhysicsDirect::submitClientCommand(const struct SharedMemoryCommand& comman
return hasStatus;
}
int PhysicsDirect::getNumBodies() const
{
return m_data->m_bodyJointMap.size();
}
int PhysicsDirect::getBodyUniqueId(int serialIndex) const
{
if ((serialIndex >= 0) && (serialIndex < getNumBodies()))
{
return m_data->m_bodyJointMap.getKeyAtIndex(serialIndex).getUid1();
}
return -1;
}
bool PhysicsDirect::getBodyInfo(int bodyUniqueId, struct b3BodyInfo& info) const
{
BodyJointInfoCache2** bodyJointsPtr = m_data->m_bodyJointMap[bodyUniqueId];
if (bodyJointsPtr && *bodyJointsPtr)
{
BodyJointInfoCache2* bodyJoints = *bodyJointsPtr;
info.m_baseName = bodyJoints->m_baseName.c_str();
return true;
}
return false;
}
int PhysicsDirect::getNumJoints(int bodyIndex) const
{
BodyJointInfoCache2** bodyJointsPtr = m_data->m_bodyJointMap[bodyIndex];

6
examples/SharedMemory/PhysicsDirect.h
View File

@@ -49,6 +49,12 @@ public:
virtual bool submitClientCommand(const struct SharedMemoryCommand& command);
virtual int getNumBodies() const;
virtual int getBodyUniqueId(int serialIndex) const;
virtual bool getBodyInfo(int bodyUniqueId, struct b3BodyInfo& info) const;
virtual int getNumJoints(int bodyIndex) const;
virtual bool getJointInfo(int bodyIndex, int jointIndex, struct b3JointInfo& info) const;

15
examples/SharedMemory/PhysicsLoopBack.cpp
View File

@@ -74,6 +74,21 @@ bool PhysicsLoopBack::submitClientCommand(const struct SharedMemoryCommand& comm
return m_data->m_physicsClient->submitClientCommand(command);
}
int PhysicsLoopBack::getNumBodies() const
{
return m_data->m_physicsClient->getNumBodies();
}
int PhysicsLoopBack::getBodyUniqueId(int serialIndex) const
{
return m_data->m_physicsClient->getBodyUniqueId(serialIndex);
}
bool PhysicsLoopBack::getBodyInfo(int bodyUniqueId, struct b3BodyInfo& info) const
{
return m_data->m_physicsClient->getBodyInfo(bodyUniqueId, info);
}
int PhysicsLoopBack::getNumJoints(int bodyIndex) const
{
return m_data->m_physicsClient->getNumJoints(bodyIndex);

6
examples/SharedMemory/PhysicsLoopBack.h
View File

@@ -38,6 +38,12 @@ public:
virtual bool submitClientCommand(const struct SharedMemoryCommand& command);
virtual int getNumBodies() const;
virtual int getBodyUniqueId(int serialIndex) const;
virtual bool getBodyInfo(int bodyUniqueId, struct b3BodyInfo& info) const;
virtual int getNumJoints(int bodyIndex) const;
virtual bool getJointInfo(int bodyIndex, int jointIndex, struct b3JointInfo& info) const;

47
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -1,6 +1,5 @@
#include "PhysicsServerCommandProcessor.h"
#include "../Importers/ImportURDFDemo/BulletUrdfImporter.h"
#include "../Importers/ImportURDFDemo/MyMultiBodyCreator.h"
#include "../Importers/ImportURDFDemo/URDF2Bullet.h"
@@ -569,7 +568,7 @@ PhysicsServerCommandProcessor::PhysicsServerCommandProcessor()
m_data = new PhysicsServerCommandProcessorInternalData();
createEmptyDynamicsWorld();
m_data->m_dynamicsWorld->getSolverInfo().m_linearSlop = 0.0001;
m_data->m_dynamicsWorld->getSolverInfo().m_linearSlop = 0.00001;
m_data->m_dynamicsWorld->getSolverInfo().m_numIterations = 100;
}
@@ -610,7 +609,7 @@ void PhysicsServerCommandProcessor::createEmptyDynamicsWorld()
m_data->m_dynamicsWorld->setGravity(btVector3(0, 0, 0));
m_data->m_dynamicsWorld->getSolverInfo().m_erp2 = 0.05;
m_data->m_dynamicsWorld->getSolverInfo().m_erp2 = 0.08;
}
@@ -1071,6 +1070,10 @@ int PhysicsServerCommandProcessor::createBodyInfoStream(int bodyUniqueId, char*
util->m_memSerializer = new btDefaultSerializer(bufferSizeInBytes ,(unsigned char*)bufferServerToClient);
//disable serialization of the collision objects (they are too big, and the client likely doesn't need them);
util->m_memSerializer->m_skipPointers.insert(mb->getBaseCollider(),0);
if (mb->getBaseName())
{
util->m_memSerializer->registerNameForPointer(mb->getBaseName(),mb->getBaseName());
}
bodyHandle->m_linkLocalInertialFrames.reserve(mb->getNumLinks());
for (int i=0;i<mb->getNumLinks();i++)
@@ -1893,6 +1896,9 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
applyJointDamping(i);
}
btScalar deltaTimeScaled = m_data->m_physicsDeltaTime*simTimeScalingFactor;
if (m_data->m_numSimulationSubSteps > 0)
@@ -2650,12 +2656,12 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
endEffectorPosWorld.serializeDouble(endEffectorWorldPosition);
endEffectorOri.serializeDouble(endEffectorWorldOrientation);
double dampIK[6] = { 1.0, 1.0, 1.0, 1.0, 1.0, 1.0 };
ikHelperPtr->computeIK(clientCmd.m_calculateInverseKinematicsArguments.m_targetPosition, clientCmd.m_calculateInverseKinematicsArguments.m_targetOrientation,
endEffectorWorldPosition.m_floats, endEffectorWorldOrientation.m_floats,
&q_current[0],
numDofs, clientCmd.m_calculateInverseKinematicsArguments.m_endEffectorLinkIndex,
&q_new[0], ikMethod, &jacobian_linear[0], &jacobian_angular[0], jacSize*2);
&q_new[0], ikMethod, &jacobian_linear[0], &jacobian_angular[0], jacSize*2, dampIK);
serverCmd.m_inverseKinematicsResultArgs.m_bodyUniqueId =clientCmd.m_calculateInverseDynamicsArguments.m_bodyUniqueId;
for (int i=0;i<numDofs;i++)
@@ -2825,6 +2831,7 @@ void PhysicsServerCommandProcessor::removePickingConstraint()
m_data->m_dynamicsWorld->removeConstraint(m_data->m_pickedConstraint);
delete m_data->m_pickedConstraint;
m_data->m_pickedConstraint = 0;
m_data->m_pickedBody->forceActivationState(ACTIVE_TAG);
m_data->m_pickedBody = 0;
}
if (m_data->m_pickingMultiBodyPoint2Point)
@@ -2894,11 +2901,12 @@ void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec)
btVector3 shiftPos = spawnDir*spawnDistance;
btVector3 spawnPos = gVRGripperPos + shiftPos;
loadUrdf("sphere_small.urdf", spawnPos, gVRGripperOrn, true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
//loadUrdf("lego/lego.urdf", spawnPos, gVRGripperOrn, true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
m_data->m_sphereId = bodyId;
InteralBodyData* parentBody = m_data->getHandle(bodyId);
if (parentBody->m_multiBody)
{
parentBody->m_multiBody->setBaseVel(spawnDir * 3);
parentBody->m_multiBody->setBaseVel(spawnDir * 5);
}
}
@@ -2941,11 +2949,15 @@ void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec)
}
}
loadUrdf("kuka_iiwa/model.urdf", btVector3(0, -2.3, 0.6), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
loadUrdf("kuka_iiwa/model.urdf", btVector3(0, -3.0, 0.0), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
m_data->m_KukaId = bodyId;
loadUrdf("lego/lego.urdf", btVector3(0, -2.5, .1), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
loadUrdf("lego/lego.urdf", btVector3(0, -2.5, .2), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
loadUrdf("lego/lego.urdf", btVector3(0, -2.5, .3), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
loadUrdf("r2d2.urdf", btVector3(2, -2, 1), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
// Load one motor gripper for kuka
loadSdf("gripper/wsg50_one_motor_gripper_new.sdf", &gBufferServerToClient[0], gBufferServerToClient.size(), true);
loadSdf("gripper/wsg50_one_motor_gripper_free_base.sdf", &gBufferServerToClient[0], gBufferServerToClient.size(), true);
m_data->m_gripperId = bodyId + 1;
InteralBodyData* kukaBody = m_data->getHandle(m_data->m_KukaId);
InteralBodyData* gripperBody = m_data->getHandle(m_data->m_gripperId);
@@ -2962,6 +2974,13 @@ void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec)
}
}
}
for (int i = 0; i < 6; i++)
{
loadUrdf("jenga/jenga.urdf", btVector3(-1-0.1*i,-0.5, .07), btQuaternion(btVector3(0,1,0),SIMD_HALF_PI), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
}
//loadUrdf("nao/nao.urdf", btVector3(2,5, 1), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
// Add slider joint for fingers
btVector3 pivotInParent1(0, 0, 0.06);
@@ -3005,7 +3024,6 @@ void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec)
loadUrdf("sphere2.urdf", btVector3(-5, 0, 1), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
loadUrdf("sphere2.urdf", btVector3(-5, 0, 2), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
loadUrdf("sphere2.urdf", btVector3(-5, 0, 3), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
loadUrdf("r2d2.urdf", btVector3(2, -2, 1), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
// Shelf area
loadSdf("kiva_shelf/model.sdf", &gBufferServerToClient[0], gBufferServerToClient.size(), true);
@@ -3013,15 +3031,6 @@ void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec)
loadUrdf("sphere_small.urdf", btVector3(-0.1, 0.6, 1.25), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
loadUrdf("cube_small.urdf", btVector3(0.3, 0.6, 0.85), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
// Table area
loadUrdf("table.urdf", btVector3(0, -1.9, 0.0), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
loadUrdf("tray.urdf", btVector3(0, -1.7, 0.64), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
loadUrdf("cup_small.urdf", btVector3(0.5, -2.0, 0.85), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
loadUrdf("pitcher_small.urdf", btVector3(0.4, -1.8, 0.85), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
loadUrdf("teddy_vhacd.urdf", btVector3(-0.1, -1.9, 0.7), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
loadUrdf("cube_small.urdf", btVector3(0.1, -1.8, 0.7), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
loadUrdf("sphere_small.urdf", btVector3(-0.2, -1.7, 0.7), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
// Chess area
loadUrdf("table_square.urdf", btVector3(2.0, 0, 0.0), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
loadUrdf("pawn.urdf", btVector3(1.8, -0.1, 0.7), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
@@ -3114,7 +3123,7 @@ void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec)
if (closeToKuka)
{
int dampIk = 1;
double dampIk[6] = {1.0, 1.0, 1.0, 1.0, 1.0, 0.0};
IKTrajectoryHelper** ikHelperPtrPtr = m_data->m_inverseKinematicsHelpers.find(bodyHandle->m_multiBody);
IKTrajectoryHelper* ikHelperPtr = 0;

14
examples/SharedMemory/PhysicsServerExample.cpp
View File

@@ -1181,14 +1181,20 @@ void PhysicsServerExample::vrControllerButtonCallback(int controllerId, int butt
} else
{
gDebugRenderToggle = 0;
simTimeScalingFactor *= 0.5;
if (simTimeScalingFactor==0)
{
simTimeScalingFactor = 1;
}
if (simTimeScalingFactor<0.01)
} else
{
simTimeScalingFactor = 0;
if (simTimeScalingFactor==1)
{
simTimeScalingFactor = 0.25;
}
else
{
simTimeScalingFactor = 0;
}
}
}
} else

7
examples/SharedMemory/SharedMemoryPublic.h
View File

@@ -117,6 +117,13 @@ struct b3JointInfo
double m_jointAxis[3]; // joint axis in parent local frame
};
struct b3BodyInfo
{
const char* m_baseName;
};
struct b3JointSensorState
{
double m_jointPosition;

3
examples/pybullet/premake4.lua
View File

@@ -3,8 +3,7 @@
project ("pybullet")
language "C++"
kind "SharedLib"
targetsuffix ("")
targetprefix ("")
includedirs {"../../src", "../../examples",
"../../examples/ThirdPartyLibs"}
defines {"PHYSICS_IN_PROCESS_EXAMPLE_BROWSER"}

117
examples/pybullet/pybullet.c
View File

@@ -567,7 +567,7 @@ static int pybullet_internalGetBasePositionAndOrientation(
const int status_type = b3GetStatusType(status_handle);
const double* actualStateQ;
// const double* jointReactionForces[];
int i;
if (status_type != CMD_ACTUAL_STATE_UPDATE_COMPLETED) {
PyErr_SetString(SpamError, "getBasePositionAndOrientation failed.");
@@ -661,10 +661,88 @@ static PyObject* pybullet_getBasePositionAndOrientation(PyObject* self,
}
}
static PyObject* pybullet_getNumBodies(PyObject* self, PyObject* args)
{
if (0 == sm) {
PyErr_SetString(SpamError, "Not connected to physics server.");
return NULL;
}
{
int numBodies = b3GetNumBodies(sm);
#if PY_MAJOR_VERSION >= 3
return PyLong_FromLong(numBodies);
#else
return PyInt_FromLong(numBodies);
#endif
}
}
static PyObject* pybullet_getBodyUniqueId(PyObject* self, PyObject* args)
{
if (0 == sm) {
PyErr_SetString(SpamError, "Not connected to physics server.");
return NULL;
}
{
int serialIndex = -1;
int bodyUniqueId = -1;
if (!PyArg_ParseTuple(args, "i", &serialIndex)) {
PyErr_SetString(SpamError, "Expected a serialIndex in range [0..number of bodies).");
return NULL;
}
bodyUniqueId = b3GetBodyUniqueId(sm, serialIndex);
#if PY_MAJOR_VERSION >= 3
return PyLong_FromLong(bodyUniqueId);
#else
return PyInt_FromLong(bodyUniqueId);
#endif
}
}
static PyObject* pybullet_getBodyInfo(PyObject* self, PyObject* args)
{
if (0 == sm) {
PyErr_SetString(SpamError, "Not connected to physics server.");
return NULL;
}
{
int bodyUniqueId= -1;
int numJoints = 0;
if (!PyArg_ParseTuple(args, "i", &bodyUniqueId))
{
PyErr_SetString(SpamError, "Expected a body unique id (integer).");
return NULL;
}
{
struct b3BodyInfo info;
if (b3GetBodyInfo(sm,bodyUniqueId,&info))
{
PyObject* pyListJointInfo = PyTuple_New(1);
PyTuple_SetItem(pyListJointInfo, 0, PyString_FromString(info.m_baseName));
return pyListJointInfo;
} else
{
PyErr_SetString(SpamError, "Couldn't get body info");
return NULL;
}
}
}
PyErr_SetString(SpamError, "error in getBodyInfo.");
return NULL;
}
// Return the number of joints in an object based on
// body index; body index is based on order of sequence
// the object is loaded into simulation
static PyObject* pybullet_getNumJoints(PyObject* self, PyObject* args) {
static PyObject* pybullet_getNumJoints(PyObject* self, PyObject* args)
{
if (0 == sm) {
PyErr_SetString(SpamError, "Not connected to physics server.");
return NULL;
@@ -888,16 +966,15 @@ static PyObject* pybullet_getJointInfo(PyObject* self, PyObject* args) {
static PyObject* pybullet_getJointState(PyObject* self, PyObject* args) {
PyObject* pyListJointForceTorque;
PyObject* pyListJointState;
PyObject* item;
struct b3JointInfo info;
struct b3JointSensorState sensorState;
int bodyIndex = -1;
int jointIndex = -1;
int sensorStateSize = 4; // size of struct b3JointSensorState
int forceTorqueSize = 6; // size of force torque list from b3JointSensorState
int i, j;
int j;
int size = PySequence_Size(args);
@@ -909,6 +986,11 @@ static PyObject* pybullet_getJointState(PyObject* self, PyObject* args) {
if (size == 2) // get body index and joint index
{
if (PyArg_ParseTuple(args, "ii", &bodyIndex, &jointIndex)) {
int status_type = 0;
b3SharedMemoryCommandHandle cmd_handle;
b3SharedMemoryStatusHandle status_handle;
if (bodyIndex < 0) {
PyErr_SetString(SpamError, "getJointState failed; invalid bodyIndex");
return NULL;
@@ -918,10 +1000,10 @@ static PyObject* pybullet_getJointState(PyObject* self, PyObject* args) {
return NULL;
}
int status_type = 0;
b3SharedMemoryCommandHandle cmd_handle =
cmd_handle =
b3RequestActualStateCommandInit(sm, bodyIndex);
b3SharedMemoryStatusHandle status_handle =
status_handle =
b3SubmitClientCommandAndWaitStatus(sm, cmd_handle);
status_type = b3GetStatusType(status_handle);
@@ -984,6 +1066,10 @@ static PyObject* pybullet_getLinkState(PyObject* self, PyObject* args) {
if (PySequence_Size(args) == 2) // body index and link index
{
if (PyArg_ParseTuple(args, "ii", &bodyIndex, &linkIndex)) {
int status_type = 0;
b3SharedMemoryCommandHandle cmd_handle;
b3SharedMemoryStatusHandle status_handle;
if (bodyIndex < 0) {
PyErr_SetString(SpamError, "getLinkState failed; invalid bodyIndex");
return NULL;
@@ -993,10 +1079,10 @@ static PyObject* pybullet_getLinkState(PyObject* self, PyObject* args) {
return NULL;
}
int status_type = 0;
b3SharedMemoryCommandHandle cmd_handle =
cmd_handle =
b3RequestActualStateCommandInit(sm, bodyIndex);
b3SharedMemoryStatusHandle status_handle =
status_handle =
b3SubmitClientCommandAndWaitStatus(sm, cmd_handle);
status_type = b3GetStatusType(status_handle);
@@ -1996,6 +2082,15 @@ static PyMethodDef SpamMethods[] = {
{"loadSDF", pybullet_loadSDF, METH_VARARGS,
"Load multibodies from an SDF file."},
{"getNumBodies", pybullet_getNumBodies, METH_VARARGS,
"Get the number of bodies in the simulation."},
{"getBodyUniqueId", pybullet_getBodyUniqueId, METH_VARARGS,
"Get the unique id of the body, given a integer serial index in range [0.. number of bodies)."},
{"getBodyInfo", pybullet_getBodyInfo, METH_VARARGS,
"Get the body info, given a body unique id."},
{"getBasePositionAndOrientation", pybullet_getBasePositionAndOrientation,
METH_VARARGS,
"Get the world position and orientation of the base of the object. "