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 pull request #689 from erwincoumans/master

Browse Source 
prepare robotics learning examples, see examples/RoboticsLearning/b3R…
This commit is contained in:
erwincoumans 2016-07-11 10:38:38 -07:00 committed by GitHub
commit e2e750efb2
25 changed files with 1175 additions and 41 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
data/cube.mtl
View File

@ -11,4 +11,6 @@ newmtl cube
Ke 0.0000 0.0000 0.0000
map_Ka cube.tga
map_Kd cube.png

32
data/cube.urdf Normal file
View File

@ -0,0 +1,32 @@
<?xml version="0.0" ?>
<robot name="cube.urdf">
<link name="baseLink">
<contact>
<lateral_friction value="1.0"/>
<rolling_friction value="0.0"/>
<contact_cfm value="0.0"/>
<contact_erp value="1.0"/>
</contact>
<inertial>
<origin rpy="0 0 0" xyz="0 0 0"/>
<mass value="1.0"/>
<inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="cube.obj" scale="1 1 1"/>
</geometry>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<box size="1 1 1"/>
</geometry>
</collision>
</link>
</robot>

24
data/plane.mtl
View File

@ -1,11 +1,15 @@
# Blender MTL File: 'None'
# Material Count: 1
newmtl Material
Ns 96.078431
Ka 0.000000 0.000000 0.000000
Kd 0.640000 0.640000 0.640000
Ks 0.500000 0.500000 0.500000
Ni 1.000000
d 1.000000
illum 2
Ns 10.0000
Ni 1.5000
d 1.0000
Tr 0.0000
Tf 1.0000 1.0000 1.0000
illum 2
Ka 0.0000 0.0000 0.0000
Kd 0.5880 0.5880 0.5880
Ks 0.0000 0.0000 0.0000
Ke 0.0000 0.0000 0.0000
map_Ka cube.tga
map_Kd checker_grid.jpg

18
data/plane.obj
View File

@ -2,11 +2,17 @@
# www.blender.org
mtllib plane.mtl
o Plane
v 1.000000 0.000000 -1.000000
v 1.000000 0.000000 1.000000
v -1.000000 0.000000 1.000000
v -1.000000 0.000000 -1.000000
v 5.000000 -5.000000 0.000000
v 5.000000 5.000000 0.000000
v -5.000000 5.000000 0.000000
v -5.000000 -5.000000 0.000000
vt 1.000000 0.000000
vt 1.000000 1.000000
vt 0.000000 1.000000
vt 0.000000 0.000000
usemtl Material
s off
f 1 2 3
f 1 3 4
f 1/1 2/2 3/3
f 1/1 3/3 4/4

2
data/r2d2.urdf
View File

@ -79,7 +79,7 @@
<cylinder length=".1" radius="0.035"/>
</geometry>
<material name="black">
<color rgba="0 0 0 1"/>
<color rgba="0.5 0.5 0.5 1"/>
</material>
</visual>
<collision>

3
data/sphere2.urdf
View File

@ -11,6 +11,9 @@
<geometry>
<mesh filename="textured_sphere_smooth.obj" scale="0.5 0.5 0.5"/>
</geometry>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>

5
examples/ExampleBrowser/CMakeLists.txt
View File

