From 7dd524075c850a005354ead244177c51922d0b6d Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Tue, 13 Nov 2018 14:32:18 -0800 Subject: [PATCH] Implement btMultiBodySphericalJointMotor, able to track a quaternion position target. Expose this btMultiBodySphericalJointMotor through PyBullet.setJointMotorControlMultiDof Expose PyBullet.getQuaternionSlerp Improve PyBullet.setJointMotorControlMultiDof Improve humanoidMotionCapture.py with slerp and using setJointMotorControlMultiDof Expose btMultiBody::spatialTransform Fix btMultiBody::setupPlanar from DeepMimic codebase Add support for multidof joints in btMultiBody::compTreeLinkVelocities, thanks to DeepMimic codebase @xbpeng --- examples/SharedMemory/PhysicsClientC_API.cpp | 43 ++++ examples/SharedMemory/PhysicsClientC_API.h | 4 + .../PhysicsClientSharedMemory.cpp | 2 + .../PhysicsServerCommandProcessor.cpp | 97 ++++++- examples/SharedMemory/SharedMemoryCommands.h | 2 +- .../examples/humanoidMotionCapture.py | 144 ++++++++--- examples/pybullet/pybullet.c | 240 +++++++++++++++++- setup.py | 1 + src/BulletDynamics/CMakeLists.txt | 1 + .../Featherstone/btMultiBody.cpp | 54 ++-- src/BulletDynamics/Featherstone/btMultiBody.h | 9 + .../btMultiBodySphericalJointMotor.cpp | 172 +++++++++++++ .../btMultiBodySphericalJointMotor.h | 77 ++++++ 13 files changed, 778 insertions(+), 68 deletions(-) create mode 100644 src/BulletDynamics/Featherstone/btMultiBodySphericalJointMotor.cpp create mode 100644 src/BulletDynamics/Featherstone/btMultiBodySphericalJointMotor.h diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index 834351f62..3517027fc 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -818,6 +818,22 @@ B3_SHARED_API int b3JointControlSetDesiredPosition(b3SharedMemoryCommandHandle c return 0; } +B3_SHARED_API int b3JointControlSetDesiredPositionMultiDof(b3SharedMemoryCommandHandle commandHandle, int qIndex, const double* position, int dofCount) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle; + b3Assert(command); + if ((qIndex >= 0) && ((qIndex + dofCount) < MAX_DEGREE_OF_FREEDOM) && dofCount > 0 && dofCount <= 4) + { + for (int dof = 0; dof < dofCount; dof++) + { + command->m_sendDesiredStateCommandArgument.m_desiredStateQ[qIndex + dof] = position[dof]; + command->m_updateFlags |= SIM_DESIRED_STATE_HAS_Q; + command->m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[qIndex + dof] |= SIM_DESIRED_STATE_HAS_Q; + } + } + return 0; +} + B3_SHARED_API int b3JointControlSetKp(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle; @@ -870,6 +886,22 @@ B3_SHARED_API int b3JointControlSetDesiredVelocity(b3SharedMemoryCommandHandle c return 0; } +B3_SHARED_API int b3JointControlSetDesiredVelocityMultiDof(b3SharedMemoryCommandHandle commandHandle, int dofIndex, const double* velocity, int dofCount) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle; + b3Assert(command); + if ((dofIndex >= 0) && ((dofIndex+ dofCount) < MAX_DEGREE_OF_FREEDOM) && dofCount>=0 && dofCount<=4) + { + for (int dof = 0; dof < dofCount; dof++) + { + command->m_sendDesiredStateCommandArgument.m_desiredStateQdot[dofIndex+dof] = velocity[dof]; + command->m_updateFlags |= SIM_DESIRED_STATE_HAS_QDOT; + command->m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex+dof] |= SIM_DESIRED_STATE_HAS_QDOT; + } + } + return 0; +} + B3_SHARED_API int b3JointControlSetMaximumForce(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle; @@ -5221,3 +5253,14 @@ B3_SHARED_API void b3InvertTransform(const double pos[3], const double orn[4], d outOrn[2] = invOrn[2]; outOrn[3] = invOrn[3]; } + +B3_SHARED_API void b3QuaternionSlerp(const double startQuat[/*4*/], const double endQuat[/*4*/], double interpolationFraction, double outOrn[/*4*/]) +{ + b3Quaternion start(startQuat[0], startQuat[1], startQuat[2], startQuat[3]); + b3Quaternion end(endQuat[0], endQuat[1], endQuat[2], endQuat[3]); + b3Quaternion result = start.slerp(end, interpolationFraction); + outOrn[0] = result[0]; + outOrn[1] = result[1]; + outOrn[2] = result[2]; + outOrn[3] = result[3]; +} diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index afceb7784..5c68c9748 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -440,12 +440,15 @@ extern "C" ///Only use when controlMode is CONTROL_MODE_POSITION_VELOCITY_PD B3_SHARED_API int b3JointControlSetDesiredPosition(b3SharedMemoryCommandHandle commandHandle, int qIndex, double value); + B3_SHARED_API int b3JointControlSetDesiredPositionMultiDof(b3SharedMemoryCommandHandle commandHandle, int qIndex, const double* position, int dofCount); + B3_SHARED_API int b3JointControlSetKp(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value); B3_SHARED_API int b3JointControlSetKd(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value); B3_SHARED_API int b3JointControlSetMaximumVelocity(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double maximumVelocity); ///Only use when controlMode is CONTROL_MODE_VELOCITY B3_SHARED_API int b3JointControlSetDesiredVelocity(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value); /* find a better name for dof/q/u indices, point to b3JointInfo */ + B3_SHARED_API int b3JointControlSetDesiredVelocityMultiDof(b3SharedMemoryCommandHandle commandHandle, int dofIndex, const double* velocity, int dofCount); B3_SHARED_API int b3JointControlSetMaximumForce(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value); ///Only use if when controlMode is CONTROL_MODE_TORQUE, B3_SHARED_API int b3JointControlSetDesiredForceTorque(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value); @@ -631,6 +634,7 @@ extern "C" B3_SHARED_API void b3MultiplyTransforms(const double posA[/*3*/], const double ornA[/*4*/], const double posB[/*3*/], const double ornB[/*4*/], double outPos[/*3*/], double outOrn[/*4*/]); B3_SHARED_API void b3InvertTransform(const double pos[/*3*/], const double orn[/*4*/], double outPos[/*3*/], double outOrn[/*4*/]); + B3_SHARED_API void b3QuaternionSlerp(const double startQuat[/*4*/], const double endQuat[/*4*/], double interpolationFraction, double outOrn[/*4*/]); #ifdef __cplusplus } diff --git a/examples/SharedMemory/PhysicsClientSharedMemory.cpp b/examples/SharedMemory/PhysicsClientSharedMemory.cpp index c9193ecef..7ba80e823 100644 --- a/examples/SharedMemory/PhysicsClientSharedMemory.cpp +++ b/examples/SharedMemory/PhysicsClientSharedMemory.cpp @@ -149,6 +149,8 @@ bool PhysicsClientSharedMemory::getJointInfo(int bodyUniqueId, int jointIndex, b if ((jointIndex >= 0) && (jointIndex < bodyJoints->m_jointInfo.size())) { info = bodyJoints->m_jointInfo[jointIndex]; + info.m_qSize = 0; + info.m_uSize = 0; switch (info.m_jointType) { case eSphericalType: diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 90ef1f1de..bba891478 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -13,6 +13,8 @@ #include "BulletDynamics/MLCPSolvers/btDantzigSolver.h" #include "BulletDynamics/MLCPSolvers/btSolveProjectedGaussSeidel.h" #include "BulletDynamics/Featherstone/btMultiBodyMLCPConstraintSolver.h" +#include "BulletDynamics/Featherstone/btMultiBodySphericalJointMotor.h" + #include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h" #include "BulletDynamics/Featherstone/btMultiBodyPoint2Point.h" @@ -2711,7 +2713,8 @@ void PhysicsServerCommandProcessor::deleteDynamicsWorld() bool PhysicsServerCommandProcessor::supportsJointMotor(btMultiBody* mb, int mbLinkIndex) { - bool canHaveMotor = (mb->getLink(mbLinkIndex).m_jointType == btMultibodyLink::eRevolute || mb->getLink(mbLinkIndex).m_jointType == btMultibodyLink::ePrismatic); + bool canHaveMotor = (mb->getLink(mbLinkIndex).m_jointType == btMultibodyLink::eRevolute + || mb->getLink(mbLinkIndex).m_jointType == btMultibodyLink::ePrismatic); return canHaveMotor; } @@ -2722,10 +2725,10 @@ void PhysicsServerCommandProcessor::createJointMotors(btMultiBody* mb) for (int i = 0; i < numLinks; i++) { int mbLinkIndex = i; + float maxMotorImpulse = 1.f; if (supportsJointMotor(mb, mbLinkIndex)) { - float maxMotorImpulse = 1.f; int dof = 0; btScalar desiredVelocity = 0.f; btMultiBodyJointMotor* motor = new btMultiBodyJointMotor(mb, mbLinkIndex, dof, desiredVelocity, maxMotorImpulse); @@ -2737,6 +2740,13 @@ void PhysicsServerCommandProcessor::createJointMotors(btMultiBody* mb) m_data->m_dynamicsWorld->addMultiBodyConstraint(motor); motor->finalizeMultiDof(); } + if (mb->getLink(mbLinkIndex).m_jointType == btMultibodyLink::eSpherical) + { + btMultiBodySphericalJointMotor* motor = new btMultiBodySphericalJointMotor(mb, mbLinkIndex, 1000*maxMotorImpulse); + mb->getLink(mbLinkIndex).m_userPtr = motor; + m_data->m_dynamicsWorld->addMultiBodyConstraint(motor); + motor->finalizeMultiDof(); + } } } @@ -2916,6 +2926,18 @@ bool PhysicsServerCommandProcessor::processImportedObjects(const char* fileName, std::string* baseName = new std::string(u2b.getLinkName(u2b.getRootLinkIndex())); m_data->m_strings.push_back(baseName); mb->setBaseName(baseName->c_str()); + +#if 0 + btAlignedObjectArray urdf; + mb->dumpUrdf(urdf); + FILE* f = fopen("e:/pybullet.urdf", "w"); + if (f) + { + fwrite(&urdf[0], urdf.size(), 1, f); + fclose(f); + } +#endif + } else { @@ -5811,6 +5833,74 @@ bool PhysicsServerCommandProcessor::processSendDesiredStateCommand(const struct numMotors++; } } + + if (mb->getLink(link).m_jointType == btMultibodyLink::eSpherical) + { + btMultiBodySphericalJointMotor* motor = (btMultiBodySphericalJointMotor*)mb->getLink(link).m_userPtr; + if (motor) + { + if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[velIndex] & SIM_DESIRED_STATE_HAS_RHS_CLAMP) != 0) + { + motor->setRhsClamp(clientCmd.m_sendDesiredStateCommandArgument.m_rhsClamp[velIndex]); + } + bool hasDesiredPosOrVel = false; + btScalar kp = 0.f; + btScalar kd = 0.f; + btVector3 desiredVelocity(0, 0, 0); + if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[velIndex] & SIM_DESIRED_STATE_HAS_QDOT) != 0) + { + hasDesiredPosOrVel = true; + desiredVelocity.setValue( + clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQdot[velIndex+0], + clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQdot[velIndex+1], + clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQdot[velIndex+2]); + kd = 0.1; + } + btQuaternion desiredPosition(0, 0, 0, 1); + if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[posIndex] & SIM_DESIRED_STATE_HAS_Q) != 0) + { + hasDesiredPosOrVel = true; + desiredPosition.setValue( + clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQ[posIndex + 0], + clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQ[posIndex + 1], + clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQ[posIndex + 2], + clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQ[posIndex + 3]); + kp = 0.1; + } + + if (hasDesiredPosOrVel) + { + if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[velIndex] & SIM_DESIRED_STATE_HAS_KP) != 0) + { + kp = clientCmd.m_sendDesiredStateCommandArgument.m_Kp[velIndex]; + } + + if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[velIndex] & SIM_DESIRED_STATE_HAS_KD) != 0) + { + kd = clientCmd.m_sendDesiredStateCommandArgument.m_Kd[velIndex]; + } + + motor->setVelocityTarget(desiredVelocity, kd); + //todo: instead of clamping, combine the motor and limit + //and combine handling of limit force and motor force. + + //clamp position + //if (mb->getLink(link).m_jointLowerLimit <= mb->getLink(link).m_jointUpperLimit) + //{ + // btClamp(desiredPosition, mb->getLink(link).m_jointLowerLimit, mb->getLink(link).m_jointUpperLimit); + //} + motor->setPositionTarget(desiredPosition, kp); + + btScalar maxImp = 1000000.f * m_data->m_physicsDeltaTime; + + if ((clientCmd.m_updateFlags & SIM_DESIRED_STATE_HAS_MAX_FORCE) != 0) + maxImp = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[velIndex] * m_data->m_physicsDeltaTime; + + motor->setMaxAppliedImpulse(maxImp); + } + numMotors++; + } + } velIndex += mb->getLink(link).m_dofCount; posIndex += mb->getLink(link).m_posVarCount; } @@ -8038,8 +8128,7 @@ bool PhysicsServerCommandProcessor::processInitPoseCommand(const struct SharedMe { mb->setJointVelMultiDof(i, &clientCmd.m_initPoseArgs.m_initialStateQdot[uDofIndex]); } - - + posVarCountIndex += mb->getLink(i).m_posVarCount; uDofIndex += mb->getLink(i).m_dofCount; } diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index cb75c0e46..50db5777b 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -445,7 +445,7 @@ enum EnumSimDesiredStateUpdateFlags SIM_DESIRED_STATE_HAS_KD = 4, SIM_DESIRED_STATE_HAS_KP = 8, SIM_DESIRED_STATE_HAS_MAX_FORCE = 16, - SIM_DESIRED_STATE_HAS_RHS_CLAMP = 32 + SIM_DESIRED_STATE_HAS_RHS_CLAMP = 32, }; enum EnumSimParamUpdateFlags diff --git a/examples/pybullet/examples/humanoidMotionCapture.py b/examples/pybullet/examples/humanoidMotionCapture.py index 232c78339..71025e5ed 100644 --- a/examples/pybullet/examples/humanoidMotionCapture.py +++ b/examples/pybullet/examples/humanoidMotionCapture.py @@ -8,11 +8,15 @@ p.connect(p.GUI) import pybullet_data useMotionCapture=True +useMotionCaptureReset=not useMotionCapture + p.setAdditionalSearchPath(pybullet_data.getDataPath()) -path = pybullet_data.getDataPath()+"/motions/humanoid3d_backflip.txt" +p.setPhysicsEngineParameter(numSolverIterations=200) +#path = pybullet_data.getDataPath()+"/motions/humanoid3d_backflip.txt" +path = pybullet_data.getDataPath()+"/motions/humanoid3d_cartwheel.txt" -p.loadURDF("plane.urdf") +p.loadURDF("plane.urdf",[0,0,-1]) print("path = ", path) with open(path, 'r') as f: motion_dict = json.load(f) @@ -31,6 +35,11 @@ humanoid = p.loadURDF("humanoid/humanoid.urdf", globalScaling=0.25) for j in range (p.getNumJoints(humanoid)): ji = p.getJointInfo(humanoid,j) + targetPosition=[0] + if (ji[2] == p.JOINT_SPHERICAL): + targetPosition=[0,0,0,1] + #p.setJointMotorControlMultiDof(humanoid,j,p.POSITION_CONTROL,targetPosition, force=0) + #print(ji) print("joint[",j,"].type=",jointTypes[ji[2]]) print("joint[",j,"].name=",ji[1]) @@ -51,7 +60,7 @@ for j in range (p.getNumJoints(humanoid)): paramIds.append(p.addUserDebugParameter(jointName.decode("utf-8"),-4,4,0)) print("jointName=",jointName, "at ", j) -p.changeVisualShape(humanoid,14,rgbaColor=[1,0,0,1]) +p.changeVisualShape(humanoid,2,rgbaColor=[1,0,0,1]) chest=1 neck=2 rightShoulder=3 @@ -66,34 +75,115 @@ leftKnee=13 leftAnkle=14 import time -p.setRealTimeSimulation(0) -while (1): - p.getCameraImage(320,200) - frame = int(p.readUserDebugParameter(frameId)) + + +once=True +p.getCameraImage(320,200) +maxForce=1000 + + +while (p.isConnected()): + frameReal = p.readUserDebugParameter(frameId) + frame = int(frameReal) + frameNext = frame+1 + if (frameNext >= numFrames): + frameNext = frame + + frameFraction = frameReal - frame + #print("frameFraction=",frameFraction) + #print("frame=",frame) + #print("frameNext=", frameNext) + + #getQuaternionSlerp + frameData = motion_dict['Frames'][frame] + frameDataNext = motion_dict['Frames'][frameNext] + #print("duration=",frameData[0]) #print(pos=[frameData]) - basePos1 = [frameData[1],frameData[2],frameData[3]] - baseOrn1 = [frameData[5],frameData[6], frameData[7],frameData[4]] + + basePos1Start = [frameData[1],frameData[2],frameData[3]] + basePos1End = [frameDataNext[1],frameDataNext[2],frameDataNext[3]] + basePos1 = [basePos1Start[0]+frameFraction*(basePos1End[0]-basePos1Start[0]), + basePos1Start[1]+frameFraction*(basePos1End[1]-basePos1Start[1]), + basePos1Start[2]+frameFraction*(basePos1End[2]-basePos1Start[2])] + baseOrn1Start = [frameData[5],frameData[6], frameData[7],frameData[4]] + baseOrn1Next = [frameDataNext[5],frameDataNext[6], frameDataNext[7],frameDataNext[4]] + baseOrn1 = p.getQuaternionSlerp(baseOrn1Start,baseOrn1Next,frameFraction) #pre-rotate to make z-up - y2zPos=[0,0,0] + y2zPos=[0,0,0.0] y2zOrn = p.getQuaternionFromEuler([1.57,0,0]) basePos,baseOrn = p.multiplyTransforms(y2zPos, y2zOrn,basePos1,baseOrn1) + p.resetBasePositionAndOrientation(humanoid, basePos,baseOrn) - chestRot = [frameData[9],frameData[10],frameData[11],frameData[8]] - neckRot = [frameData[13],frameData[14],frameData[15],frameData[12]] - rightHipRot = [frameData[17],frameData[18],frameData[19],frameData[16]] - rightKneeRot = [frameData[20]] - rightAnkleRot = [frameData[22],frameData[23],frameData[24],frameData[21]] - rightShoulderRot = [frameData[26],frameData[27],frameData[28],frameData[25]] - rightElbowRot = [frameData[29]] - leftHipRot = [frameData[31],frameData[32],frameData[33],frameData[30]] - leftKneeRot = [frameData[34]] - leftAnkleRot = [frameData[36],frameData[37],frameData[38],frameData[35]] - leftShoulderRot = [frameData[40],frameData[41],frameData[42],frameData[39]] - leftElbowRot = [frameData[43]] + # once=False + chestRotStart = [frameData[9],frameData[10],frameData[11],frameData[8]] + chestRotEnd = [frameDataNext[9],frameDataNext[10],frameDataNext[11],frameDataNext[8]] + chestRot = p.getQuaternionSlerp(chestRotStart,chestRotEnd,frameFraction) + + neckRotStart = [frameData[13],frameData[14],frameData[15],frameData[12]] + neckRotEnd= [frameDataNext[13],frameDataNext[14],frameDataNext[15],frameDataNext[12]] + neckRot = p.getQuaternionSlerp(neckRotStart,neckRotEnd,frameFraction) + + rightHipRotStart = [frameData[17],frameData[18],frameData[19],frameData[16]] + rightHipRotEnd = [frameDataNext[17],frameDataNext[18],frameDataNext[19],frameDataNext[16]] + rightHipRot = p.getQuaternionSlerp(rightHipRotStart,rightHipRotEnd,frameFraction) + + rightKneeRotStart = [frameData[20]] + rightKneeRotEnd = [frameDataNext[20]] + rightKneeRot = [rightKneeRotStart[0]+frameFraction*(rightKneeRotEnd[0]-rightKneeRotStart[0])] + + rightAnkleRotStart = [frameData[22],frameData[23],frameData[24],frameData[21]] + rightAnkleRotEnd = [frameDataNext[22],frameDataNext[23],frameDataNext[24],frameDataNext[21]] + rightAnkleRot = p.getQuaternionSlerp(rightAnkleRotStart,rightAnkleRotEnd,frameFraction) + + rightShoulderRotStart = [frameData[26],frameData[27],frameData[28],frameData[25]] + rightShoulderRotEnd = [frameDataNext[26],frameDataNext[27],frameDataNext[28],frameDataNext[25]] + rightShoulderRot = p.getQuaternionSlerp(rightShoulderRotStart,rightShoulderRotEnd,frameFraction) + + rightElbowRotStart = [frameData[29]] + rightElbowRotEnd = [frameDataNext[29]] + rightElbowRot = [rightElbowRotStart[0]+frameFraction*(rightElbowRotEnd[0]-rightElbowRotStart[0])] + + leftHipRotStart = [frameData[31],frameData[32],frameData[33],frameData[30]] + leftHipRotEnd = [frameDataNext[31],frameDataNext[32],frameDataNext[33],frameDataNext[30]] + leftHipRot = p.getQuaternionSlerp(leftHipRotStart,leftHipRotEnd,frameFraction) + + leftKneeRotStart = [frameData[34]] + leftKneeRotEnd = [frameDataNext[34]] + leftKneeRot = [leftKneeRotStart[0] +frameFraction*(leftKneeRotEnd[0]-leftKneeRotStart[0]) ] + + leftAnkleRotStart = [frameData[36],frameData[37],frameData[38],frameData[35]] + leftAnkleRotEnd = [frameDataNext[36],frameDataNext[37],frameDataNext[38],frameDataNext[35]] + leftAnkleRot = p.getQuaternionSlerp(leftAnkleRotStart,leftAnkleRotEnd,frameFraction) + + leftShoulderRotStart = [frameData[40],frameData[41],frameData[42],frameData[39]] + leftShoulderRotEnd = [frameDataNext[40],frameDataNext[41],frameDataNext[42],frameDataNext[39]] + leftShoulderRot = p.getQuaternionSlerp(leftShoulderRotStart,leftShoulderRotEnd,frameFraction) + leftElbowRotStart = [frameData[43]] + leftElbowRotEnd = [frameDataNext[43]] + leftElbowRot = [leftElbowRotStart[0]+frameFraction*(leftElbowRotEnd[0]-leftElbowRotStart[0])] + #print("chestRot=",chestRot) + p.setGravity(0,0,0) + + + kp=1 if (useMotionCapture): + + p.setJointMotorControlMultiDof(humanoid,chest,p.POSITION_CONTROL, targetPosition=chestRot,positionGain=kp, force=maxForce) + p.setJointMotorControlMultiDof(humanoid,neck,p.POSITION_CONTROL,targetPosition=neckRot,positionGain=kp, force=maxForce) + p.setJointMotorControlMultiDof(humanoid,rightHip,p.POSITION_CONTROL,targetPosition=rightHipRot,positionGain=kp, force=maxForce) + p.setJointMotorControlMultiDof(humanoid,rightKnee,p.POSITION_CONTROL,targetPosition=rightKneeRot,positionGain=kp, force=maxForce) + p.setJointMotorControlMultiDof(humanoid,rightAnkle,p.POSITION_CONTROL,targetPosition=rightAnkleRot,positionGain=kp, force=maxForce) + p.setJointMotorControlMultiDof(humanoid,rightShoulder,p.POSITION_CONTROL,targetPosition=rightShoulderRot,positionGain=kp, force=maxForce) + p.setJointMotorControlMultiDof(humanoid,rightElbow, p.POSITION_CONTROL,targetPosition=rightElbowRot,positionGain=kp, force=maxForce) + p.setJointMotorControlMultiDof(humanoid,leftHip, p.POSITION_CONTROL,targetPosition=leftHipRot,positionGain=kp, force=maxForce) + p.setJointMotorControlMultiDof(humanoid,leftKnee, p.POSITION_CONTROL,targetPosition=leftKneeRot,positionGain=kp, force=maxForce) + p.setJointMotorControlMultiDof(humanoid,leftAnkle, p.POSITION_CONTROL,targetPosition=leftAnkleRot,positionGain=kp, force=maxForce) + p.setJointMotorControlMultiDof(humanoid,leftShoulder, p.POSITION_CONTROL,targetPosition=leftShoulderRot,positionGain=kp, force=maxForce) + p.setJointMotorControlMultiDof(humanoid,leftElbow, p.POSITION_CONTROL,targetPosition=leftElbowRot,positionGain=kp, force=maxForce) + if (useMotionCaptureReset): p.resetJointStateMultiDof(humanoid,chest,chestRot) p.resetJointStateMultiDof(humanoid,neck,neckRot) p.resetJointStateMultiDof(humanoid,rightHip,rightHipRot) @@ -106,11 +196,5 @@ while (1): p.resetJointStateMultiDof(humanoid,leftAnkle, leftAnkleRot) p.resetJointStateMultiDof(humanoid,leftShoulder, leftShoulderRot) p.resetJointStateMultiDof(humanoid,leftElbow, leftElbowRot) - else: - for i in range(len(paramIds)): - c = paramIds[i] - targetPos = p.readUserDebugParameter(c) - - p.setJointMotorControl2(humanoid,jointIds[i], - p.POSITION_CONTROL,targetPos, force=5*240.) - time.sleep(0.1) + p.stepSimulation() + #time.sleep(1./240.) diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index 777a011a9..5ac3335cb 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -208,7 +208,7 @@ static int pybullet_internalSetVectord(PyObject* obVec, double vector[3]) return 0; } -// vector - double[3] which will be set by values from obVec +// vector - double[4] which will be set by values from obVec static int pybullet_internalSetVector4d(PyObject* obVec, double vector[4]) { int i, len; @@ -1879,6 +1879,7 @@ static PyObject* pybullet_resetSimulation(PyObject* self, PyObject* args, PyObje Py_INCREF(Py_None); return Py_None; } + //this method is obsolete, use pybullet_setJointMotorControl2 instead static PyObject* pybullet_setJointMotorControl(PyObject* self, PyObject* args) { @@ -2387,6 +2388,180 @@ static PyObject* pybullet_setJointMotorControlArray(PyObject* self, PyObject* ar // return NULL; } + +static PyObject* pybullet_setJointMotorControlMultiDof(PyObject* self, PyObject* args, PyObject* keywds) +{ + int bodyUniqueId, jointIndex, controlMode; + + double targetPositionArray[4] = { 0, 0, 0, 1 }; + double targetVelocityArray[3] = { 0, 0, 0 }; + int targetPositionSize = 0; + int targetVelocitySize = 0; + PyObject* targetPositionObj = 0; + PyObject* targetVelocityObj = 0; + + double force = 100000.0; + double kp = 0.1; + double kd = 1.0; + double maxVelocity = -1; + b3PhysicsClientHandle sm = 0; + + int physicsClientId = 0; + static char* kwlist[] = { "bodyUniqueId", "jointIndex", "controlMode", "targetPosition", "targetVelocity", "force", "positionGain", "velocityGain", "maxVelocity", "physicsClientId", NULL }; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "iii|OOddddi", kwlist, &bodyUniqueId, &jointIndex, &controlMode, + &targetPositionObj, &targetVelocityObj, &force, &kp, &kd, &maxVelocity, &physicsClientId)) + { + return NULL; + } + sm = getPhysicsClient(physicsClientId); + if (sm == 0) + { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + + if (targetPositionObj) + { + PyObject* targetPositionSeq = 0; + int i = 0; + targetPositionSeq = PySequence_Fast(targetPositionObj, "expected a targetPosition sequence"); + targetPositionSize = PySequence_Size(targetPositionObj); + + if (targetPositionSize < 0) + { + targetPositionSize = 0; + } + if (targetPositionSize >4) + { + targetPositionSize = 4; + } + for (i = 0; i < targetPositionSize; i++) + { + targetPositionArray[i] = pybullet_internalGetFloatFromSequence(targetPositionSeq, i); + } + } + + if (targetVelocityObj) + { + int i = 0; + PyObject* targetVelocitySeq = 0; + targetVelocitySeq = PySequence_Fast(targetVelocityObj, "expected a targetVelocity sequence"); + targetVelocitySize = PySequence_Size(targetVelocityObj); + + if (targetVelocitySize < 0) + { + targetVelocitySize = 0; + } + if (targetVelocitySize >3) + { + targetVelocitySize = 3; + } + for (i = 0; i < targetVelocitySize; i++) + { + targetVelocityArray[i] = pybullet_internalGetFloatFromSequence(targetVelocitySeq, i); + } + + } + + //if (targetPositionSize == 0 && targetVelocitySize == 0) + //{ + + { + int numJoints; + b3SharedMemoryCommandHandle commandHandle; + b3SharedMemoryStatusHandle statusHandle; + struct b3JointInfo info; + + numJoints = b3GetNumJoints(sm, bodyUniqueId); + if ((jointIndex >= numJoints) || (jointIndex < 0)) + { + PyErr_SetString(SpamError, "Joint index out-of-range."); + return NULL; + } + + if (//(controlMode != CONTROL_MODE_VELOCITY)&& + //(controlMode != CONTROL_MODE_TORQUE) && + (controlMode != CONTROL_MODE_POSITION_VELOCITY_PD)//&& + //(controlMode != CONTROL_MODE_PD) + ) + { + PyErr_SetString(SpamError, "Illegal control mode."); + return NULL; + } + + commandHandle = b3JointControlCommandInit2(sm, bodyUniqueId, controlMode); + + b3GetJointInfo(sm, bodyUniqueId, jointIndex, &info); + + switch (controlMode) + { +#if 0 + case CONTROL_MODE_VELOCITY: + { + b3JointControlSetDesiredVelocity(commandHandle, info.m_uIndex, + targetVelocity); + b3JointControlSetKd(commandHandle, info.m_uIndex, kd); + b3JointControlSetMaximumForce(commandHandle, info.m_uIndex, force); + break; + } + + case CONTROL_MODE_TORQUE: + { + b3JointControlSetDesiredForceTorque(commandHandle, info.m_uIndex, + force); + break; + } +#endif + case CONTROL_MODE_POSITION_VELOCITY_PD: + case CONTROL_MODE_PD: + { + + //make sure size == info.m_qSize + + if (maxVelocity > 0) + { + b3JointControlSetMaximumVelocity(commandHandle, info.m_uIndex, maxVelocity); + } + + if (info.m_qSize == targetPositionSize) + { + b3JointControlSetDesiredPositionMultiDof(commandHandle, info.m_qIndex, + targetPositionArray, targetPositionSize); + } + else + { + //printf("Warning: targetPosition array size doesn't match joint position size (got %d, expected %d).",targetPositionSize, info.m_qSize); + } + + b3JointControlSetKp(commandHandle, info.m_uIndex, kp); + if (info.m_uSize == targetVelocitySize) + { + b3JointControlSetDesiredVelocityMultiDof(commandHandle, info.m_uIndex, + targetVelocityArray, targetVelocitySize); + } + else + { + //printf("Warning: targetVelocity array size doesn't match joint dimentions (got %d, expected %d).", targetVelocitySize, info.m_uSize); + } + b3JointControlSetKd(commandHandle, info.m_uIndex, kd); + b3JointControlSetMaximumForce(commandHandle, info.m_uIndex, force); + break; + } + default: + { + } + }; + + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); + + Py_INCREF(Py_None); + return Py_None; + } + // PyErr_SetString(SpamError, "Error parsing arguments in setJointControl."); + // return NULL; +} + + static PyObject* pybullet_setJointMotorControl2(PyObject* self, PyObject* args, PyObject* keywds) { int bodyUniqueId, jointIndex, controlMode; @@ -3454,14 +3629,13 @@ static PyObject* pybullet_resetJointState(PyObject* self, PyObject* args, PyObje static PyObject* pybullet_resetJointStateMultiDof(PyObject* self, PyObject* args, PyObject* keywds) { { + b3PhysicsClientHandle sm = 0; int bodyUniqueId; int jointIndex; - double targetPositionArray[4] = { 0, 0, 0, 0 }; + double targetPositionArray[4] = { 0, 0, 0, 1 }; double targetVelocityArray[3] = { 0, 0, 0 }; int targetPositionSize = 0; int targetVelocitySize = 0; - - b3PhysicsClientHandle sm = 0; PyObject* targetPositionObj = 0; PyObject* targetVelocityObj = 0; @@ -8634,6 +8808,56 @@ static PyObject* pybullet_invertTransform(PyObject* self, return NULL; } + +static PyObject* pybullet_getQuaternionSlerp(PyObject* self, PyObject* args, PyObject* keywds) +{ + PyObject* quatStartObj; + PyObject* quatEndObj; + double quatStart[4]; + double quatEnd[4]; + double interpolationFraction; + int physicsClientId = 0; + int hasQuatStart = 0; + int hasQuatEnd = 0; + + static char* kwlist[] = { "quaternionStart", "quaternionEnd", "interpolationFraction", "physicsClientId", NULL }; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "OOd|i", kwlist, &quatStartObj, &quatEndObj, &interpolationFraction, &physicsClientId)) + { + return NULL; + } + + if (quatStartObj) + { + hasQuatStart = pybullet_internalSetVector4d(quatStartObj, quatStart); + } + + if (quatEndObj) + { + hasQuatEnd = pybullet_internalSetVector4d(quatEndObj, quatEnd); + } + if (hasQuatStart && hasQuatEnd) + { + double quatOut[4]; + b3QuaternionSlerp(quatStart, quatEnd, interpolationFraction, quatOut); + { + PyObject* pylist; + int i; + pylist = PyTuple_New(4); + for (i = 0; i < 4; i++) + PyTuple_SetItem(pylist, i, PyFloat_FromDouble(quatOut[i])); + return pylist; + } + } + else + { + PyErr_SetString(SpamError, "Require start and end quaternion, each with 4 components [x,y,z,w]."); + return NULL; + } + Py_INCREF(Py_None); + return Py_None; +} + + /// quaternion <-> euler yaw/pitch/roll convention from URDF/SDF, see Gazebo /// https://github.com/arpg/Gazebo/blob/master/gazebo/math/Quaternion.cc static PyObject* pybullet_getEulerFromQuaternion(PyObject* self, @@ -9750,6 +9974,11 @@ static PyMethodDef SpamMethods[] = { "Set a single joint motor control mode and desired target value. There is " "no immediate state change, stepSimulation will process the motors."}, + { "setJointMotorControlMultiDof", (PyCFunction)pybullet_setJointMotorControlMultiDof, METH_VARARGS | METH_KEYWORDS, + "Set a single joint motor control mode and desired target value. There is " + "no immediate state change, stepSimulation will process the motors." + "This method sets multi-degree-of-freedom motor such as the spherical joint motor." }, + {"setJointMotorControlArray", (PyCFunction)pybullet_setJointMotorControlArray, METH_VARARGS | METH_KEYWORDS, "Set an array of motors control mode and desired target value. There is " "no immediate state change, stepSimulation will process the motors." @@ -9883,6 +10112,9 @@ static PyMethodDef SpamMethods[] = { {"getMatrixFromQuaternion", (PyCFunction)pybullet_getMatrixFromQuaternion, METH_VARARGS | METH_KEYWORDS, "Compute the 3x3 matrix from a quaternion, as a list of 9 values (row-major)"}, + { "getQuaternionSlerp", (PyCFunction)pybullet_getQuaternionSlerp, METH_VARARGS | METH_KEYWORDS, + "Compute the spherical interpolation given a start and end quaternion and an interpolation value in range [0..1]" }, + {"calculateInverseDynamics", (PyCFunction)pybullet_calculateInverseDynamics, METH_VARARGS | METH_KEYWORDS, "Given an object id, joint positions, joint velocities and joint " "accelerations, compute the joint forces using Inverse Dynamics"}, diff --git a/setup.py b/setup.py index a38907320..9c836e725 100644 --- a/setup.py +++ b/setup.py @@ -317,6 +317,7 @@ sources = ["examples/pybullet/pybullet.c"]\ +["src/BulletDynamics/Featherstone/btMultiBodyMLCPConstraintSolver.cpp"]\ +["src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp"]\ +["src/BulletDynamics/Featherstone/btMultiBodySliderConstraint.cpp"]\ ++["src/BulletDynamics/Featherstone/btMultiBodySphericalJointMotor.cpp"]\ +["src/BulletDynamics/Vehicle/btRaycastVehicle.cpp"]\ +["src/BulletDynamics/Vehicle/btWheelInfo.cpp"]\ +["src/BulletDynamics/Character/btKinematicCharacterController.cpp"]\ diff --git a/src/BulletDynamics/CMakeLists.txt b/src/BulletDynamics/CMakeLists.txt index d416d7d5f..3332440f2 100644 --- a/src/BulletDynamics/CMakeLists.txt +++ b/src/BulletDynamics/CMakeLists.txt @@ -41,6 +41,7 @@ SET(BulletDynamics_SRCS Featherstone/btMultiBodyMLCPConstraintSolver.cpp Featherstone/btMultiBodyPoint2Point.cpp Featherstone/btMultiBodySliderConstraint.cpp + Featherstone/btMultiBodySphericalJointMotor.cpp MLCPSolvers/btDantzigLCP.cpp MLCPSolvers/btMLCPSolver.cpp MLCPSolvers/btLemkeAlgorithm.cpp diff --git a/src/BulletDynamics/Featherstone/btMultiBody.cpp b/src/BulletDynamics/Featherstone/btMultiBody.cpp index 9db732ff4..5b009ac1c 100644 --- a/src/BulletDynamics/Featherstone/btMultiBody.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBody.cpp @@ -37,19 +37,21 @@ const btScalar SLEEP_EPSILON = btScalar(0.05); // this is a squared velocity (m const btScalar SLEEP_TIMEOUT = btScalar(2); // in seconds } // namespace -namespace -{ -void SpatialTransform(const btMatrix3x3 &rotation_matrix, // rotates vectors in 'from' frame to vectors in 'to' frame - const btVector3 &displacement, // vector from origin of 'from' frame to origin of 'to' frame, in 'to' coordinates - const btVector3 &top_in, // top part of input vector - const btVector3 &bottom_in, // bottom part of input vector - btVector3 &top_out, // top part of output vector - btVector3 &bottom_out) // bottom part of output vector +void btMultiBody::spatialTransform(const btMatrix3x3 &rotation_matrix, // rotates vectors in 'from' frame to vectors in 'to' frame + const btVector3 &displacement, // vector from origin of 'from' frame to origin of 'to' frame, in 'to' coordinates + const btVector3 &top_in, // top part of input vector + const btVector3 &bottom_in, // bottom part of input vector + btVector3 &top_out, // top part of output vector + btVector3 &bottom_out) // bottom part of output vector { top_out = rotation_matrix * top_in; bottom_out = -displacement.cross(top_out) + rotation_matrix * bottom_in; } +namespace +{ + + #if 0 void InverseSpatialTransform(const btMatrix3x3 &rotation_matrix, const btVector3 &displacement, @@ -330,6 +332,9 @@ void btMultiBody::setupPlanar(int i, m_links[i].updateCacheMultiDof(); // updateLinksDofOffsets(); + + m_links[i].setAxisBottom(1, m_links[i].getAxisBottom(1).normalized()); + m_links[i].setAxisBottom(2, m_links[i].getAxisBottom(2).normalized()); } void btMultiBody::finalizeMultiDof() @@ -540,35 +545,26 @@ void btMultiBody::compTreeLinkVelocities(btVector3 *omega, btVector3 *vel) const { int num_links = getNumLinks(); // Calculates the velocities of each link (and the base) in its local frame - omega[0] = quatRotate(m_baseQuat, getBaseOmega()); - vel[0] = quatRotate(m_baseQuat, getBaseVel()); + const btQuaternion& base_rot = getWorldToBaseRot(); + omega[0] = quatRotate(base_rot, getBaseOmega()); + vel[0] = quatRotate(base_rot, getBaseVel()); for (int i = 0; i < num_links; ++i) { - const int parent = m_links[i].m_parent; + const btMultibodyLink& link = getLink(i); + const int parent = link.m_parent; // transform parent vel into this frame, store in omega[i+1], vel[i+1] - SpatialTransform(btMatrix3x3(m_links[i].m_cachedRotParentToThis), m_links[i].m_cachedRVector, - omega[parent + 1], vel[parent + 1], - omega[i + 1], vel[i + 1]); + spatialTransform(btMatrix3x3(link.m_cachedRotParentToThis), link.m_cachedRVector, + omega[parent + 1], vel[parent + 1], + omega[i + 1], vel[i + 1]); // now add qidot * shat_i - //only supported for revolute/prismatic joints, todo: spherical and planar joints - switch (m_links[i].m_jointType) + const btScalar* jointVel = getJointVelMultiDof(i); + for (int dof = 0; dof < link.m_dofCount; ++dof) { - case btMultibodyLink::ePrismatic: - case btMultibodyLink::eRevolute: - { - btVector3 axisTop = m_links[i].getAxisTop(0); - btVector3 axisBottom = m_links[i].getAxisBottom(0); - btScalar jointVel = getJointVel(i); - omega[i + 1] += jointVel * axisTop; - vel[i + 1] += jointVel * axisBottom; - break; - } - default: - { - } + omega[i + 1] += jointVel[dof] * link.getAxisTop(dof); + vel[i + 1] += jointVel[dof] * link.getAxisBottom(dof); } } } diff --git a/src/BulletDynamics/Featherstone/btMultiBody.h b/src/BulletDynamics/Featherstone/btMultiBody.h index 54b047c4e..e42055f12 100644 --- a/src/BulletDynamics/Featherstone/btMultiBody.h +++ b/src/BulletDynamics/Featherstone/btMultiBody.h @@ -600,6 +600,15 @@ public: m_userIndex2 = index; } + static void spatialTransform(const btMatrix3x3 &rotation_matrix, // rotates vectors in 'from' frame to vectors in 'to' frame + const btVector3 &displacement, // vector from origin of 'from' frame to origin of 'to' frame, in 'to' coordinates + const btVector3 &top_in, // top part of input vector + const btVector3 &bottom_in, // bottom part of input vector + btVector3 &top_out, // top part of output vector + btVector3 &bottom_out); // bottom part of output vector + + + private: btMultiBody(const btMultiBody &); // not implemented void operator=(const btMultiBody &); // not implemented diff --git a/src/BulletDynamics/Featherstone/btMultiBodySphericalJointMotor.cpp b/src/BulletDynamics/Featherstone/btMultiBodySphericalJointMotor.cpp new file mode 100644 index 000000000..3e5aa30f2 --- /dev/null +++ b/src/BulletDynamics/Featherstone/btMultiBodySphericalJointMotor.cpp @@ -0,0 +1,172 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2018 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 "btMultiBodySphericalJointMotor.h" +#include "btMultiBody.h" +#include "btMultiBodyLinkCollider.h" +#include "BulletCollision/CollisionDispatch/btCollisionObject.h" +#include "LinearMath/btTransformUtil.h" +#include "BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h" + +btMultiBodySphericalJointMotor::btMultiBodySphericalJointMotor(btMultiBody* body, int link, btScalar maxMotorImpulse) + : btMultiBodyConstraint(body, body, link, body->getLink(link).m_parent, 3, true), + m_desiredVelocity(0, 0, 0), + m_desiredPosition(0,0,0,1), + m_kd(1.), + m_kp(0.2), + m_erp(1), + m_rhsClamp(SIMD_INFINITY) +{ + + m_maxAppliedImpulse = maxMotorImpulse; +} + + +void btMultiBodySphericalJointMotor::finalizeMultiDof() +{ + allocateJacobiansMultiDof(); + // note: we rely on the fact that data.m_jacobians are + // always initialized to zero by the Constraint ctor + int linkDoF = 0; + unsigned int offset = 6 + (m_bodyA->getLink(m_linkA).m_dofOffset + linkDoF); + + // row 0: the lower bound + // row 0: the lower bound + jacobianA(0)[offset] = 1; + + m_numDofsFinalized = m_jacSizeBoth; +} + + +btMultiBodySphericalJointMotor::~btMultiBodySphericalJointMotor() +{ +} + +int btMultiBodySphericalJointMotor::getIslandIdA() const +{ + if (this->m_linkA < 0) + { + btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider(); + if (col) + return col->getIslandTag(); + } + else + { + if (m_bodyA->getLink(m_linkA).m_collider) + { + return m_bodyA->getLink(m_linkA).m_collider->getIslandTag(); + } + } + return -1; +} + +int btMultiBodySphericalJointMotor::getIslandIdB() const +{ + if (m_linkB < 0) + { + btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider(); + if (col) + return col->getIslandTag(); + } + else + { + if (m_bodyB->getLink(m_linkB).m_collider) + { + return m_bodyB->getLink(m_linkB).m_collider->getIslandTag(); + } + } + return -1; +} + +void btMultiBodySphericalJointMotor::createConstraintRows(btMultiBodyConstraintArray& constraintRows, + btMultiBodyJacobianData& data, + const btContactSolverInfo& infoGlobal) +{ + // only positions need to be updated -- data.m_jacobians and force + // directions were set in the ctor and never change. + + if (m_numDofsFinalized != m_jacSizeBoth) + { + finalizeMultiDof(); + } + + //don't crash + if (m_numDofsFinalized != m_jacSizeBoth) + return; + + + if (m_maxAppliedImpulse == 0.f) + return; + + const btScalar posError = 0; + const btVector3 dummy(0, 0, 0); + + + btVector3 axis[3] = { btVector3(1, 0, 0), btVector3(0, 1, 0), btVector3(0, 0, 1) }; + + btQuaternion desiredQuat = m_desiredPosition; + btQuaternion currentQuat(m_bodyA->getJointPosMultiDof(m_linkA)[0], + m_bodyA->getJointPosMultiDof(m_linkA)[1], + m_bodyA->getJointPosMultiDof(m_linkA)[2], + m_bodyA->getJointPosMultiDof(m_linkA)[3]); + +btQuaternion relRot = currentQuat.inverse() * desiredQuat; + btVector3 angleDiff; + btGeneric6DofSpring2Constraint::matrixToEulerXYZ(btMatrix3x3(relRot), angleDiff); + + + + for (int row = 0; row < getNumRows(); row++) + { + btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing(); + + int dof = row; + + btScalar currentVelocity = m_bodyA->getJointVelMultiDof(m_linkA)[dof]; + btScalar desiredVelocity = this->m_desiredVelocity[row]; + + btScalar velocityError = desiredVelocity - currentVelocity; + + btMatrix3x3 frameAworld; + frameAworld.setIdentity(); + frameAworld = m_bodyA->localFrameToWorld(m_linkA, frameAworld); + btScalar posError = 0; + { + btAssert(m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::eSpherical); + switch (m_bodyA->getLink(m_linkA).m_jointType) + { + case btMultibodyLink::eSpherical: + { + btVector3 constraintNormalAng = frameAworld.getColumn(row % 3); + posError = m_kp*angleDiff[row % 3]; + fillMultiBodyConstraint(constraintRow, data, 0, 0, constraintNormalAng, + btVector3(0,0,0), dummy, dummy, + posError, + infoGlobal, + -m_maxAppliedImpulse, m_maxAppliedImpulse, true); + constraintRow.m_orgConstraint = this; + constraintRow.m_orgDofIndex = row; + break; + } + default: + { + btAssert(0); + } + }; + } + } +} diff --git a/src/BulletDynamics/Featherstone/btMultiBodySphericalJointMotor.h b/src/BulletDynamics/Featherstone/btMultiBodySphericalJointMotor.h new file mode 100644 index 000000000..621beab5a --- /dev/null +++ b/src/BulletDynamics/Featherstone/btMultiBodySphericalJointMotor.h @@ -0,0 +1,77 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2018 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_SPHERICAL_JOINT_MOTOR_H +#define BT_MULTIBODY_SPHERICAL_JOINT_MOTOR_H + +#include "btMultiBodyConstraint.h" +struct btSolverInfo; + +class btMultiBodySphericalJointMotor : public btMultiBodyConstraint +{ +protected: + btVector3 m_desiredVelocity; + btQuaternion m_desiredPosition; + btScalar m_kd; + btScalar m_kp; + btScalar m_erp; + btScalar m_rhsClamp; //maximum error + +public: + btMultiBodySphericalJointMotor(btMultiBody* body, int link, btScalar maxMotorImpulse); + + virtual ~btMultiBodySphericalJointMotor(); + virtual void finalizeMultiDof(); + + virtual int getIslandIdA() const; + virtual int getIslandIdB() const; + + virtual void createConstraintRows(btMultiBodyConstraintArray& constraintRows, + btMultiBodyJacobianData& data, + const btContactSolverInfo& infoGlobal); + + virtual void setVelocityTarget(const btVector3& velTarget, btScalar kd = 1.f) + { + m_desiredVelocity = velTarget; + m_kd = kd; + } + + virtual void setPositionTarget(const btQuaternion& posTarget, btScalar kp = 1.f) + { + m_desiredPosition = posTarget; + m_kp = kp; + } + + virtual void setErp(btScalar erp) + { + m_erp = erp; + } + virtual btScalar getErp() const + { + return m_erp; + } + virtual void setRhsClamp(btScalar rhsClamp) + { + m_rhsClamp = rhsClamp; + } + virtual void debugDraw(class btIDebugDraw* drawer) + { + //todo(erwincoumans) + } +}; + +#endif //BT_MULTIBODY_SPHERICAL_JOINT_MOTOR_H