Expose fixed constraint in RobotSimAPI.

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;

examples/RoboticsLearning/b3RobotSimAPI.h

@@ -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();

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)

examples/SharedMemory/PhysicsClientC_API.h

@@ -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);

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,9 +951,12 @@ bool PhysicsServerCommandProcessor::loadUrdf(const char* fileName, const btVecto
 
 		} else
 		{
-
+            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");

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;
     };
 };
 

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