@ -189,7 +189,10 @@ SET(BulletExampleBrowser_SRCS
../RenderingExamples/TimeSeriesCanvas.h
../RenderingExamples/TimeSeriesFontData.cpp
../RenderingExamples/TimeSeriesFontData.h
../RoboticsLearning/b3RobotSimAPI.cpp
../RoboticsLearning/b3RobotSimAPI.h
../RoboticsLearning/R2D2GraspExample.cpp
../RoboticsLearning/R2D2GraspExample.h
../RenderingExamples/CoordinateSystemDemo.cpp
../RenderingExamples/CoordinateSystemDemo.h
../RenderingExamples/RaytracerSetup.cpp

10
examples/ExampleBrowser/ExampleEntries.cpp
View File

@ -45,6 +45,7 @@
#include "../Tutorial/Dof6ConstraintTutorial.h"
#include "../MultiThreading/MultiThreadingExample.h"
#include "../InverseDynamics/InverseDynamicsExample.h"
#include "../RoboticsLearning/R2D2GraspExample.h"
#ifdef ENABLE_LUA
#include "../LuaDemo/LuaPhysicsSetup.h"
@ -90,11 +91,11 @@ struct ExampleEntry
static ExampleEntry gDefaultExamples[]=
{
ExampleEntry(0,"API"),
ExampleEntry(1,"Basic Example","Create some rigid bodies using box collision shapes. This is a good example to familiarize with the basic initialization of Bullet. The Basic Example can also be compiled without graphical user interface, as a console application. Press W for wireframe, A to show AABBs, I to suspend/restart physics simulation. Press D to toggle auto-deactivation of the simulation. ", BasicExampleCreateFunc),
ExampleEntry(1,"Rolling Friction", "Damping is often not good enough to keep rounded objects from rolling down a sloped surface. Instead, you can set the rolling friction of a rigid body. Generally it is best to leave the rolling friction to zero, to avoid artifacts.", RollingFrictionCreateFunc),
ExampleEntry(1,"Constraints","Show the use of the various constraints in Bullet. Press the L key to visualize the constraint limits. Press the C key to visualize the constraint frames.",
@ -114,8 +115,6 @@ static ExampleEntry gDefaultExamples[]=
ExampleEntry(1,"Soft Contact", "Using the error correction parameter (ERP) and constraint force mixing (CFM) values for contacts to simulate compliant contact.",RigidBodySoftContactCreateFunc),
ExampleEntry(0,"MultiBody"),
ExampleEntry(1,"MultiDofCreateFunc","Create a basic btMultiBody with 3-DOF spherical joints (mobilizers). The demo uses a fixed base or a floating base at restart.", MultiDofCreateFunc),
ExampleEntry(1,"TestJointTorque","Apply a torque to a btMultiBody with 1-DOF joints (mobilizers). This setup is similar to API/TestHingeTorque.", TestJointTorqueCreateFunc),
@ -241,11 +240,12 @@ static ExampleEntry gDefaultExamples[]=
PhysicsServerCreateFunc,PHYSICS_SERVER_ENABLE_COMMAND_LOGGING),
ExampleEntry(1,"Physics Server (Replay Log)", "Create a physics server that replay a command log from disk.",
PhysicsServerCreateFunc,PHYSICS_SERVER_REPLAY_FROM_COMMAND_LOG),
ExampleEntry(1, "Physics Client (Shared Mem)", "Create a physics client that can communicate with a physics server over shared memory.", PhysicsClientCreateFunc),
ExampleEntry(1, "Physics Client (Direct)", "Create a physics client that can communicate with a physics server directly in-process.", PhysicsClientCreateFunc,eCLIENTEXAMPLE_DIRECT),
ExampleEntry(1,"R2D2 Grasp","Load the R2D2 robot from URDF file and control it to grasp objects", R2D2GraspExampleCreateFunc, eROBOTIC_LEARN_GRASP),
ExampleEntry(1,"URDF Compliant Contact","Experiment/improve compliant rigid contact using parameters from URDF file (contact_cfm, contact_erp, lateral_friction, rolling_friction)", R2D2GraspExampleCreateFunc,eROBOTIC_LEARN_COMPLIANT_CONTACT),
#ifdef ENABLE_LUA

1
examples/ExampleBrowser/premake4.lua
View File

@ -94,6 +94,7 @@ project "App_BulletExampleBrowser"
"../ExtendedTutorials/Bridge.cpp",
"../ExtendedTutorials/RigidBodyFromObj.cpp",
"../Collision/*",
"../RoboticsLearning/*",
"../Collision/Internal/*",
"../Benchmarks/*",
"../CommonInterfaces/*",

11
examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp
View File

@ -1005,6 +1005,17 @@ bool BulletURDFImporter::getLinkColor(int linkIndex, btVector4& colorRGBA) const
return false;
}
bool BulletURDFImporter::getLinkContactInfo(int linkIndex, URDFLinkContactInfo& contactInfo ) const
{
UrdfLink* const* linkPtr = m_data->m_urdfParser.getModel().m_links.getAtIndex(linkIndex);
if (linkPtr)
{
const UrdfLink* link = *linkPtr;
contactInfo = link->m_contactInfo;
return true;
}
return false;
}
void BulletURDFImporter::convertLinkVisualShapes2(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame, class btCollisionObject* colObj) const
{

2
examples/Importers/ImportURDFDemo/BulletUrdfImporter.h
View File

@ -37,6 +37,8 @@ public:
virtual std::string getLinkName(int linkIndex) const;
virtual bool getLinkColor(int linkIndex, btVector4& colorRGBA) const;
virtual bool getLinkContactInfo(int linkIndex, URDFLinkContactInfo& contactInfo ) const;
virtual std::string getJointName(int linkIndex) const;

4
examples/Importers/ImportURDFDemo/ImportURDFSetup.cpp
View File

@ -132,7 +132,7 @@ ImportUrdfSetup::ImportUrdfSetup(struct GUIHelperInterface* helper, int option,
if (gFileNameArray.size()==0)
{
gFileNameArray.push_back("sphere2.urdf");
gFileNameArray.push_back("r2d2.urdf");
}
@ -200,7 +200,7 @@ void ImportUrdfSetup::initPhysics()
btVector3 gravity(0,0,0);
gravity[upAxis]=-9.8;
//gravity[upAxis]=-9.8;
m_dynamicsWorld->setGravity(gravity);

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

@ -392,9 +392,18 @@ void ConvertURDF2BulletInternal(const URDFImporterInterface& u2b, MultiBodyCreat
u2b.convertLinkVisualShapes2(urdfLinkIndex,pathPrefix,localInertialFrame,col);
btScalar friction = 0.5f;
URDFLinkContactInfo contactInfo;
u2b.getLinkContactInfo(urdfLinkIndex,contactInfo);
col->setFriction(friction);
if ((contactInfo.m_flags & URDF_CONTACT_HAS_LATERAL_FRICTION)!=0)
{
col->setFriction(contactInfo.m_lateralFriction);
}
if ((contactInfo.m_flags & URDF_CONTACT_HAS_ROLLING_FRICTION)!=0)
{
col->setRollingFriction(contactInfo.m_rollingFriction);
}
if (mbLinkIndex>=0) //???? double-check +/- 1
{

4
examples/Importers/ImportURDFDemo/URDFImporterInterface.h
View File

@ -6,6 +6,7 @@
#include "LinearMath/btTransform.h"
#include "URDFJointTypes.h"
class URDFImporterInterface
{
@ -28,6 +29,9 @@ public:
virtual std::string getLinkName(int linkIndex) const =0;
/// optional method to provide the link color. return true if the color is available and copied into colorRGBA, return false otherwise
virtual bool getLinkColor(int linkIndex, btVector4& colorRGBA) const { return false;}
///this API will likely change, don't override it!
virtual bool getLinkContactInfo(int linkIndex, URDFLinkContactInfo& contactInfo ) const { return false;}
virtual std::string getJointName(int linkIndex) const = 0;

27
examples/Importers/ImportURDFDemo/URDFJointTypes.h
View File

@ -11,6 +11,33 @@ enum UrdfJointTypes
URDFPlanarJoint,
URDFFixedJoint,
};
#include "LinearMath/btScalar.h"
enum URDF_LinkContactFlags
{
URDF_CONTACT_HAS_LATERAL_FRICTION=1,
URDF_CONTACT_HAS_ROLLING_FRICTION=2,
URDF_CONTACT_HAS_CONTACT_CFM=4,
URDF_CONTACT_HAS_CONTACT_ERP=8
};
struct URDFLinkContactInfo
{
btScalar m_lateralFriction;
btScalar m_rollingFriction;
btScalar m_contactCfm;
btScalar m_contactErp;
int m_flags;
URDFLinkContactInfo()
:m_lateralFriction(0.5),
m_rollingFriction(0),
m_contactCfm(0),
m_contactErp(0)
{
m_flags = URDF_CONTACT_HAS_LATERAL_FRICTION;
}
};
#endif //URDF_JOINT_TYPES_H

25
examples/Importers/ImportURDFDemo/UrdfParser.cpp
View File

@ -569,6 +569,31 @@ bool UrdfParser::parseLink(UrdfModel& model, UrdfLink& link, TiXmlElement *confi
}
}
{
//optional 'contact' parameters
TiXmlElement* ci = config->FirstChildElement("contact");
if (ci)
{
TiXmlElement *friction_xml = ci->FirstChildElement("lateral_friction");
if (friction_xml)
{
if (m_parseSDF)
{
link.m_contactInfo.m_lateralFriction = urdfLexicalCast<double>(friction_xml->GetText());
} else
{
if (!friction_xml->Attribute("value"))
{
logger->reportError("Link/contact: lateral_friction element must have value attribute");
return false;
}
link.m_contactInfo.m_lateralFriction = urdfLexicalCast<double>(friction_xml->Attribute("value"));
}
}
}
}
// Inertial (optional)
TiXmlElement *i = config->FirstChildElement("inertial");
if (i)

2
examples/Importers/ImportURDFDemo/UrdfParser.h
View File

@ -99,6 +99,8 @@ struct UrdfLink
int m_linkIndex;
URDFLinkContactInfo m_contactInfo;
UrdfLink()
:m_parentLink(0),
m_parentJoint(0)

3
examples/InverseDynamics/premake4.lua
View File

@ -91,7 +91,8 @@ files {
"../Importers/ImportURDFDemo/UrdfParser.cpp",
"../Importers/ImportURDFDemo/urdfStringSplit.cpp",
"../Importers/ImportMeshUtility/b3ImportMeshUtility.cpp",
"../ThirdPartyLibs/stb_image/stb_image.cpp",
"../ThirdPartyLibs/stb_image/stb_image.cpp",
"../Utils/b3Clock.cpp",
}
if os.is("Linux") then initX11() end

164
examples/RoboticsLearning/R2D2GraspExample.cpp Normal file
View File

@ -0,0 +1,164 @@
#include "R2D2GraspExample.h"
#include "../CommonInterfaces/CommonGraphicsAppInterface.h"
#include "Bullet3Common/b3Quaternion.h"
#include "Bullet3Common/b3AlignedObjectArray.h"
#include "../CommonInterfaces/CommonRenderInterface.h"
#include "../CommonInterfaces/CommonExampleInterface.h"
#include "../CommonInterfaces/CommonGUIHelperInterface.h"
#include "../SharedMemory/PhysicsServerSharedMemory.h"
#include "../SharedMemory/PhysicsClientC_API.h"
#include <string>
#include "b3RobotSimAPI.h"
#include "../Utils/b3Clock.h"
///quick demo showing the right-handed coordinate system and positive rotations around each axis
class R2D2GraspExample : public CommonExampleInterface
{
CommonGraphicsApp* m_app;
GUIHelperInterface* m_guiHelper;
b3RobotSimAPI m_robotSim;
int m_options;
int m_r2d2Index;
float m_x;
float m_y;
float m_z;
b3AlignedObjectArray<int> m_movingInstances;
enum
{
numCubesX = 20,
numCubesY = 20
};
public:
R2D2GraspExample(GUIHelperInterface* helper, int options)
:m_app(helper->getAppInterface()),
m_guiHelper(helper),
m_options(options),
m_r2d2Index(-1),
m_x(0),
m_y(0),
m_z(0)
{
m_app->setUpAxis(2);
}
virtual ~R2D2GraspExample()
{
m_app->m_renderer->enableBlend(false);
}
virtual void physicsDebugDraw(int debugDrawMode)
{
}
virtual void initPhysics()
{
bool connected = m_robotSim.connect(m_guiHelper);
b3Printf("robotSim connected = %d",connected);
b3RobotSimLoadURDFArgs args("");
if ((m_options & eROBOTIC_LEARN_GRASP)!=0)
{
args.m_urdfFileName = "r2d2.urdf";
args.m_startPosition.setValue(0,0,.5);
m_r2d2Index = m_robotSim.loadURDF(args);
int numJoints = m_robotSim.getNumJoints(m_r2d2Index);
b3Printf("numJoints = %d",numJoints);
for (int i=0;i<numJoints;i++)
{
b3JointInfo jointInfo;
m_robotSim.getJointInfo(m_r2d2Index,i,&jointInfo);
b3Printf("joint[%d].m_jointName=%s",i,jointInfo.m_jointName);
}
int wheelJointIndices[4]={2,3,6,7};
int wheelTargetVelocities[4]={30,30,30,30};
for (int i=0;i<4;i++)
{
b3JointMotorArgs controlArgs(CONTROL_MODE_VELOCITY);
controlArgs.m_targetVelocity = wheelTargetVelocities[i];
controlArgs.m_maxTorqueValue = 1e30;
m_robotSim.setJointMotorControl(m_r2d2Index,wheelJointIndices[i],controlArgs);
}
}
if ((m_options & eROBOTIC_LEARN_COMPLIANT_CONTACT)!=0)
{
args.m_urdfFileName = "cube.urdf";
args.m_startPosition.setValue(0,0,2.5);
m_robotSim.loadURDF(args);
args.m_startOrientation.setEulerZYX(0,0.2,0);
}
args.m_urdfFileName = "plane.urdf";
args.m_startPosition.setValue(0,0,0);
args.m_forceOverrideFixedBase = true;
m_robotSim.loadURDF(args);
m_robotSim.setGravity(b3MakeVector3(0,0,-10));
}
virtual void exitPhysics()
{
m_robotSim.disconnect();
}
virtual void stepSimulation(float deltaTime)
{
m_robotSim.stepSimulation();
}
virtual void renderScene()
{
m_robotSim.renderScene();
//m_app->m_renderer->renderScene();
}
virtual void physicsDebugDraw()
{
}
virtual bool mouseMoveCallback(float x,float y)
{
return false;
}
virtual bool mouseButtonCallback(int button, int state, float x, float y)
{
return false;
}
virtual bool keyboardCallback(int key, int state)
{
return false;
}
virtual void resetCamera()
{
float dist = 10;
float pitch = 50;
float yaw = 13;
float targetPos[3]={-1,0,-0.3};
if (m_app->m_renderer && m_app->m_renderer->getActiveCamera())
{
m_app->m_renderer->getActiveCamera()->setCameraDistance(dist);
m_app->m_renderer->getActiveCamera()->setCameraPitch(pitch);
m_app->m_renderer->getActiveCamera()->setCameraYaw(yaw);
m_app->m_renderer->getActiveCamera()->setCameraTargetPosition(targetPos[0],targetPos[1],targetPos[2]);
}
}
};
class CommonExampleInterface* R2D2GraspExampleCreateFunc(struct CommonExampleOptions& options)
{
return new R2D2GraspExample(options.m_guiHelper, options.m_option);
}

28
examples/RoboticsLearning/R2D2GraspExample.h Normal file
View File

@ -0,0 +1,28 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2016 Google Inc. http://bulletphysics.org
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef R2D2_GRASP_EXAMPLE_H
#define R2D2_GRASP_EXAMPLE_H
enum RobotLearningExampleOptions
{
eROBOTIC_LEARN_GRASP=1,
eROBOTIC_LEARN_COMPLIANT_CONTACT=2,
};
class CommonExampleInterface* R2D2GraspExampleCreateFunc(struct CommonExampleOptions& options);
#endif //R2D2_GRASP_EXAMPLE_H

729
examples/RoboticsLearning/b3RobotSimAPI.cpp Normal file
View File

@ -0,0 +1,729 @@
#include "b3RobotSimAPI.h"
//#include "../CommonInterfaces/CommonGraphicsAppInterface.h"
#include "Bullet3Common/b3Quaternion.h"
#include "Bullet3Common/b3AlignedObjectArray.h"
#include "../CommonInterfaces/CommonRenderInterface.h"
//#include "../CommonInterfaces/CommonExampleInterface.h"
#include "../CommonInterfaces/CommonGUIHelperInterface.h"
#include "../SharedMemory/PhysicsServerSharedMemory.h"
#include "../SharedMemory/PhysicsClientC_API.h"
#include <string>
#include "../Utils/b3Clock.h"
#include "../MultiThreading/b3ThreadSupportInterface.h"
void RobotThreadFunc(void* userPtr,void* lsMemory);
void* RobotlsMemoryFunc();
#define MAX_ROBOT_NUM_THREADS 1
enum
{
numCubesX = 20,
numCubesY = 20
};
enum TestRobotSimCommunicationEnums
{
eRequestTerminateRobotSim= 13,
eRobotSimIsUnInitialized,
eRobotSimIsInitialized,
eRobotSimInitializationFailed,
eRobotSimHasTerminated
};
enum MultiThreadedGUIHelperCommunicationEnums
{
eRobotSimGUIHelperIdle= 13,
eRobotSimGUIHelperRegisterTexture,
eRobotSimGUIHelperRegisterGraphicsShape,
eRobotSimGUIHelperRegisterGraphicsInstance,
eRobotSimGUIHelperCreateCollisionShapeGraphicsObject,
eRobotSimGUIHelperCreateCollisionObjectGraphicsObject,
eRobotSimGUIHelperRemoveAllGraphicsInstances,
};
#include <stdio.h>
//#include "BulletMultiThreaded/PlatformDefinitions.h"
#ifndef _WIN32
#include "../MultiThreading/b3PosixThreadSupport.h"
b3ThreadSupportInterface* createRobotSimThreadSupport(int numThreads)
{
b3PosixThreadSupport::ThreadConstructionInfo constructionInfo("RobotSimThreads",
RobotThreadFunc,
RobotlsMemoryFunc,
numThreads);
b3ThreadSupportInterface* threadSupport = new b3PosixThreadSupport(constructionInfo);
return threadSupport;
}
#elif defined( _WIN32)
#include "../MultiThreading/b3Win32ThreadSupport.h"
b3ThreadSupportInterface* createRobotSimThreadSupport(int numThreads)
{
b3Win32ThreadSupport::Win32ThreadConstructionInfo threadConstructionInfo("RobotSimThreads",RobotThreadFunc,RobotlsMemoryFunc,numThreads);
b3Win32ThreadSupport* threadSupport = new b3Win32ThreadSupport(threadConstructionInfo);
return threadSupport;
}
#endif
struct RobotSimArgs
{
RobotSimArgs()
:m_physicsServerPtr(0)
{
}
b3CriticalSection* m_cs;
PhysicsServerSharedMemory* m_physicsServerPtr;
b3AlignedObjectArray<b3Vector3> m_positions;
};
struct RobotSimThreadLocalStorage
{
int threadId;
};
void RobotThreadFunc(void* userPtr,void* lsMemory)
{
printf("RobotThreadFunc thread started\n");
RobotSimThreadLocalStorage* localStorage = (RobotSimThreadLocalStorage*) lsMemory;
RobotSimArgs* args = (RobotSimArgs*) userPtr;
int workLeft = true;
b3Clock clock;
clock.reset();
bool init = true;
if (init)
{
args->m_cs->lock();
args->m_cs->setSharedParam(0,eRobotSimIsInitialized);
args->m_cs->unlock();
do
{
//todo(erwincoumans): do we want some sleep to reduce CPU resources in this thread?
#if 0
double deltaTimeInSeconds = double(clock.getTimeMicroseconds())/1000000.;
if (deltaTimeInSeconds<(1./260.))
{
if (deltaTimeInSeconds<.001)
continue;
}
clock.reset();
#endif //
args->m_physicsServerPtr->processClientCommands();
} while (args->m_cs->getSharedParam(0)!=eRequestTerminateRobotSim);
} else
{
args->m_cs->lock();
args->m_cs->setSharedParam(0,eRobotSimInitializationFailed);
args->m_cs->unlock();
}
//do nothing
}
void* RobotlsMemoryFunc()
{
//don't create local store memory, just return 0
return new RobotSimThreadLocalStorage;
}
ATTRIBUTE_ALIGNED16(class) MultiThreadedOpenGLGuiHelper : public GUIHelperInterface
{
CommonGraphicsApp* m_app;
b3CriticalSection* m_cs;
public:
BT_DECLARE_ALIGNED_ALLOCATOR();
GUIHelperInterface* m_childGuiHelper;
const unsigned char* m_texels;
int m_textureWidth;
int m_textureHeight;
int m_shapeIndex;
const float* m_position;
const float* m_quaternion;
const float* m_color;
const float* m_scaling;
const float* m_vertices;
int m_numvertices;
const int* m_indices;
int m_numIndices;
int m_primitiveType;
int m_textureId;
int m_instanceId;
MultiThreadedOpenGLGuiHelper(CommonGraphicsApp* app, GUIHelperInterface* guiHelper)
:m_app(app)
,m_cs(0),
m_texels(0),
m_textureId(-1)
{
m_childGuiHelper = guiHelper;;
}
virtual ~MultiThreadedOpenGLGuiHelper()
{
delete m_childGuiHelper;
}
void setCriticalSection(b3CriticalSection* cs)
{
m_cs = cs;
}
b3CriticalSection* getCriticalSection()
{
return m_cs;
}
virtual void createRigidBodyGraphicsObject(btRigidBody* body,const btVector3& color){}
btCollisionObject* m_obj;
btVector3 m_color2;
virtual void createCollisionObjectGraphicsObject(btCollisionObject* obj,const btVector3& color)
{
m_obj = obj;
m_color2 = color;
m_cs->lock();
m_cs->setSharedParam(1,eRobotSimGUIHelperCreateCollisionObjectGraphicsObject);
m_cs->unlock();
while (m_cs->getSharedParam(1)!=eRobotSimGUIHelperIdle)
{
}
}
btCollisionShape* m_colShape;
virtual void createCollisionShapeGraphicsObject(btCollisionShape* collisionShape)
{
m_colShape = collisionShape;
m_cs->lock();
m_cs->setSharedParam(1,eRobotSimGUIHelperCreateCollisionShapeGraphicsObject);
m_cs->unlock();
while (m_cs->getSharedParam(1)!=eRobotSimGUIHelperIdle)
{
}
}
virtual void syncPhysicsToGraphics(const btDiscreteDynamicsWorld* rbWorld)
{
//this check is to prevent a crash, in case we removed all graphics instances, but there are still physics objects.
//the check will be obsolete, once we have a better/safer way of synchronizing physics->graphics transforms
if ( m_childGuiHelper->getRenderInterface()->getTotalNumInstances()>0)
{
m_childGuiHelper->syncPhysicsToGraphics(rbWorld);
}
}
virtual void render(const btDiscreteDynamicsWorld* rbWorld)
{
m_childGuiHelper->render(0);
}
virtual void createPhysicsDebugDrawer( btDiscreteDynamicsWorld* rbWorld){}
virtual int registerTexture(const unsigned char* texels, int width, int height)
{
m_texels = texels;
m_textureWidth = width;
m_textureHeight = height;
m_cs->lock();
m_cs->setSharedParam(1,eRobotSimGUIHelperRegisterTexture);
m_cs->unlock();
while (m_cs->getSharedParam(1)!=eRobotSimGUIHelperIdle)
{
}
return m_textureId;
}
virtual int registerGraphicsShape(const float* vertices, int numvertices, const int* indices, int numIndices,int primitiveType, int textureId)
{
m_vertices = vertices;
m_numvertices = numvertices;
m_indices = indices;
m_numIndices = numIndices;
m_primitiveType = primitiveType;
m_textureId = textureId;
m_cs->lock();
m_cs->setSharedParam(1,eRobotSimGUIHelperRegisterGraphicsShape);
m_cs->unlock();
while (m_cs->getSharedParam(1)!=eRobotSimGUIHelperIdle)
{
}
return m_shapeIndex;
}
virtual int registerGraphicsInstance(int shapeIndex, const float* position, const float* quaternion, const float* color, const float* scaling)
{
m_shapeIndex = shapeIndex;
m_position = position;
m_quaternion = quaternion;
m_color = color;
m_scaling = scaling;
m_cs->lock();
m_cs->setSharedParam(1,eRobotSimGUIHelperRegisterGraphicsInstance);
m_cs->unlock();
while (m_cs->getSharedParam(1)!=eRobotSimGUIHelperIdle)
{
}
return m_instanceId;
}
virtual void removeAllGraphicsInstances()
{
m_cs->lock();
m_cs->setSharedParam(1,eRobotSimGUIHelperRemoveAllGraphicsInstances);
m_cs->unlock();
while (m_cs->getSharedParam(1)!=eRobotSimGUIHelperIdle)
{
}
}
virtual Common2dCanvasInterface* get2dCanvasInterface()
{
return 0;
}
virtual CommonParameterInterface* getParameterInterface()
{
return 0;
}
virtual CommonRenderInterface* getRenderInterface()
{
return 0;
}
virtual CommonGraphicsApp* getAppInterface()
{
return m_childGuiHelper->getAppInterface();
}
virtual void setUpAxis(int axis)
{
m_childGuiHelper->setUpAxis(axis);
}
virtual void resetCamera(float camDist, float pitch, float yaw, float camPosX,float camPosY, float camPosZ)
{
}
virtual void copyCameraImageData(unsigned char* pixelsRGBA, int rgbaBufferSizeInPixels, float* depthBuffer, int depthBufferSizeInPixels, int startPixelIndex, int* width, int* height, int* numPixelsCopied)
{
if (width)
*width = 0;
if (height)
*height = 0;
if (numPixelsCopied)
*numPixelsCopied = 0;
}
virtual void autogenerateGraphicsObjects(btDiscreteDynamicsWorld* rbWorld)
{
}
virtual void drawText3D( const char* txt, float posX, float posZY, float posZ, float size)
{
}
};
struct b3RobotSimAPI_InternalData
{
//GUIHelperInterface* m_guiHelper;
PhysicsServerSharedMemory m_physicsServer;
b3PhysicsClientHandle m_physicsClient;
b3ThreadSupportInterface* m_threadSupport;
RobotSimArgs m_args[MAX_ROBOT_NUM_THREADS];
MultiThreadedOpenGLGuiHelper* m_multiThreadedHelper;
bool m_connected;
b3RobotSimAPI_InternalData()
:m_multiThreadedHelper(0),
m_physicsClient(0),
m_connected(false)
{
}
};
b3RobotSimAPI::b3RobotSimAPI()
{
m_data = new b3RobotSimAPI_InternalData;
}
void b3RobotSimAPI::stepSimulation()
{
b3SharedMemoryStatusHandle statusHandle;
int statusType;
b3Assert(b3CanSubmitCommand(m_data->m_physicsClient));
if (b3CanSubmitCommand(m_data->m_physicsClient))
{
statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClient, b3InitStepSimulationCommand(m_data->m_physicsClient));
statusType = b3GetStatusType(statusHandle);
b3Assert(statusType==CMD_STEP_FORWARD_SIMULATION_COMPLETED);
}
}
void b3RobotSimAPI::setGravity(const b3Vector3& gravityAcceleration)
{
b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(m_data->m_physicsClient);
b3SharedMemoryStatusHandle statusHandle;
b3PhysicsParamSetGravity(command, gravityAcceleration[0],gravityAcceleration[1],gravityAcceleration[2]);
statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClient, command);
b3Assert(b3GetStatusType(statusHandle)==CMD_CLIENT_COMMAND_COMPLETED);
}
b3RobotSimAPI::~b3RobotSimAPI()
{
delete m_data;
}
void b3RobotSimAPI::processMultiThreadedGraphicsRequests()
{
switch (m_data->m_multiThreadedHelper->getCriticalSection()->getSharedParam(1))
{
case eRobotSimGUIHelperCreateCollisionShapeGraphicsObject:
{
m_data->m_multiThreadedHelper->m_childGuiHelper->createCollisionShapeGraphicsObject(m_data->m_multiThreadedHelper->m_colShape);
m_data->m_multiThreadedHelper->getCriticalSection()->lock();
m_data->m_multiThreadedHelper->getCriticalSection()->setSharedParam(1,eRobotSimGUIHelperIdle);
m_data->m_multiThreadedHelper->getCriticalSection()->unlock();
break;
}
case eRobotSimGUIHelperCreateCollisionObjectGraphicsObject:
{
m_data->m_multiThreadedHelper->m_childGuiHelper->createCollisionObjectGraphicsObject(m_data->m_multiThreadedHelper->m_obj,
m_data->m_multiThreadedHelper->m_color2);
m_data->m_multiThreadedHelper->getCriticalSection()->lock();
m_data->m_multiThreadedHelper->getCriticalSection()->setSharedParam(1,eRobotSimGUIHelperIdle);
m_data->m_multiThreadedHelper->getCriticalSection()->unlock();
break;
}
case eRobotSimGUIHelperRegisterTexture:
{
m_data->m_multiThreadedHelper->m_textureId = m_data->m_multiThreadedHelper->m_childGuiHelper->registerTexture(m_data->m_multiThreadedHelper->m_texels,
m_data->m_multiThreadedHelper->m_textureWidth,m_data->m_multiThreadedHelper->m_textureHeight);
m_data->m_multiThreadedHelper->getCriticalSection()->lock();
m_data->m_multiThreadedHelper->getCriticalSection()->setSharedParam(1,eRobotSimGUIHelperIdle);
m_data->m_multiThreadedHelper->getCriticalSection()->unlock();
break;
}
case eRobotSimGUIHelperRegisterGraphicsShape:
{
m_data->m_multiThreadedHelper->m_shapeIndex = m_data->m_multiThreadedHelper->m_childGuiHelper->registerGraphicsShape(
m_data->m_multiThreadedHelper->m_vertices,
m_data->m_multiThreadedHelper->m_numvertices,
m_data->m_multiThreadedHelper->m_indices,
m_data->m_multiThreadedHelper->m_numIndices,
m_data->m_multiThreadedHelper->m_primitiveType,
m_data->m_multiThreadedHelper->m_textureId);
m_data->m_multiThreadedHelper->getCriticalSection()->lock();
m_data->m_multiThreadedHelper->getCriticalSection()->setSharedParam(1,eRobotSimGUIHelperIdle);
m_data->m_multiThreadedHelper->getCriticalSection()->unlock();
break;
}
case eRobotSimGUIHelperRegisterGraphicsInstance:
{
m_data->m_multiThreadedHelper->m_instanceId = m_data->m_multiThreadedHelper->m_childGuiHelper->registerGraphicsInstance(
m_data->m_multiThreadedHelper->m_shapeIndex,
m_data->m_multiThreadedHelper->m_position,
m_data->m_multiThreadedHelper->m_quaternion,
m_data->m_multiThreadedHelper->m_color,
m_data->m_multiThreadedHelper->m_scaling);
m_data->m_multiThreadedHelper->getCriticalSection()->lock();
m_data->m_multiThreadedHelper->getCriticalSection()->setSharedParam(1,eRobotSimGUIHelperIdle);
m_data->m_multiThreadedHelper->getCriticalSection()->unlock();
break;
}
case eRobotSimGUIHelperRemoveAllGraphicsInstances:
{
m_data->m_multiThreadedHelper->m_childGuiHelper->removeAllGraphicsInstances();
int numRenderInstances = m_data->m_multiThreadedHelper->m_childGuiHelper->getRenderInterface()->getTotalNumInstances();
b3Assert(numRenderInstances==0);
m_data->m_multiThreadedHelper->getCriticalSection()->lock();
m_data->m_multiThreadedHelper->getCriticalSection()->setSharedParam(1,eRobotSimGUIHelperIdle);
m_data->m_multiThreadedHelper->getCriticalSection()->unlock();
break;
}
case eRobotSimGUIHelperIdle:
default:
{
}
}
#if 0
if (m_options == PHYSICS_SERVER_USE_RTC_CLOCK)
{
btClock rtc;
btScalar endTime = rtc.getTimeMilliseconds() + deltaTime*btScalar(800);
while (rtc.getTimeMilliseconds()<endTime)
{
m_physicsServer.processClientCommands();
}
} else
{
//for (int i=0;i<10;i++)
m_physicsServer.processClientCommands();
}
#endif
}
b3SharedMemoryStatusHandle b3RobotSimAPI::submitClientCommandAndWaitStatusMultiThreaded(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle)
{
int timeout = 1024*1024*1024;
b3SharedMemoryStatusHandle statusHandle=0;
b3SubmitClientCommand(physClient,commandHandle);
while ((statusHandle==0) && (timeout-- > 0))
{
statusHandle =b3ProcessServerStatus(physClient);
processMultiThreadedGraphicsRequests();
}
return (b3SharedMemoryStatusHandle) statusHandle;
}
int b3RobotSimAPI::getNumJoints(int bodyUniqueId) const
{
return b3GetNumJoints(m_data->m_physicsClient,bodyUniqueId);
}
bool b3RobotSimAPI::getJointInfo(int bodyUniqueId, int jointIndex, b3JointInfo* jointInfo)
{
return (b3GetJointInfo(m_data->m_physicsClient,bodyUniqueId, jointIndex,jointInfo)!=0);
}
void b3RobotSimAPI::setJointMotorControl(int bodyUniqueId, int jointIndex, const b3JointMotorArgs& args)
{
b3SharedMemoryStatusHandle statusHandle;
switch (args.m_controlMode)
{
case CONTROL_MODE_VELOCITY:
{
b3SharedMemoryCommandHandle command = b3JointControlCommandInit( m_data->m_physicsClient, CONTROL_MODE_VELOCITY);
b3JointInfo jointInfo;
b3GetJointInfo(m_data->m_physicsClient, bodyUniqueId, jointIndex, &jointInfo);
int uIndex = jointInfo.m_uIndex;
b3JointControlSetDesiredVelocity(command,uIndex,args.m_targetVelocity);
b3JointControlSetMaximumForce(command,uIndex,args.m_maxTorqueValue);
statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClient, command);
break;
}
case CONTROL_MODE_POSITION_VELOCITY_PD:
{
b3SharedMemoryCommandHandle command = b3JointControlCommandInit( m_data->m_physicsClient, CONTROL_MODE_POSITION_VELOCITY_PD);
b3JointInfo jointInfo;
b3GetJointInfo(m_data->m_physicsClient, bodyUniqueId, jointIndex, &jointInfo);
int uIndex = jointInfo.m_uIndex;
int qIndex = jointInfo.m_qIndex;
b3JointControlSetDesiredPosition(command,qIndex,args.m_targetPosition);
b3JointControlSetKp(command,uIndex,args.m_kp);
b3JointControlSetDesiredVelocity(command,uIndex,args.m_targetVelocity);
b3JointControlSetKd(command,uIndex,args.m_kd);
b3JointControlSetMaximumForce(command,uIndex,args.m_maxTorqueValue);
statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClient, command);
break;
}
case CONTROL_MODE_TORQUE:
{
b3SharedMemoryCommandHandle command = b3JointControlCommandInit( m_data->m_physicsClient, CONTROL_MODE_TORQUE);
b3JointInfo jointInfo;
b3GetJointInfo(m_data->m_physicsClient, bodyUniqueId, jointIndex, &jointInfo);
int uIndex = jointInfo.m_uIndex;
b3JointControlSetDesiredForceTorque(command,uIndex,args.m_maxTorqueValue);
statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClient, command);
break;
}
default:
{
b3Error("Unknown control command in b3RobotSimAPI::setJointMotorControl");
}
}
}
int b3RobotSimAPI::loadURDF(const b3RobotSimLoadURDFArgs& args)
{
int robotUniqueId = -1;
b3Assert(m_data->m_connected);
{
b3SharedMemoryStatusHandle statusHandle;
int statusType;
b3SharedMemoryCommandHandle command = b3LoadUrdfCommandInit(m_data->m_physicsClient, args.m_urdfFileName.c_str());
//setting the initial position, orientation and other arguments are optional
b3LoadUrdfCommandSetStartPosition(command, args.m_startPosition[0],
args.m_startPosition[1],
args.m_startPosition[2]);
b3LoadUrdfCommandSetStartOrientation(command,args.m_startOrientation[0]
,args.m_startOrientation[1]
,args.m_startOrientation[2]
,args.m_startOrientation[3]);
if (args.m_forceOverrideFixedBase)
{
b3LoadUrdfCommandSetUseFixedBase(command,true);
}
statusHandle = submitClientCommandAndWaitStatusMultiThreaded(m_data->m_physicsClient, command);
statusType = b3GetStatusType(statusHandle);
b3Assert(statusType==CMD_URDF_LOADING_COMPLETED);
robotUniqueId = b3GetStatusBodyIndex(statusHandle);
}
return robotUniqueId;
}
bool b3RobotSimAPI::connect(GUIHelperInterface* guiHelper)
{
m_data->m_multiThreadedHelper = new MultiThreadedOpenGLGuiHelper(guiHelper->getAppInterface(),guiHelper);
MultiThreadedOpenGLGuiHelper* guiHelperWrapper = new MultiThreadedOpenGLGuiHelper(guiHelper->getAppInterface(),guiHelper);
m_data->m_threadSupport = createRobotSimThreadSupport(MAX_ROBOT_NUM_THREADS);
for (int i=0;i<m_data->m_threadSupport->getNumTasks();i++)
{
RobotSimThreadLocalStorage* storage = (RobotSimThreadLocalStorage*) m_data->m_threadSupport->getThreadLocalMemory(i);
b3Assert(storage);
storage->threadId = i;
//storage->m_sharedMem = data->m_sharedMem;
}
for (int w=0;w<MAX_ROBOT_NUM_THREADS;w++)
{
m_data->m_args[w].m_cs = m_data->m_threadSupport->createCriticalSection();
m_data->m_args[w].m_cs->setSharedParam(0,eRobotSimIsUnInitialized);
int numMoving = 0;
m_data->m_args[w].m_positions.resize(numMoving);
m_data->m_args[w].m_physicsServerPtr = &m_data->m_physicsServer;
int index = 0;
m_data->m_threadSupport->runTask(B3_THREAD_SCHEDULE_TASK, (void*) &m_data->m_args[w], w);
while (m_data->m_args[w].m_cs->getSharedParam(0)==eRobotSimIsUnInitialized)
{
}
}
m_data->m_args[0].m_cs->setSharedParam(1,eRobotSimGUIHelperIdle);
m_data->m_multiThreadedHelper->setCriticalSection(m_data->m_args[0].m_cs);
m_data->m_connected = m_data->m_physicsServer.connectSharedMemory( m_data->m_multiThreadedHelper);
b3Assert(m_data->m_connected);
m_data->m_physicsClient = b3ConnectSharedMemory(SHARED_MEMORY_KEY);
int canSubmit = b3CanSubmitCommand(m_data->m_physicsClient);
b3Assert(canSubmit);
return m_data->m_connected && canSubmit;
}
void b3RobotSimAPI::disconnect()
{
for (int i=0;i<MAX_ROBOT_NUM_THREADS;i++)
{
m_data->m_args[i].m_cs->lock();
m_data->m_args[i].m_cs->setSharedParam(0,eRequestTerminateRobotSim);
m_data->m_args[i].m_cs->unlock();
}
int numActiveThreads = MAX_ROBOT_NUM_THREADS;
while (numActiveThreads)
{
int arg0,arg1;
if (m_data->m_threadSupport->isTaskCompleted(&arg0,&arg1,0))
{
numActiveThreads--;
printf("numActiveThreads = %d\n",numActiveThreads);
} else
{
}
};
printf("stopping threads\n");
delete m_data->m_threadSupport;
m_data->m_threadSupport = 0;
b3DisconnectSharedMemory(m_data->m_physicsClient);
m_data->m_physicsServer.disconnectSharedMemory(true);
m_data->m_connected = false;
}
void b3RobotSimAPI::renderScene()
{
if (m_data->m_multiThreadedHelper->m_childGuiHelper->getRenderInterface())
{
m_data->m_multiThreadedHelper->m_childGuiHelper->getRenderInterface()->writeTransforms();
}
m_data->m_physicsServer.renderScene();
}

