diff --git a/examples/MultiBody/MultiDofDemo.cpp b/examples/MultiBody/MultiDofDemo.cpp index a040702bc..33719a2a8 100644 --- a/examples/MultiBody/MultiDofDemo.cpp +++ b/examples/MultiBody/MultiDofDemo.cpp @@ -11,6 +11,7 @@ #include "BulletDynamics/Featherstone/btMultiBodyJointMotor.h" #include "BulletDynamics/Featherstone/btMultiBodyPoint2Point.h" #include "BulletDynamics/Featherstone/btMultiBodyFixedConstraint.h" +#include "BulletDynamics/Featherstone/btMultiBodySliderConstraint.h" #include "../OpenGLWindow/GLInstancingRenderer.h" #include "BulletCollision/CollisionShapes/btShapeHull.h" @@ -227,7 +228,7 @@ void MultiDofDemo::initPhysics() startTransform.setOrigin(btVector3( btScalar(0.0), - -0.95, + 0.0, btScalar(0.0))); @@ -245,7 +246,8 @@ void MultiDofDemo::initPhysics() btMatrix3x3 frameInB; frameInA.setIdentity(); frameInB.setIdentity(); - btMultiBodyFixedConstraint* p2p = new btMultiBodyFixedConstraint(mbC,numLinks-1,body,pointInA,pointInB,frameInA,frameInB); + btVector3 jointAxis(1.0,0.0,0.0); + btMultiBodySliderConstraint* p2p = new btMultiBodySliderConstraint(mbC,numLinks-1,body,pointInA,pointInB,frameInA,frameInB,jointAxis); //btMultiBodyFixedConstraint* p2p = new btMultiBodyFixedConstraint(mbC,numLinks-1,mbC,numLinks-4,pointInA,pointInA,frameInA,frameInB); p2p->setMaxAppliedImpulse(2.0); m_dynamicsWorld->addMultiBodyConstraint(p2p); @@ -307,7 +309,7 @@ btMultiBody* MultiDofDemo::createFeatherstoneMultiBody_testMultiDof(btMultiBodyD pMultiBody->setupRevolute(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f), hingeJointAxis, parentComToCurrentPivot, currentPivotToCurrentCom, false); else //pMultiBody->setupPlanar(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f)/*quat0*/, btVector3(1, 0, 0), parentComToCurrentPivot*2, false); - pMultiBody->setupSpherical(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f), parentComToCurrentPivot, currentPivotToCurrentCom, false); + pMultiBody->setupSpherical(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f), parentComToCurrentPivot, currentPivotToCurrentCom, false); } pMultiBody->finalizeMultiDof(); diff --git a/examples/RoboticsLearning/GripperGraspExample.cpp b/examples/RoboticsLearning/GripperGraspExample.cpp index 7db87b26a..a6086ada9 100644 --- a/examples/RoboticsLearning/GripperGraspExample.cpp +++ b/examples/RoboticsLearning/GripperGraspExample.cpp @@ -87,7 +87,7 @@ public: args.m_fileName = "cube_small.urdf"; args.m_startPosition.setValue(0, 0, .107); args.m_startOrientation.setEulerZYX(0, 0, 0); - args.m_useMultiBody = true; + args.m_useMultiBody = false; m_robotSim.loadFile(args, results); } @@ -152,7 +152,7 @@ public: } m_robotSim.setGravity(b3MakeVector3(0,0,-10)); - + m_robotSim.setNumSimulationSubSteps(4); } diff --git a/examples/RoboticsLearning/b3RobotSimAPI.cpp b/examples/RoboticsLearning/b3RobotSimAPI.cpp index 4c825acab..686eeecdf 100644 --- a/examples/RoboticsLearning/b3RobotSimAPI.cpp +++ b/examples/RoboticsLearning/b3RobotSimAPI.cpp @@ -460,6 +460,15 @@ void b3RobotSimAPI::setGravity(const b3Vector3& gravityAcceleration) } +void b3RobotSimAPI::setNumSimulationSubSteps(int numSubSteps) +{ + b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(m_data->m_physicsClient); + b3SharedMemoryStatusHandle statusHandle; + b3PhysicsParamSetNumSubSteps(command, numSubSteps); + statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClient, command); + b3Assert(b3GetStatusType(statusHandle)==CMD_CLIENT_COMMAND_COMPLETED); +} + b3RobotSimAPI::~b3RobotSimAPI() { delete m_data; @@ -596,6 +605,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, 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..88506e871 100644 --- a/examples/RoboticsLearning/b3RobotSimAPI.h +++ b/examples/RoboticsLearning/b3RobotSimAPI.h @@ -87,12 +87,16 @@ public: int getNumJoints(int bodyUniqueId) const; bool getJointInfo(int bodyUniqueId, int jointIndex, b3JointInfo* jointInfo); + + void createJoint(int parentBodyIndex, int parentJointIndex, int childBodyIndex, int childJointIndex, b3JointInfo* jointInfo); void setJointMotorControl(int bodyUniqueId, int jointIndex, const struct b3JointMotorArgs& args); void stepSimulation(); void setGravity(const b3Vector3& gravityAcceleration); + + void setNumSimulationSubSteps(int numSubSteps); void renderScene(); void debugDraw(int debugDrawMode); diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index 98d6d2e19..7f144eeac 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -154,6 +154,15 @@ int b3PhysicsParamSetTimeStep(b3SharedMemoryCommandHandle commandHandle, double return 0; } +int b3PhysicsParamSetNumSubSteps(b3SharedMemoryCommandHandle commandHandle, int numSubSteps) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; + b3Assert(command->m_type == CMD_SEND_PHYSICS_SIMULATION_PARAMETERS); + command->m_updateFlags |= SIM_PARAM_UPDATE_NUM_SIMULATION_SUB_STEPS; + command->m_physSimParamArgs.m_numSimulationSubSteps = numSubSteps; + return 0; +} + b3SharedMemoryCommandHandle b3InitStepSimulationCommand(b3PhysicsClientHandle physClient) { PhysicsClient* cl = (PhysicsClient* ) physClient; @@ -730,6 +739,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, struct 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..351c87677 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, struct 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 @@ -82,6 +84,7 @@ void b3GetCameraImageData(b3PhysicsClientHandle physClient, struct b3CameraImage b3SharedMemoryCommandHandle b3InitPhysicsParamCommand(b3PhysicsClientHandle physClient); int b3PhysicsParamSetGravity(b3SharedMemoryCommandHandle commandHandle, double gravx,double gravy, double gravz); int b3PhysicsParamSetTimeStep(b3SharedMemoryCommandHandle commandHandle, double timeStep); +int b3PhysicsParamSetNumSubSteps(b3SharedMemoryCommandHandle commandHandle, int numSubSteps); int b3PhysicsParamSetRealTimeSimulation(b3SharedMemoryCommandHandle commandHandle, int enableRealTimeSimulation); b3SharedMemoryCommandHandle b3InitStepSimulationCommand(b3PhysicsClientHandle physClient); diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 96dc083a7..9d7dcf5e5 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" @@ -384,6 +385,7 @@ struct PhysicsServerCommandProcessorInternalData btScalar m_physicsDeltaTime; + btScalar m_numSimulationSubSteps; btAlignedObjectArray m_multiBodyJointFeedbacks; btHashMap m_inverseDynamicsBodies; @@ -427,6 +429,7 @@ struct PhysicsServerCommandProcessorInternalData m_commandLogger(0), m_logPlayback(0), m_physicsDeltaTime(1./240.), + m_numSimulationSubSteps(0), m_dynamicsWorld(0), m_remoteDebugDrawer(0), m_guiHelper(0), @@ -748,6 +751,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 +779,7 @@ bool PhysicsServerCommandProcessor::loadSdf(const char* fileName, char* bufferSe mb = creation.getBulletMultiBody(); + rb = creation.getRigidBody(); if (mb) { bodyHandle->m_multiBody = mb; @@ -814,6 +819,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 +884,7 @@ bool PhysicsServerCommandProcessor::loadUrdf(const char* fileName, const btVecto } btMultiBody* mb = creation.getBulletMultiBody(); + btRigidBody* rb = creation.getRigidBody(); if (useMultiBody) { @@ -946,8 +953,11 @@ bool PhysicsServerCommandProcessor::loadUrdf(const char* fileName, const btVecto } else { - - return true; + if (rb) + { + bodyHandle->m_rigidBody = rb; + return true; + } } } @@ -1804,7 +1814,10 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm { applyJointDamping(i); } - m_data->m_dynamicsWorld->stepSimulation(m_data->m_physicsDeltaTime,0); + if (m_data->m_numSimulationSubSteps > 0) + m_data->m_dynamicsWorld->stepSimulation(m_data->m_physicsDeltaTime,m_data->m_numSimulationSubSteps,m_data->m_physicsDeltaTime/m_data->m_numSimulationSubSteps); + else + m_data->m_dynamicsWorld->stepSimulation(m_data->m_physicsDeltaTime,0); SharedMemoryStatus& serverCmd =serverStatusOut; serverCmd.m_type = CMD_STEP_FORWARD_SIMULATION_COMPLETED; @@ -1834,6 +1847,10 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm } } + if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_NUM_SIMULATION_SUB_STEPS) + { + m_data->m_numSimulationSubSteps = clientCmd.m_physSimParamArgs.m_numSimulationSubSteps; + } SharedMemoryStatus& serverCmd =serverStatusOut; serverCmd.m_type = CMD_CLIENT_COMMAND_COMPLETED; @@ -2239,6 +2256,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 diff --git a/src/BulletDynamics/CMakeLists.txt b/src/BulletDynamics/CMakeLists.txt index e511e6372..656fc8433 100644 --- a/src/BulletDynamics/CMakeLists.txt +++ b/src/BulletDynamics/CMakeLists.txt @@ -33,6 +33,7 @@ SET(BulletDynamics_SRCS Featherstone/btMultiBodyConstraint.cpp Featherstone/btMultiBodyPoint2Point.cpp Featherstone/btMultiBodyFixedConstraint.cpp + Featherstone/btMultiBodySliderConstraint.cpp Featherstone/btMultiBodyJointMotor.cpp MLCPSolvers/btDantzigLCP.cpp MLCPSolvers/btMLCPSolver.cpp @@ -91,6 +92,7 @@ SET(Featherstone_HDRS Featherstone/btMultiBodyConstraint.h Featherstone/btMultiBodyPoint2Point.h Featherstone/btMultiBodyFixedConstraint.h + Featherstone/btMultiBodySliderConstraint.h Featherstone/btMultiBodyJointMotor.h ) diff --git a/src/BulletDynamics/Featherstone/btMultiBodySliderConstraint.cpp b/src/BulletDynamics/Featherstone/btMultiBodySliderConstraint.cpp new file mode 100644 index 000000000..b307385bd --- /dev/null +++ b/src/BulletDynamics/Featherstone/btMultiBodySliderConstraint.cpp @@ -0,0 +1,230 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2013 Erwin Coumans 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. +*/ + +///This file was written by Erwin Coumans + +#include "btMultiBodySliderConstraint.h" +#include "btMultiBodyLinkCollider.h" +#include "BulletDynamics/Dynamics/btRigidBody.h" +#include "BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h" +#include "LinearMath/btIDebugDraw.h" + +#define BTMBSLIDERCONSTRAINT_DIM 6 +#define EPSILON 0.000001 + +btMultiBodySliderConstraint::btMultiBodySliderConstraint(btMultiBody* body, int link, btRigidBody* bodyB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB, const btVector3& jointAxis) + :btMultiBodyConstraint(body,0,link,-1,BTMBSLIDERCONSTRAINT_DIM,false), + m_rigidBodyA(0), + m_rigidBodyB(bodyB), + m_pivotInA(pivotInA), + m_pivotInB(pivotInB), + m_frameInA(frameInA), + m_frameInB(frameInB), + m_jointAxis(jointAxis) +{ + m_data.resize(BTMBSLIDERCONSTRAINT_DIM);//at least store the applied impulses +} + +btMultiBodySliderConstraint::btMultiBodySliderConstraint(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB, const btVector3& jointAxis) + :btMultiBodyConstraint(bodyA,bodyB,linkA,linkB,BTMBSLIDERCONSTRAINT_DIM,false), + m_rigidBodyA(0), + m_rigidBodyB(0), + m_pivotInA(pivotInA), + m_pivotInB(pivotInB), + m_frameInA(frameInA), + m_frameInB(frameInB), + m_jointAxis(jointAxis) +{ + m_data.resize(BTMBSLIDERCONSTRAINT_DIM);//at least store the applied impulses +} + +void btMultiBodySliderConstraint::finalizeMultiDof() +{ + //not implemented yet + btAssert(0); +} + +btMultiBodySliderConstraint::~btMultiBodySliderConstraint() +{ +} + + +int btMultiBodySliderConstraint::getIslandIdA() const +{ + if (m_rigidBodyA) + return m_rigidBodyA->getIslandTag(); + + if (m_bodyA) + { + btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider(); + if (col) + return col->getIslandTag(); + for (int i=0;igetNumLinks();i++) + { + if (m_bodyA->getLink(i).m_collider) + return m_bodyA->getLink(i).m_collider->getIslandTag(); + } + } + return -1; +} + +int btMultiBodySliderConstraint::getIslandIdB() const +{ + if (m_rigidBodyB) + return m_rigidBodyB->getIslandTag(); + if (m_bodyB) + { + btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider(); + if (col) + return col->getIslandTag(); + + for (int i=0;igetNumLinks();i++) + { + col = m_bodyB->getLink(i).m_collider; + if (col) + return col->getIslandTag(); + } + } + return -1; +} + +void btMultiBodySliderConstraint::createConstraintRows(btMultiBodyConstraintArray& constraintRows, btMultiBodyJacobianData& data, const btContactSolverInfo& infoGlobal) +{ + // Convert local points back to world + btVector3 pivotAworld = m_pivotInA; + btMatrix3x3 frameAworld = m_frameInA; + btVector3 jointAxis = m_jointAxis; + if (m_rigidBodyA) + { + pivotAworld = m_rigidBodyA->getCenterOfMassTransform()*m_pivotInA; + frameAworld = m_frameInA.transpose()*btMatrix3x3(m_rigidBodyA->getOrientation()); + jointAxis = quatRotate(m_rigidBodyA->getOrientation(),m_jointAxis); + + } else if (m_bodyA) { + pivotAworld = m_bodyA->localPosToWorld(m_linkA, m_pivotInA); + frameAworld = m_bodyA->localFrameToWorld(m_linkA, m_frameInA); + jointAxis = m_bodyA->localDirToWorld(m_linkA, m_jointAxis); + } + btVector3 pivotBworld = m_pivotInB; + btMatrix3x3 frameBworld = m_frameInB; + if (m_rigidBodyB) + { + pivotBworld = m_rigidBodyB->getCenterOfMassTransform()*m_pivotInB; + frameBworld = m_frameInB.transpose()*btMatrix3x3(m_rigidBodyB->getOrientation()); + + } else if (m_bodyB) { + pivotBworld = m_bodyB->localPosToWorld(m_linkB, m_pivotInB); + frameBworld = m_bodyB->localFrameToWorld(m_linkB, m_frameInB); + } + + btVector3 constraintAxis[2]; + for (int i = 0; i < 3; ++i) + { + constraintAxis[0] = frameAworld.getColumn(i).cross(jointAxis); + if (constraintAxis[0].norm() > EPSILON) + { + constraintAxis[0] = constraintAxis[0].normalized(); + constraintAxis[1] = jointAxis.cross(constraintAxis[0]); + constraintAxis[1] = constraintAxis[1].normalized(); + break; + } + } + + btMatrix3x3 relRot = frameAworld.inverse()*frameBworld; + btVector3 angleDiff; + btGeneric6DofSpring2Constraint::matrixToEulerXYZ(relRot,angleDiff); + + int numDim = BTMBSLIDERCONSTRAINT_DIM; + for (int i=0;igetCompanionId(); + } + if (m_rigidBodyB) + { + constraintRow.m_solverBodyIdB = m_rigidBodyB->getCompanionId(); + } + + btVector3 constraintNormalLin(0,0,0); + btVector3 constraintNormalAng(0,0,0); + btScalar posError = 0.0; + if (i < 2) { + constraintNormalLin = constraintAxis[i]; + posError = (pivotAworld-pivotBworld).dot(constraintNormalLin); + fillMultiBodyConstraint(constraintRow, data, 0, 0, constraintNormalAng, + constraintNormalLin, pivotAworld, pivotBworld, + posError, + infoGlobal, + -m_maxAppliedImpulse, m_maxAppliedImpulse + ); + } + else { //i>=2 + constraintNormalAng = frameAworld.getColumn(i%3); + posError = angleDiff[i%3]; + fillMultiBodyConstraint(constraintRow, data, 0, 0, constraintNormalAng, + constraintNormalLin, pivotAworld, pivotBworld, + posError, + infoGlobal, + -m_maxAppliedImpulse, m_maxAppliedImpulse, true + ); + } + } +} + +void btMultiBodySliderConstraint::debugDraw(class btIDebugDraw* drawer) +{ + btTransform tr; + tr.setIdentity(); + + if (m_rigidBodyA) + { + btVector3 pivot = m_rigidBodyA->getCenterOfMassTransform() * m_pivotInA; + tr.setOrigin(pivot); + drawer->drawTransform(tr, 0.1); + } + if (m_bodyA) + { + btVector3 pivotAworld = m_bodyA->localPosToWorld(m_linkA, m_pivotInA); + tr.setOrigin(pivotAworld); + drawer->drawTransform(tr, 0.1); + } + if (m_rigidBodyB) + { + // that ideally should draw the same frame + btVector3 pivot = m_rigidBodyB->getCenterOfMassTransform() * m_pivotInB; + tr.setOrigin(pivot); + drawer->drawTransform(tr, 0.1); + } + if (m_bodyB) + { + btVector3 pivotBworld = m_bodyB->localPosToWorld(m_linkB, m_pivotInB); + tr.setOrigin(pivotBworld); + drawer->drawTransform(tr, 0.1); + } +} diff --git a/src/BulletDynamics/Featherstone/btMultiBodySliderConstraint.h b/src/BulletDynamics/Featherstone/btMultiBodySliderConstraint.h new file mode 100644 index 000000000..571dcd53b --- /dev/null +++ b/src/BulletDynamics/Featherstone/btMultiBodySliderConstraint.h @@ -0,0 +1,105 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2013 Erwin Coumans 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. +*/ + +///This file was written by Erwin Coumans + +#ifndef BT_MULTIBODY_SLIDER_CONSTRAINT_H +#define BT_MULTIBODY_SLIDER_CONSTRAINT_H + +#include "btMultiBodyConstraint.h" + +class btMultiBodySliderConstraint : public btMultiBodyConstraint +{ +protected: + + btRigidBody* m_rigidBodyA; + btRigidBody* m_rigidBodyB; + btVector3 m_pivotInA; + btVector3 m_pivotInB; + btMatrix3x3 m_frameInA; + btMatrix3x3 m_frameInB; + btVector3 m_jointAxis; + +public: + + btMultiBodySliderConstraint(btMultiBody* body, int link, btRigidBody* bodyB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB, const btVector3& jointAxis); + btMultiBodySliderConstraint(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB, const btVector3& jointAxis); + + virtual ~btMultiBodySliderConstraint(); + + virtual void finalizeMultiDof(); + + virtual int getIslandIdA() const; + virtual int getIslandIdB() const; + + virtual void createConstraintRows(btMultiBodyConstraintArray& constraintRows, + btMultiBodyJacobianData& data, + const btContactSolverInfo& infoGlobal); + + const btVector3& getPivotInA() const + { + return m_pivotInA; + } + + void setPivotInA(const btVector3& pivotInA) + { + m_pivotInA = pivotInA; + } + + const btVector3& getPivotInB() const + { + return m_pivotInB; + } + + void setPivotInB(const btVector3& pivotInB) + { + m_pivotInB = pivotInB; + } + + const btMatrix3x3& getFrameInA() const + { + return m_frameInA; + } + + void setFrameInA(const btMatrix3x3& frameInA) + { + m_frameInA = frameInA; + } + + const btMatrix3x3& getFrameInB() const + { + return m_frameInB; + } + + void setFrameInB(const btMatrix3x3& frameInB) + { + m_frameInB = frameInB; + } + + const btVector3& getJointAxis() const + { + return m_jointAxis; + } + + void setJointAxis(const btVector3& jointAxis) + { + m_jointAxis = jointAxis; + } + + virtual void debugDraw(class btIDebugDraw* drawer); + +}; + +#endif //BT_MULTIBODY_SLIDER_CONSTRAINT_H