diff --git a/examples/RoboticsLearning/b3RobotSimAPI.cpp b/examples/RoboticsLearning/b3RobotSimAPI.cpp index 4c825acab..84e939716 100644 --- a/examples/RoboticsLearning/b3RobotSimAPI.cpp +++ b/examples/RoboticsLearning/b3RobotSimAPI.cpp @@ -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; diff --git a/examples/RoboticsLearning/b3RobotSimAPI.h b/examples/RoboticsLearning/b3RobotSimAPI.h index d435b2925..e42c3d6ea 100644 --- a/examples/RoboticsLearning/b3RobotSimAPI.h +++ b/examples/RoboticsLearning/b3RobotSimAPI.h @@ -87,6 +87,8 @@ public: int getNumJoints(int bodyUniqueId) const; 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); diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index 98d6d2e19..2e1e841b1 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -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) diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index 545b70b9e..21a9d9b77 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -58,6 +58,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 diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 96dc083a7..6b98d3b0b 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -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"); diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index 5e2bd41c5..130f786cf 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -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; }; }; diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index fb5d41364..87efcb7dd 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -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