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

Add API to set bunny properties. Add example to show coupling between softbody and multibody.

Browse Source
This commit is contained in:
yunfeibai
2016-11-01 16:45:10 -07:00
parent 9d54f0cf8d
commit 3c37db0804
10 changed files with 115 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
examples/ExampleBrowser/ExampleEntries.cpp
View File

@@ -269,6 +269,7 @@ static ExampleEntry gDefaultExamples[]=
ExampleEntry(1,"Two Point Grasp","Grasp experiment with two point contact to test rolling friction", GripperGraspExampleCreateFunc, eTWO_POINT_GRASP),
ExampleEntry(1,"One Motor Gripper Grasp","Grasp experiment with a gripper with one motor to test slider constraint for closed loop structure", GripperGraspExampleCreateFunc, eONE_MOTOR_GRASP),
ExampleEntry(1,"Grasp Soft Body","Grasp soft body experiment", GripperGraspExampleCreateFunc, eGRASP_SOFT_BODY),
ExampleEntry(1,"Softbody Multibody Coupling","Two way coupling between soft body and multibody experiment", GripperGraspExampleCreateFunc, eSOFTBODY_MULTIBODY_COUPLING),
#ifdef ENABLE_LUA

42
examples/RoboticsLearning/GripperGraspExample.cpp
View File

@@ -408,7 +408,7 @@ public:
}
m_robotSim.setGravity(b3MakeVector3(0,0,-10));
m_robotSim.loadBunny();
m_robotSim.loadBunny(0.1,0.1,0.02);
b3JointInfo revoluteJoint1;
revoluteJoint1.m_parentFrame[0] = -0.007;
@@ -571,6 +571,46 @@ public:
m_robotSim.createJoint(0, 2, 0, 4, &revoluteJoint7);
m_robotSim.createJoint(0, 3, 0, 5, &revoluteJoint8);
}
if ((m_options & eSOFTBODY_MULTIBODY_COUPLING)!=0)
{
{
b3RobotSimLoadFileArgs args("");
args.m_fileName = "kuka_iiwa/model_free_base.urdf";
args.m_startPosition.setValue(0,1.0,2.0);
args.m_startOrientation.setEulerZYX(0,0,1.57);
args.m_forceOverrideFixedBase = false;
args.m_useMultiBody = true;
b3RobotSimLoadFileResults results;
m_robotSim.loadFile(args,results);
int kukaId = results.m_uniqueObjectIds[0];
int numJoints = m_robotSim.getNumJoints(kukaId);
b3Printf("numJoints = %d",numJoints);
for (int i=0;i<numJoints;i++)
{
b3JointInfo jointInfo;
m_robotSim.getJointInfo(kukaId,i,&jointInfo);
b3Printf("joint[%d].m_jointName=%s",i,jointInfo.m_jointName);
b3JointMotorArgs controlArgs(CONTROL_MODE_VELOCITY);
controlArgs.m_maxTorqueValue = 0.0;
m_robotSim.setJointMotorControl(kukaId,i,controlArgs);
}
}
{
b3RobotSimLoadFileArgs args("");
args.m_fileName = "plane.urdf";
args.m_startPosition.setValue(0,0,0);
args.m_startOrientation.setEulerZYX(0,0,0);
args.m_forceOverrideFixedBase = true;
args.m_useMultiBody = true;
b3RobotSimLoadFileResults results;
m_robotSim.loadFile(args,results);
}
m_robotSim.setGravity(b3MakeVector3(0,0,-10));
m_robotSim.loadBunny(0.5,10.0,0.1);
}
}
virtual void exitPhysics()
{

1
examples/RoboticsLearning/GripperGraspExample.h
View File

@@ -22,6 +22,7 @@ enum GripperGraspExampleOptions
eTWO_POINT_GRASP=2,
eONE_MOTOR_GRASP=4,
eGRASP_SOFT_BODY=8,
eSOFTBODY_MULTIBODY_COUPLING=16,
};
class CommonExampleInterface* GripperGraspExampleCreateFunc(struct CommonExampleOptions& options);

7
examples/RoboticsLearning/b3RobotSimAPI.cpp
View File

@@ -1000,8 +1000,11 @@ void b3RobotSimAPI::getLinkState(int bodyUniqueId, int linkIndex, double* worldP
}
}
void b3RobotSimAPI::loadBunny()
void b3RobotSimAPI::loadBunny(double scale, double mass, double collisionMargin)
{
b3SharedMemoryCommandHandle command = b3LoadBunnyCommandInit(m_data->m_physicsClient);
b3LoadBunnySetScale(command, scale);
b3LoadBunnySetMass(command, mass);
b3LoadBunnySetCollisionMargin(command, collisionMargin);
b3SubmitClientCommand(m_data->m_physicsClient, command);
}
}

2
examples/RoboticsLearning/b3RobotSimAPI.h
View File

@@ -165,7 +165,7 @@ public:
void getLinkState(int bodyUniqueId, int linkIndex, double* worldPosition, double* worldOrientation);
void loadBunny();
void loadBunny(double scale, double mass, double collisionMargin);
};
#endif //B3_ROBOT_SIM_API_H

