Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Implement btMultiBodySphericalJointMotor, able to track a quaternion position target.

Browse Source 
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
This commit is contained in:
erwincoumans
2018-11-13 14:32:18 -08:00
parent a06b5de7b6
commit 7dd524075c
13 changed files with 778 additions and 68 deletions
Download Patch File Download Diff File Expand all files Collapse all files

43
examples/SharedMemory/PhysicsClientC_API.cpp
View File

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

4
examples/SharedMemory/PhysicsClientC_API.h
View File

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

2
examples/SharedMemory/PhysicsClientSharedMemory.cpp
View File

@@ -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:

97
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

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

2
examples/SharedMemory/SharedMemoryCommands.h
View File

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