81
examples/RoboticsLearning/b3RobotSimAPI.h Normal file
View File

@ -0,0 +1,81 @@
#ifndef B3_ROBOT_SIM_API_H
#define B3_ROBOT_SIM_API_H
///todo: remove those includes from this header
#include "../SharedMemory/PhysicsClientC_API.h"
#include "../SharedMemory/SharedMemoryPublic.h"
#include "Bullet3Common/b3Vector3.h"
#include "Bullet3Common/b3Quaternion.h"
#include <string>
struct b3RobotSimLoadURDFArgs
{
std::string m_urdfFileName;
b3Vector3 m_startPosition;
b3Quaternion m_startOrientation;
bool m_forceOverrideFixedBase;
b3RobotSimLoadURDFArgs(const std::string& urdfFileName)
:m_urdfFileName(urdfFileName),
m_startPosition(b3MakeVector3(0,0,0)),
m_startOrientation(b3Quaternion(0,0,0,1)),
m_forceOverrideFixedBase(false)
{
}
};
struct b3JointMotorArgs
{
int m_controlMode;
double m_targetPosition;
double m_kp;
double m_targetVelocity;
double m_kd;
double m_maxTorqueValue;
b3JointMotorArgs(int controlMode)
:m_controlMode(controlMode),
m_targetPosition(0),
m_kp(0.1),
m_targetVelocity(0),
m_kd(0.1),
m_maxTorqueValue(1000)
{
}
};
class b3RobotSimAPI
{
struct b3RobotSimAPI_InternalData* m_data;
void processMultiThreadedGraphicsRequests();
b3SharedMemoryStatusHandle submitClientCommandAndWaitStatusMultiThreaded(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle);
public:
b3RobotSimAPI();
virtual ~b3RobotSimAPI();
bool connect(struct GUIHelperInterface* guiHelper);
void disconnect();
int loadURDF(const struct b3RobotSimLoadURDFArgs& args);
int getNumJoints(int bodyUniqueId) const;
bool getJointInfo(int bodyUniqueId, int jointIndex, b3JointInfo* jointInfo);
void setJointMotorControl(int bodyUniqueId, int jointIndex, const struct b3JointMotorArgs& args);
void stepSimulation();
void setGravity(const b3Vector3& gravityAcceleration);
void renderScene();
};
#endif //B3_ROBOT_SIM_API_H