29
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@@ -89,6 +89,33 @@ b3SharedMemoryCommandHandle b3LoadBunnyCommandInit(b3PhysicsClientHandle physCli
return (b3SharedMemoryCommandHandle) command;
}
int b3LoadBunnySetScale(b3SharedMemoryCommandHandle commandHandle, double scale)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command->m_type == CMD_LOAD_BUNNY);
command->m_loadBunnyArguments.m_scale = scale;
command->m_updateFlags |= LOAD_BUNNY_UPDATE_SCALE;
return 0;
}
int b3LoadBunnySetMass(b3SharedMemoryCommandHandle commandHandle, double mass)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command->m_type == CMD_LOAD_BUNNY);
command->m_loadBunnyArguments.m_mass = mass;
command->m_updateFlags |= LOAD_BUNNY_UPDATE_MASS;
return 0;
}
int b3LoadBunnySetCollisionMargin(b3SharedMemoryCommandHandle commandHandle, double collisionMargin)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command->m_type == CMD_LOAD_BUNNY);
command->m_loadBunnyArguments.m_collisionMargin = collisionMargin;
command->m_updateFlags |= LOAD_BUNNY_UPDATE_COLLISION_MARGIN;
return 0;
}
int b3LoadUrdfCommandSetUseMultiBody(b3SharedMemoryCommandHandle commandHandle, int useMultiBody)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
@@ -1580,4 +1607,4 @@ int b3GetStatusInverseKinematicsJointPositions(b3SharedMemoryStatusHandle status
}
return true;
}
}

3
examples/SharedMemory/PhysicsClientC_API.h
View File

@@ -229,6 +229,9 @@ void b3ApplyExternalForce(b3SharedMemoryCommandHandle commandHandle, int bodyUni
void b3ApplyExternalTorque(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkId, const double torque[3], int flags);
b3SharedMemoryCommandHandle b3LoadBunnyCommandInit(b3PhysicsClientHandle physClient);
int b3LoadBunnySetScale(b3SharedMemoryCommandHandle commandHandle, double scale);
int b3LoadBunnySetMass(b3SharedMemoryCommandHandle commandHandle, double mass);
int b3LoadBunnySetCollisionMargin(b3SharedMemoryCommandHandle commandHandle, double collisionMargin);
#ifdef __cplusplus
}

21
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -1629,6 +1629,21 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
case CMD_LOAD_BUNNY:
{
#ifdef USE_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
double scale = 0.1;
double mass = 0.1;
double collisionMargin = 0.02;
if (clientCmd.m_updateFlags & LOAD_BUNNY_UPDATE_SCALE)
{
scale = clientCmd.m_loadBunnyArguments.m_scale;
}
if (clientCmd.m_updateFlags & LOAD_BUNNY_UPDATE_MASS)
{
mass = clientCmd.m_loadBunnyArguments.m_mass;
}
if (clientCmd.m_updateFlags & LOAD_BUNNY_UPDATE_COLLISION_MARGIN)
{
collisionMargin = clientCmd.m_loadBunnyArguments.m_collisionMargin;
}
m_data->m_softBodyWorldInfo.air_density = (btScalar)1.2;
m_data->m_softBodyWorldInfo.water_density = 0;
m_data->m_softBodyWorldInfo.water_offset = 0;
@@ -1648,9 +1663,9 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
psb->randomizeConstraints();
psb->rotate(btQuaternion(0.70711,0,0,0.70711));
psb->translate(btVector3(0,0,1.0));
psb->scale(btVector3(0.1,0.1,0.1));
psb->setTotalMass(0.1,true);
psb->getCollisionShape()->setMargin(0.02);
psb->scale(btVector3(scale,scale,scale));
psb->setTotalMass(mass,true);
psb->getCollisionShape()->setMargin(collisionMargin);
m_data->m_dynamicsWorld->addSoftBody(psb);
#endif

15
examples/SharedMemory/SharedMemoryCommands.h
View File

@@ -255,6 +255,13 @@ enum EnumSimParamUpdateFlags
SIM_PARAM_UPDATE_INTERNAL_SIMULATION_FLAGS=64
};
enum EnumLoadBunnyUpdateFlags
{
LOAD_BUNNY_UPDATE_SCALE=1,
LOAD_BUNNY_UPDATE_MASS=2,
LOAD_BUNNY_UPDATE_COLLISION_MARGIN=4
};
enum EnumSimParamInternalSimFlags
{
SIM_PARAM_INTERNAL_CREATE_ROBOT_ASSETS=1,
@@ -274,6 +281,13 @@ struct SendPhysicsSimulationParameters
double m_defaultContactERP;
};
struct LoadBunnyArgs
{
double m_scale;
double m_mass;
double m_collisionMargin;
};
struct RequestActualStateArgs
{
int m_bodyUniqueId;
@@ -501,6 +515,7 @@ struct SharedMemoryCommand
struct UpdateVisualShapeDataArgs m_updateVisualShapeDataArguments;
struct LoadTextureArgs m_loadTextureArguments;
struct CalculateInverseKinematicsArgs m_calculateInverseKinematicsArguments;
struct LoadBunnyArgs m_loadBunnyArguments;
};
};

2
src/BulletSoftBody/btSoftBody.cpp
View File

@@ -18,6 +18,8 @@ subject to the following restrictions:
#include "BulletSoftBody/btSoftBodySolvers.h"
#include "btSoftBodyData.h"
#include "LinearMath/btSerializer.h"
#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
#include "BulletDynamics/Featherstone/btMultiBodyConstraint.h"
//