AuroraMiddleware/bullet3
1
0
Fork 0
mirror of https://github.com/bulletphysics/bullet3 synced 2024-12-14 05:40:05 +00:00
Code Issues Releases Wiki Activity

Expose fixed constraint in RobotSimAPI.

Browse Source
This commit is contained in:
yunfeibai 2016-08-22 18:14:29 -07:00
parent 8eccac6fd8
commit d46710e447
7 changed files with 94 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
examples/RoboticsLearning/b3RobotSimAPI.cpp
View File

@ -596,6 +596,16 @@ bool b3RobotSimAPI::getJointInfo(int bodyUniqueId, int jointIndex, b3JointInfo*
return (b3GetJointInfo(m_data->m_physicsClient,bodyUniqueId, jointIndex,jointInfo)!=0);
}
void b3RobotSimAPI::createJoint(int parentBodyIndex, int parentJointIndex, int childBodyIndex, int childJointIndex, const b3JointInfo* jointInfo)
{
b3SharedMemoryStatusHandle statusHandle;
b3Assert(b3CanSubmitCommand(m_data->m_physicsClient));
if (b3CanSubmitCommand(m_data->m_physicsClient))
{
statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClient, b3CreateJoint(m_data->m_physicsClient, parentBodyIndex, parentJointIndex, childBodyIndex, childJointIndex, jointInfo));
}
}
void b3RobotSimAPI::setJointMotorControl(int bodyUniqueId, int jointIndex, const b3JointMotorArgs& args)
{
b3SharedMemoryStatusHandle statusHandle;

2
examples/RoboticsLearning/b3RobotSimAPI.h
View File

@ -88,6 +88,8 @@ public:
bool getJointInfo(int bodyUniqueId, int jointIndex, b3JointInfo* jointInfo);
void createJoint(int parentBodyIndex, int parentJointIndex, int childBodyIndex, int childJointIndex, const b3JointInfo* jointInfo);
void setJointMotorControl(int bodyUniqueId, int jointIndex, const struct b3JointMotorArgs& args);
void stepSimulation();

23
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@ -730,6 +730,29 @@ int b3GetJointInfo(b3PhysicsClientHandle physClient, int bodyIndex, int jointInd
return cl->getJointInfo(bodyIndex, jointIndex, *info);
}
b3SharedMemoryCommandHandle b3CreateJoint(b3PhysicsClientHandle physClient, int parentBodyIndex, int parentJointIndex, int childBodyIndex, int childJointIndex, const b3JointInfo* info)
{
PhysicsClient* cl = (PhysicsClient* ) physClient;
b3Assert(cl);
b3Assert(cl->canSubmitCommand());
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
b3Assert(command);
command->m_type = CMD_CREATE_JOINT;
command->m_createJointArguments.m_parentBodyIndex = parentBodyIndex;
command->m_createJointArguments.m_parentJointIndex = parentJointIndex;
command->m_createJointArguments.m_childBodyIndex = childBodyIndex;
command->m_createJointArguments.m_childJointIndex = childJointIndex;
for (int i = 0; i < 7; ++i) {
command->m_createJointArguments.m_parentFrame[i] = info->m_parentFrame[i];
command->m_createJointArguments.m_childFrame[i] = info->m_childFrame[i];
}
for (int i = 0; i < 3; ++i) {
command->m_createJointArguments.m_jointAxis[i] = info->m_jointAxis[i];
}
return (b3SharedMemoryCommandHandle)command;
}
b3SharedMemoryCommandHandle b3PickBody(b3PhysicsClientHandle physClient, double rayFromWorldX,
double rayFromWorldY, double rayFromWorldZ,
double rayToWorldX, double rayToWorldY, double rayToWorldZ)

2
examples/SharedMemory/PhysicsClientC_API.h
View File

@ -59,6 +59,8 @@ int b3GetNumJoints(b3PhysicsClientHandle physClient, int bodyIndex);
///given a body and joint index, return the joint information. See b3JointInfo in SharedMemoryPublic.h
int b3GetJointInfo(b3PhysicsClientHandle physClient, int bodyIndex, int jointIndex, struct b3JointInfo* info);
b3SharedMemoryCommandHandle b3CreateJoint(b3PhysicsClientHandle physClient, int parentBodyIndex, int parentJointIndex, int childBodyIndex, int childJointIndex, const b3JointInfo* info);
///Request debug lines for debug visualization. The flags in debugMode are the same as used in Bullet
///See btIDebugDraw::DebugDrawModes in Bullet/src/LinearMath/btIDebugDraw.h
b3SharedMemoryCommandHandle b3InitRequestDebugLinesCommand(b3PhysicsClientHandle physClient, int debugMode);

42
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@ -11,6 +11,7 @@
#include "BulletDynamics/Featherstone/btMultiBodyPoint2Point.h"
#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
#include "BulletDynamics/Featherstone/btMultiBodyJointFeedback.h"
#include "BulletDynamics/Featherstone/btMultiBodyFixedConstraint.h"
#include "LinearMath/btHashMap.h"
#include "BulletInverseDynamics/MultiBodyTree.hpp"
@ -748,6 +749,7 @@ bool PhysicsServerCommandProcessor::loadSdf(const char* fileName, char* bufferSe
u2b.activateModel(m);
btMultiBody* mb = 0;
btRigidBody* rb = 0;
//get a body index
int bodyUniqueId = m_data->allocHandle();
@ -775,6 +777,7 @@ bool PhysicsServerCommandProcessor::loadSdf(const char* fileName, char* bufferSe
mb = creation.getBulletMultiBody();
rb = creation.getRigidBody();
if (mb)
{
bodyHandle->m_multiBody = mb;
@ -814,6 +817,7 @@ bool PhysicsServerCommandProcessor::loadSdf(const char* fileName, char* bufferSe
} else
{
b3Warning("No multibody loaded from URDF. Could add btRigidBody+btTypedConstraint solution later.");
bodyHandle->m_rigidBody = rb;
}
}
@ -878,6 +882,7 @@ bool PhysicsServerCommandProcessor::loadUrdf(const char* fileName, const btVecto
}
btMultiBody* mb = creation.getBulletMultiBody();
btRigidBody* rb = creation.getRigidBody();
if (useMultiBody)
{
@ -946,8 +951,11 @@ bool PhysicsServerCommandProcessor::loadUrdf(const char* fileName, const btVecto
} else
{
return true;
if (rb)
{
bodyHandle->m_rigidBody = rb;
return true;
}
}
}
@ -2239,6 +2247,36 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
hasStatus = true;
break;
}
case CMD_CREATE_JOINT:
{
InteralBodyData* parentBody = m_data->getHandle(clientCmd.m_createJointArguments.m_parentBodyIndex);
if (parentBody && parentBody->m_multiBody)
{
InteralBodyData* childBody = m_data->getHandle(clientCmd.m_createJointArguments.m_childBodyIndex);
if (childBody)
{
btVector3 pivotInParent(clientCmd.m_createJointArguments.m_parentFrame[0], clientCmd.m_createJointArguments.m_parentFrame[1], clientCmd.m_createJointArguments.m_parentFrame[2]);
btVector3 pivotInChild(clientCmd.m_createJointArguments.m_childFrame[0], clientCmd.m_createJointArguments.m_childFrame[1], clientCmd.m_createJointArguments.m_childFrame[2]);
btMatrix3x3 frameInParent(btQuaternion(clientCmd.m_createJointArguments.m_parentFrame[3], clientCmd.m_createJointArguments.m_parentFrame[4], clientCmd.m_createJointArguments.m_parentFrame[5], clientCmd.m_createJointArguments.m_parentFrame[6]));
btMatrix3x3 frameInChild(btQuaternion(clientCmd.m_createJointArguments.m_childFrame[3], clientCmd.m_createJointArguments.m_childFrame[4], clientCmd.m_createJointArguments.m_childFrame[5], clientCmd.m_createJointArguments.m_childFrame[6]));
if (childBody->m_multiBody)
{
btMultiBodyFixedConstraint* multibodyFixed = new btMultiBodyFixedConstraint(parentBody->m_multiBody,clientCmd.m_createJointArguments.m_parentJointIndex,childBody->m_multiBody,clientCmd.m_createJointArguments.m_childJointIndex,pivotInParent,pivotInChild,frameInParent,frameInChild);
multibodyFixed->setMaxAppliedImpulse(2.0);
m_data->m_dynamicsWorld->addMultiBodyConstraint(multibodyFixed);
}
else
{
btMultiBodyFixedConstraint* rigidbodyFixed = new btMultiBodyFixedConstraint(parentBody->m_multiBody,clientCmd.m_createJointArguments.m_parentJointIndex,childBody->m_rigidBody,pivotInParent,pivotInChild,frameInParent,frameInChild);
rigidbodyFixed->setMaxAppliedImpulse(2.0);
btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*) m_data->m_dynamicsWorld;
world->addMultiBodyConstraint(rigidbodyFixed);
}
}
}
hasStatus = true;
break;
}
default:
{
b3Error("Unknown command encountered");

12
examples/SharedMemory/SharedMemoryCommands.h
View File

@ -366,6 +366,17 @@ struct CalculateInverseDynamicsResultArgs
double m_jointForces[MAX_DEGREE_OF_FREEDOM];
};
struct CreateJointArgs
{
int m_parentBodyIndex;
int m_parentJointIndex;
int m_childBodyIndex;
int m_childJointIndex;
double m_parentFrame[7];
double m_childFrame[7];
double m_jointAxis[3];
};
struct SharedMemoryCommand
{
int m_type;
@ -393,6 +404,7 @@ struct SharedMemoryCommand
struct PickBodyArgs m_pickBodyArguments;
struct ExternalForceArgs m_externalForceArguments;
struct CalculateInverseDynamicsArgs m_calculateInverseDynamicsArguments;
struct CreateJointArgs m_createJointArguments;
};
};

6
examples/SharedMemory/SharedMemoryPublic.h
View File

@ -28,7 +28,8 @@ enum EnumSharedMemoryClientCommand
CMD_REQUEST_CAMERA_IMAGE_DATA,
CMD_APPLY_EXTERNAL_FORCE,
CMD_CALCULATE_INVERSE_DYNAMICS,
CMD_MAX_CLIENT_COMMANDS
CMD_MAX_CLIENT_COMMANDS,
CMD_CREATE_JOINT
};
enum EnumSharedMemoryServerStatus
@ -99,6 +100,9 @@ struct b3JointInfo
int m_flags;
double m_jointDamping;
double m_jointFriction;
double m_parentFrame[7]; // position and orientation (quaternion)
double m_childFrame[7]; // ^^^
double m_jointAxis[3]; // joint axis in parent local frame
};
struct b3JointSensorState