12
examples/SharedMemory/PhysicsServerExample.cpp
View File

@ -1,5 +1,6 @@
//todo(erwincoumans): re-use the upcoming b3RobotSimAPI here
#include "PhysicsServerExample.h"
@ -12,8 +13,6 @@
#include "../Utils/b3Clock.h"
#include "../MultiThreading/b3ThreadSupportInterface.h"
int blockme = false;
int blockme2 = false;
void MotionThreadFunc(void* userPtr,void* lsMemory);
void* MotionlsMemoryFunc();
@ -116,6 +115,8 @@ void MotionThreadFunc(void* userPtr,void* lsMemory)
do
{
//todo(erwincoumans): do we want some sleep to reduce CPU resources in this thread?
#if 0
double deltaTimeInSeconds = double(clock.getTimeMicroseconds())/1000000.;
if (deltaTimeInSeconds<(1./260.))
{
@ -125,11 +126,9 @@ void MotionThreadFunc(void* userPtr,void* lsMemory)
}
clock.reset();
#endif
args->m_physicsServerPtr->processClientCommands();
if (!blockme)
{
args->m_physicsServerPtr->processClientCommands();
}
} while (args->m_cs->getSharedParam(0)!=eRequestTerminateMotion);
} else
@ -710,7 +709,6 @@ void PhysicsServerExample::stepSimulation(float deltaTime)
}
#endif
if (!blockme2)
{
if (m_multiThreadedHelper->m_childGuiHelper->getRenderInterface())
{

11
src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp
View File

@ -634,13 +634,14 @@ void btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold*
#define ENABLE_FRICTION
#ifdef ENABLE_FRICTION
solverConstraint.m_frictionIndex = frictionIndex;
#if ROLLING_FRICTION
//#define ROLLING_FRICTION
#ifdef ROLLING_FRICTION
int rollingFriction=1;
btVector3 angVelA(0,0,0),angVelB(0,0,0);
if (rb0)
angVelA = rb0->getAngularVelocity();
if (rb1)
angVelB = rb1->getAngularVelocity();
if (mbA)
angVelA = mbA->getVelocityVector()>getLink(fcA->m_link).l>getAngularVelocity();
if (mbB)
angVelB = mbB->getAngularVelocity();
btVector3 relAngVel = angVelB-angVelA;
if ((cp.m_combinedRollingFriction>0.f) && (rollingFriction>0))

3
test/SharedMemory/test.c
View File

@ -58,6 +58,7 @@ void testSharedMemory(b3PhysicsClientHandle sm)
int statusType;
int bodyIndicesOut[10];//MAX_SDF_BODIES = 10
int numJoints, numBodies;
int bodyUniqueId;
b3SharedMemoryCommandHandle command = b3LoadSdfCommandInit(sm, sdfFileName);
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
statusType = b3GetStatusType(statusHandle);
@ -65,7 +66,7 @@ void testSharedMemory(b3PhysicsClientHandle sm)
numBodies = b3GetStatusBodyIndices(statusHandle, bodyIndicesOut, 10);
ASSERT_EQ(numBodies,1);
int bodyUniqueId = bodyIndicesOut[0];
bodyUniqueId = bodyIndicesOut[0];
numJoints = b3GetNumJoints(sm,bodyUniqueId);
ASSERT_EQ(numJoints,7);