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

Add preliminary PhysX 4.0 backend for PyBullet

Browse Source 
Add inverse dynamics / mass matrix code from DeepMimic, thanks to Xue Bin (Jason) Peng
Add example how to use stable PD control for humanoid with spherical joints (see humanoidMotionCapture.py)
Fix related to TinyRenderer object transforms not updating when using collision filtering
This commit is contained in:
erwincoumans
2019-01-22 21:08:37 -08:00
parent 80684f44ea
commit ae8e83988b
366 changed files with 131855 additions and 359 deletions
Download Patch File Download Diff File Expand all files Collapse all files

105
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@@ -903,6 +903,22 @@ B3_SHARED_API int b3JointControlSetDesiredVelocityMultiDof(b3SharedMemoryCommand
return 0;
}
B3_SHARED_API int b3JointControlSetDesiredForceTorqueMultiDof(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double* forces, int dofCount)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
b3Assert(command);
if ((dofIndex >= 0) && (dofIndex < MAX_DEGREE_OF_FREEDOM ) && dofCount >= 0 && dofCount <= 4)
{
for (int dof = 0; dof < dofCount; dof++)
{
command->m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[dofIndex+dof] = forces[dof];
command->m_updateFlags |= SIM_DESIRED_STATE_HAS_MAX_FORCE;
command->m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex + dof] |= SIM_DESIRED_STATE_HAS_MAX_FORCE;
}
}
return 0;
}
B3_SHARED_API int b3JointControlSetMaximumForce(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
@@ -3459,6 +3475,7 @@ B3_SHARED_API b3SharedMemoryCommandHandle b3InitUserDebugAddParameter(b3PhysicsC
{
command->m_userDebugDrawArgs.m_text[0] = 0;
}
command->m_userDebugDrawArgs.m_rangeMin = rangeMin;
command->m_userDebugDrawArgs.m_rangeMax = rangeMax;
command->m_userDebugDrawArgs.m_startValue = startValue;
@@ -3477,6 +3494,7 @@ B3_SHARED_API b3SharedMemoryCommandHandle b3InitUserDebugReadParameter(b3Physics
command->m_type = CMD_USER_DEBUG_DRAW;
command->m_updateFlags = USER_DEBUG_READ_PARAMETER;
command->m_userDebugDrawArgs.m_itemUniqueId = debugItemUniqueId;
command->m_userDebugDrawArgs.m_parentObjectUniqueId = -1;
return (b3SharedMemoryCommandHandle)command;
}
@@ -3505,6 +3523,7 @@ B3_SHARED_API b3SharedMemoryCommandHandle b3InitUserDebugDrawRemove(b3PhysicsCli
command->m_type = CMD_USER_DEBUG_DRAW;
command->m_updateFlags = USER_DEBUG_REMOVE_ONE_ITEM;
command->m_userDebugDrawArgs.m_itemUniqueId = debugItemUniqueId;
command->m_userDebugDrawArgs.m_parentObjectUniqueId = -1;
return (b3SharedMemoryCommandHandle)command;
}
@@ -3517,6 +3536,7 @@ B3_SHARED_API b3SharedMemoryCommandHandle b3InitUserDebugDrawRemoveAll(b3Physics
b3Assert(command);
command->m_type = CMD_USER_DEBUG_DRAW;
command->m_updateFlags = USER_DEBUG_REMOVE_ALL;
command->m_userDebugDrawArgs.m_parentObjectUniqueId = -1;
return (b3SharedMemoryCommandHandle)command;
}
@@ -3539,6 +3559,7 @@ B3_SHARED_API b3SharedMemoryCommandHandle b3InitDebugDrawingCommand(b3PhysicsCli
b3Assert(command);
command->m_type = CMD_USER_DEBUG_DRAW;
command->m_updateFlags = 0;
command->m_userDebugDrawArgs.m_parentObjectUniqueId = -1;
return (b3SharedMemoryCommandHandle)command;
}
@@ -4377,7 +4398,8 @@ B3_SHARED_API b3SharedMemoryCommandHandle b3CalculateInverseDynamicsCommandInit(
command->m_type = CMD_CALCULATE_INVERSE_DYNAMICS;
command->m_updateFlags = 0;
command->m_calculateInverseDynamicsArguments.m_bodyUniqueId = bodyUniqueId;
command->m_calculateInverseDynamicsArguments.m_flags = 0;
int dofCount = b3ComputeDofCount(physClient, bodyUniqueId);
for (int i = 0; i < dofCount; i++)
@@ -4405,6 +4427,7 @@ B3_SHARED_API b3SharedMemoryCommandHandle b3CalculateInverseDynamicsCommandInit2
command->m_type = CMD_CALCULATE_INVERSE_DYNAMICS;
command->m_updateFlags = 0;
command->m_calculateInverseDynamicsArguments.m_bodyUniqueId = bodyUniqueId;
command->m_calculateInverseDynamicsArguments.m_flags = 0;
command->m_calculateInverseDynamicsArguments.m_dofCountQ = dofCountQ;
for (int i = 0; i < dofCountQ; i++)
@@ -4422,6 +4445,12 @@ B3_SHARED_API b3SharedMemoryCommandHandle b3CalculateInverseDynamicsCommandInit2
return (b3SharedMemoryCommandHandle)command;
}
B3_SHARED_API void b3CalculateInverseDynamicsSetFlags(b3SharedMemoryCommandHandle commandHandle, int flags)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
command->m_calculateInverseDynamicsArguments.m_flags = flags;
}
B3_SHARED_API int b3GetStatusInverseDynamicsJointForces(b3SharedMemoryStatusHandle statusHandle,
int* bodyUniqueId,
int* dofCount,
@@ -4511,8 +4540,13 @@ B3_SHARED_API int b3GetStatusJacobian(b3SharedMemoryStatusHandle statusHandle, i
return true;
}
B3_SHARED_API void b3CalculateMassMatrixSetFlags(b3SharedMemoryCommandHandle commandHandle, int flags)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
command->m_calculateMassMatrixArguments.m_flags = flags;
}
B3_SHARED_API b3SharedMemoryCommandHandle b3CalculateMassMatrixCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId, const double* jointPositionsQ)
B3_SHARED_API b3SharedMemoryCommandHandle b3CalculateMassMatrixCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId, const double* jointPositionsQ, int dofCountQ)
{
PhysicsClient* cl = (PhysicsClient*)physClient;
b3Assert(cl);
@@ -4522,11 +4556,14 @@ B3_SHARED_API b3SharedMemoryCommandHandle b3CalculateMassMatrixCommandInit(b3Phy
command->m_type = CMD_CALCULATE_MASS_MATRIX;
command->m_updateFlags = 0;
int numJoints = cl->getNumJoints(bodyUniqueId);
for (int i = 0; i < numJoints; i++)
for (int i = 0; i < dofCountQ; i++)
{
command->m_calculateMassMatrixArguments.m_jointPositionsQ[i] = jointPositionsQ[i];
}
command->m_calculateMassMatrixArguments.m_bodyUniqueId = bodyUniqueId;
command->m_calculateMassMatrixArguments.m_dofCountQ = dofCountQ;
command->m_calculateMassMatrixArguments.m_flags = 0;
return (b3SharedMemoryCommandHandle)command;
}
@@ -5360,3 +5397,63 @@ B3_SHARED_API void b3GetQuaternionDifference(const double startQuat[/*4*/], cons
outOrn[3] = dorn[3];
}
b3Scalar b3GetMatrixElem(const b3Matrix3x3& mat, int index)
{
int i = index % 3;
int j = index / 3;
return mat[i][j];
}
static bool MyMatrixToEulerXYZ(const b3Matrix3x3& mat, b3Vector3& xyz)
{
// rot = cy*cz -cy*sz sy
// cz*sx*sy+cx*sz cx*cz-sx*sy*sz -cy*sx
// -cx*cz*sy+sx*sz cz*sx+cx*sy*sz cx*cy
b3Scalar fi = b3GetMatrixElem(mat, 2);
if (fi < b3Scalar(1.0f))
{
if (fi > b3Scalar(-1.0f))
{
xyz[0] = b3Atan2(-b3GetMatrixElem(mat, 5), b3GetMatrixElem(mat, 8));
xyz[1] = b3Asin(b3GetMatrixElem(mat, 2));
xyz[2] = b3Atan2(-b3GetMatrixElem(mat, 1), b3GetMatrixElem(mat, 0));
return true;
}
else
{
// WARNING. Not unique. XA - ZA = -atan2(r10,r11)
xyz[0] = -b3Atan2(b3GetMatrixElem(mat, 3), b3GetMatrixElem(mat, 4));
xyz[1] = -B3_HALF_PI;
xyz[2] = b3Scalar(0.0);
return false;
}
}
else
{
// WARNING. Not unique. XAngle + ZAngle = atan2(r10,r11)
xyz[0] = b3Atan2(b3GetMatrixElem(mat, 3), b3GetMatrixElem(mat, 4));
xyz[1] = B3_HALF_PI;
xyz[2] = 0.0;
}
return false;
}
B3_SHARED_API void b3GetAxisDifferenceQuaternion(const double startQuat[/*4*/], const double endQuat[/*4*/], double axisOut[/*3*/])
{
b3Quaternion currentQuat(startQuat[0], startQuat[1], startQuat[2], startQuat[3]);
b3Quaternion desiredQuat(endQuat[0], endQuat[1], endQuat[2], endQuat[3]);
b3Quaternion relRot = currentQuat.inverse() * desiredQuat;
b3Vector3 angleDiff;
MyMatrixToEulerXYZ(b3Matrix3x3(relRot), angleDiff);
axisOut[0] = angleDiff[0];
axisOut[1] = angleDiff[1];
axisOut[2] = angleDiff[2];
}

8
examples/SharedMemory/PhysicsClientC_API.h
View File

@@ -386,6 +386,7 @@ extern "C"
const double* jointPositionsQ, const double* jointVelocitiesQdot, const double* jointAccelerations);
B3_SHARED_API b3SharedMemoryCommandHandle b3CalculateInverseDynamicsCommandInit2(b3PhysicsClientHandle physClient, int bodyUniqueId,
const double* jointPositionsQ, int dofCountQ, const double* jointVelocitiesQdot, const double* jointAccelerations, int dofCountQdot);
B3_SHARED_API void b3CalculateInverseDynamicsSetFlags(b3SharedMemoryCommandHandle commandHandle, int flags);
B3_SHARED_API int b3GetStatusInverseDynamicsJointForces(b3SharedMemoryStatusHandle statusHandle,
int* bodyUniqueId,
@@ -398,7 +399,8 @@ extern "C"
double* linearJacobian,
double* angularJacobian);
B3_SHARED_API b3SharedMemoryCommandHandle b3CalculateMassMatrixCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId, const double* jointPositionsQ);
B3_SHARED_API b3SharedMemoryCommandHandle b3CalculateMassMatrixCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId, const double* jointPositionsQ, int dofCountQ);
B3_SHARED_API void b3CalculateMassMatrixSetFlags(b3SharedMemoryCommandHandle commandHandle, int flags);
///the mass matrix is stored in column-major layout of size dofCount*dofCount
B3_SHARED_API int b3GetStatusMassMatrix(b3PhysicsClientHandle physClient, b3SharedMemoryStatusHandle statusHandle, int* dofCount, double* massMatrix);
@@ -452,7 +454,10 @@ extern "C"
///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);
B3_SHARED_API int b3JointControlSetDesiredForceTorqueMultiDof(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double* forces, int dofCount);
///Only use if when controlMode is CONTROL_MODE_TORQUE,
B3_SHARED_API int b3JointControlSetDesiredForceTorque(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value);
@@ -641,6 +646,7 @@ extern "C"
B3_SHARED_API void b3GetQuaternionFromAxisAngle(const double axis[/*3*/], double angle, double outQuat[/*4*/]);
B3_SHARED_API void b3GetAxisAngleFromQuaternion(const double quat[/*4*/], double axis[/*3*/], double* angle);
B3_SHARED_API void b3GetQuaternionDifference(const double startQuat[/*4*/], const double endQuat[/*4*/], double outOrn[/*4*/]);
B3_SHARED_API void b3GetAxisDifferenceQuaternion(const double startQuat[/*4*/], const double endQuat[/*4*/], double axisOut[/*3*/]);
B3_SHARED_API void b3CalculateVelocityQuaternion(const double startQuat[/*4*/], const double endQuat[/*4*/], double deltaTime, double angVelOut[/*3*/]);
B3_SHARED_API void b3RotateVector(const double quat[/*4*/], const double vec[/*3*/], double vecOut[/*3*/]);

413
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -67,6 +67,13 @@
#include "plugins/pdControlPlugin/pdControlPlugin.h"
#endif //SKIP_STATIC_PD_CONTROL_PLUGIN
#ifdef STATIC_LINK_SPD_PLUGIN
#include "plugins/stablePDPlugin/BulletConversion.h"
#include "plugins/stablePDPlugin/RBDModel.h"
#include "plugins/stablePDPlugin/RBDutil.h"
#endif
#ifdef STATIC_LINK_VR_PLUGIN
#include "plugins/vrSyncPlugin/vrSyncPlugin.h"
#endif
@@ -1790,6 +1797,143 @@ struct PhysicsServerCommandProcessorInternalData
m_userVisualShapeHandles.initHandles();
}
#ifdef STATIC_LINK_SPD_PLUGIN
b3HashMap<btHashPtr, cRBDModel*> m_rbdModels;
cRBDModel* findOrCreateRBDModel(btMultiBody* multiBody, const double* jointPositionsQ, const double* jointVelocitiesQdot)
{
cRBDModel* rbdModel = 0;
cRBDModel** rbdModelPtr = m_rbdModels.find(multiBody);
if (rbdModelPtr)
{
rbdModel = *rbdModelPtr;
}
else
{
rbdModel = new cRBDModel();
Eigen::MatrixXd bodyDefs;
Eigen::MatrixXd jointMat;
btExtractJointBodyFromBullet(multiBody, bodyDefs, jointMat);
btVector3 grav = m_dynamicsWorld->getGravity();
tVector3 gravity(grav[0], grav[1], grav[2], 0);
rbdModel->Init(jointMat, bodyDefs, gravity);
m_rbdModels.insert(multiBody, rbdModel);
}
//sync pose and vel
int baseDofQ = multiBody->hasFixedBase() ? 0 : 7;
int baseDofQdot = multiBody->hasFixedBase() ? 0 : 6;
Eigen::VectorXd pose, vel;
pose.resize(7 + multiBody->getNumPosVars());
vel.resize(7 + multiBody->getNumPosVars()); //??
btTransform tr = multiBody->getBaseWorldTransform();
int dofsrc = 0;
int velsrcdof = 0;
if (baseDofQ == 7)
{
pose[0] = jointPositionsQ[dofsrc++];
pose[1] = jointPositionsQ[dofsrc++];
pose[2] = jointPositionsQ[dofsrc++];
double quatXYZW[4];
quatXYZW[0] = jointPositionsQ[dofsrc++];
quatXYZW[1] = jointPositionsQ[dofsrc++];
quatXYZW[2] = jointPositionsQ[dofsrc++];
quatXYZW[3] = jointPositionsQ[dofsrc++];
pose[3] = quatXYZW[3];
pose[4] = quatXYZW[0];
pose[5] = quatXYZW[1];
pose[6] = quatXYZW[2];
}
else
{
pose[0] = tr.getOrigin()[0];
pose[1] = tr.getOrigin()[1];
pose[2] = tr.getOrigin()[2];
pose[3] = tr.getRotation()[3];
pose[4] = tr.getRotation()[0];
pose[5] = tr.getRotation()[1];
pose[6] = tr.getRotation()[2];
}
if (baseDofQdot == 6)
{
vel[0] = jointVelocitiesQdot[velsrcdof++];
vel[1] = jointVelocitiesQdot[velsrcdof++];
vel[2] = jointVelocitiesQdot[velsrcdof++];
vel[3] = jointVelocitiesQdot[velsrcdof++];
vel[4] = jointVelocitiesQdot[velsrcdof++];
vel[5] = jointVelocitiesQdot[velsrcdof++];
vel[6] = jointVelocitiesQdot[velsrcdof++];
vel[6] = 0;
}
else
{
vel[0] = multiBody->getBaseVel()[0];
vel[1] = multiBody->getBaseVel()[1];
vel[2] = multiBody->getBaseVel()[2];
vel[3] = multiBody->getBaseOmega()[0];
vel[4] = multiBody->getBaseOmega()[1];
vel[5] = multiBody->getBaseOmega()[2];
vel[6] = 0;
}
int dof = 7;
int veldof = 7;
for (int l = 0; l < multiBody->getNumLinks(); l++)
{
switch (multiBody->getLink(l).m_jointType)
{
case btMultibodyLink::eRevolute:
case btMultibodyLink::ePrismatic:
{
pose[dof++] = jointPositionsQ[dofsrc++];
vel[veldof++] = jointVelocitiesQdot[velsrcdof++];
break;
}
case btMultibodyLink::eSpherical:
{
double quatXYZW[4];
quatXYZW[0] = jointPositionsQ[dofsrc++];
quatXYZW[1] = jointPositionsQ[dofsrc++];
quatXYZW[2] = jointPositionsQ[dofsrc++];
quatXYZW[3] = jointPositionsQ[dofsrc++];
pose[dof++] = quatXYZW[3];
pose[dof++] = quatXYZW[0];
pose[dof++] = quatXYZW[1];
pose[dof++] = quatXYZW[2];
vel[veldof++] = jointVelocitiesQdot[velsrcdof++];
vel[veldof++] = jointVelocitiesQdot[velsrcdof++];
vel[veldof++] = jointVelocitiesQdot[velsrcdof++];
vel[veldof++] = jointVelocitiesQdot[velsrcdof++];
break;
}
case btMultibodyLink::eFixed:
{
break;
}
default:
{
assert(0);
}
}
}
btVector3 gravOrg = m_dynamicsWorld->getGravity();
tVector grav (gravOrg[0], gravOrg[1], gravOrg[2], 0);
rbdModel->SetGravity(grav);
rbdModel->Update(pose, vel);
return rbdModel;
}
#endif
btInverseDynamics::MultiBodyTree* findOrCreateTree(btMultiBody* multiBody)
{
btInverseDynamics::MultiBodyTree* tree = 0;
@@ -3703,7 +3847,7 @@ bool PhysicsServerCommandProcessor::processRequestCameraImageCommand(const struc
for (int i = 0; i < m_data->m_dynamicsWorld->getNumCollisionObjects(); i++)
{
const btCollisionObject* colObj = m_data->m_dynamicsWorld->getCollisionObjectArray()[i];
m_data->m_pluginManager.getRenderInterface()->syncTransform(colObj->getBroadphaseHandle()->getUid(), colObj->getWorldTransform(), colObj->getCollisionShape()->getLocalScaling());
m_data->m_pluginManager.getRenderInterface()->syncTransform(colObj->getUserIndex3(), colObj->getWorldTransform(), colObj->getCollisionShape()->getLocalScaling());
}
if ((clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_HAS_CAMERA_MATRICES) != 0)
@@ -8142,9 +8286,10 @@ bool PhysicsServerCommandProcessor::processInitPoseCommand(const struct SharedMe
int posVarCountIndex = 7;
for (int i = 0; i < mb->getNumLinks(); i++)
{
bool hasPosVar = true;
for (int j = 0; j < mb->getLink(i).m_posVarCount; j++)
int posVarCount = mb->getLink(i).m_posVarCount;
bool hasPosVar = posVarCount > 0;
for (int j = 0; j < posVarCount; j++)
{
if (clientCmd.m_initPoseArgs.m_hasInitialStateQ[posVarCountIndex+j] == 0)
{
@@ -8154,7 +8299,13 @@ bool PhysicsServerCommandProcessor::processInitPoseCommand(const struct SharedMe
}
if (hasPosVar)
{
mb->setJointPosMultiDof(i, &clientCmd.m_initPoseArgs.m_initialStateQ[posVarCountIndex]);
btQuaternion q(
clientCmd.m_initPoseArgs.m_initialStateQ[posVarCountIndex],
clientCmd.m_initPoseArgs.m_initialStateQ[posVarCountIndex+1],
clientCmd.m_initPoseArgs.m_initialStateQ[posVarCountIndex+2],
clientCmd.m_initPoseArgs.m_initialStateQ[posVarCountIndex+3]);
q.normalize();
mb->setJointPosMultiDof(i, &q[0]);
double vel[6] = { 0, 0, 0, 0, 0, 0 };
mb->setJointVelMultiDof(i, vel);
}
@@ -8540,82 +8691,111 @@ bool PhysicsServerCommandProcessor::processInverseDynamicsCommand(const struct S
serverCmd.m_type = CMD_CALCULATED_INVERSE_DYNAMICS_FAILED;
if (bodyHandle && bodyHandle->m_multiBody)
{
btInverseDynamics::MultiBodyTree* tree = m_data->findOrCreateTree(bodyHandle->m_multiBody);
int baseDofQ = bodyHandle->m_multiBody->hasFixedBase() ? 0 : 7;
int baseDofQdot = bodyHandle->m_multiBody->hasFixedBase() ? 0 : 6;
const int num_dofs = bodyHandle->m_multiBody->getNumDofs();
if (tree && clientCmd.m_calculateInverseDynamicsArguments.m_dofCountQ == (baseDofQ+ num_dofs) &&
clientCmd.m_calculateInverseDynamicsArguments.m_dofCountQdot == (baseDofQdot+ num_dofs))
if (clientCmd.m_calculateInverseDynamicsArguments.m_flags & 1)
{
btInverseDynamics::vecx nu(num_dofs + baseDofQdot), qdot(num_dofs + baseDofQdot), q(num_dofs + baseDofQdot), joint_force(num_dofs + baseDofQdot);
//for floating base, inverse dynamics expects euler angle x,y,z and position x,y,z in that order
//PyBullet expects quaternion, so convert and swap to have a more consistent PyBullet API
if (baseDofQ)
#ifdef STATIC_LINK_SPD_PLUGIN
{
btVector3 pos(clientCmd.m_calculateInverseDynamicsArguments.m_jointPositionsQ[0],
clientCmd.m_calculateInverseDynamicsArguments.m_jointPositionsQ[1],
clientCmd.m_calculateInverseDynamicsArguments.m_jointPositionsQ[2]);
btQuaternion orn(clientCmd.m_calculateInverseDynamicsArguments.m_jointPositionsQ[0],
clientCmd.m_calculateInverseDynamicsArguments.m_jointPositionsQ[1],
clientCmd.m_calculateInverseDynamicsArguments.m_jointPositionsQ[2],
clientCmd.m_calculateInverseDynamicsArguments.m_jointPositionsQ[3]);
btScalar yawZ, pitchY, rollX;
orn.getEulerZYX(yawZ, pitchY, rollX);
q[0] = rollX;
q[1] = pitchY;
q[2] = yawZ;
q[3] = pos[0];
q[4] = pos[1];
q[5] = pos[2];
}
else
{
for (int i = 0; i < num_dofs; i++)
cRBDModel* rbdModel = m_data->findOrCreateRBDModel(bodyHandle->m_multiBody,
clientCmd.m_calculateInverseDynamicsArguments.m_jointPositionsQ,
clientCmd.m_calculateInverseDynamicsArguments.m_jointVelocitiesQdot);
if (rbdModel)
{
q[i] = clientCmd.m_calculateInverseDynamicsArguments.m_jointPositionsQ[i];
int posVal = bodyHandle->m_multiBody->getNumPosVars();
Eigen::VectorXd acc2 = Eigen::VectorXd::Zero(7 + posVal);
Eigen::VectorXd out_tau = Eigen::VectorXd::Zero(7 + posVal);
cRBDUtil::SolveInvDyna(*rbdModel, acc2, out_tau);
int dof = 7 + bodyHandle->m_multiBody->getNumPosVars();
for (int i = 0; i < dof; i++)
{
serverCmd.m_inverseDynamicsResultArgs.m_jointForces[i] = out_tau[i];
}
serverCmd.m_inverseDynamicsResultArgs.m_bodyUniqueId = clientCmd.m_calculateInverseDynamicsArguments.m_bodyUniqueId;
serverCmd.m_inverseDynamicsResultArgs.m_dofCount = dof;
serverCmd.m_type = CMD_CALCULATED_INVERSE_DYNAMICS_COMPLETED;
}
}
for (int i = 0; i < num_dofs + baseDofQdot; i++)
#endif
} else
{
btInverseDynamics::MultiBodyTree* tree = m_data->findOrCreateTree(bodyHandle->m_multiBody);
int baseDofQ = bodyHandle->m_multiBody->hasFixedBase() ? 0 : 7;
int baseDofQdot = bodyHandle->m_multiBody->hasFixedBase() ? 0 : 6;
const int num_dofs = bodyHandle->m_multiBody->getNumDofs();
if (tree && clientCmd.m_calculateInverseDynamicsArguments.m_dofCountQ == (baseDofQ + num_dofs) &&
clientCmd.m_calculateInverseDynamicsArguments.m_dofCountQdot == (baseDofQdot + num_dofs))
{
qdot[i] = clientCmd.m_calculateInverseDynamicsArguments.m_jointVelocitiesQdot[i];
nu[i] = clientCmd.m_calculateInverseDynamicsArguments.m_jointAccelerations[i];
}
// Set the gravity to correspond to the world gravity
btInverseDynamics::vec3 id_grav(m_data->m_dynamicsWorld->getGravity());
btInverseDynamics::vecx nu(num_dofs + baseDofQdot), qdot(num_dofs + baseDofQdot), q(num_dofs + baseDofQdot), joint_force(num_dofs + baseDofQdot);
if (-1 != tree->setGravityInWorldFrame(id_grav) &&
-1 != tree->calculateInverseDynamics(q, qdot, nu, &joint_force))
{
serverCmd.m_inverseDynamicsResultArgs.m_bodyUniqueId = clientCmd.m_calculateInverseDynamicsArguments.m_bodyUniqueId;
serverCmd.m_inverseDynamicsResultArgs.m_dofCount = num_dofs+ baseDofQdot;
//inverse dynamics stores angular before linear, swap it to have a consistent PyBullet API.
if (baseDofQdot)
//for floating base, inverse dynamics expects euler angle x,y,z and position x,y,z in that order
//PyBullet expects quaternion, so convert and swap to have a more consistent PyBullet API
if (baseDofQ)
{
serverCmd.m_inverseDynamicsResultArgs.m_jointForces[0] = joint_force[3];
serverCmd.m_inverseDynamicsResultArgs.m_jointForces[1] = joint_force[4];
serverCmd.m_inverseDynamicsResultArgs.m_jointForces[2] = joint_force[5];
serverCmd.m_inverseDynamicsResultArgs.m_jointForces[3] = joint_force[0];
serverCmd.m_inverseDynamicsResultArgs.m_jointForces[4] = joint_force[1];
serverCmd.m_inverseDynamicsResultArgs.m_jointForces[5] = joint_force[2];
btVector3 pos(clientCmd.m_calculateInverseDynamicsArguments.m_jointPositionsQ[0],
clientCmd.m_calculateInverseDynamicsArguments.m_jointPositionsQ[1],
clientCmd.m_calculateInverseDynamicsArguments.m_jointPositionsQ[2]);
btQuaternion orn(clientCmd.m_calculateInverseDynamicsArguments.m_jointPositionsQ[0],
clientCmd.m_calculateInverseDynamicsArguments.m_jointPositionsQ[1],
clientCmd.m_calculateInverseDynamicsArguments.m_jointPositionsQ[2],
clientCmd.m_calculateInverseDynamicsArguments.m_jointPositionsQ[3]);
btScalar yawZ, pitchY, rollX;
orn.getEulerZYX(yawZ, pitchY, rollX);
q[0] = rollX;
q[1] = pitchY;
q[2] = yawZ;
q[3] = pos[0];
q[4] = pos[1];
q[5] = pos[2];
}
for (int i = baseDofQdot; i < num_dofs+ baseDofQdot; i++)
else
{
serverCmd.m_inverseDynamicsResultArgs.m_jointForces[i] = joint_force[i];
for (int i = 0; i < num_dofs; i++)
{
q[i] = clientCmd.m_calculateInverseDynamicsArguments.m_jointPositionsQ[i];
}
}
for (int i = 0; i < num_dofs + baseDofQdot; i++)
{
qdot[i] = clientCmd.m_calculateInverseDynamicsArguments.m_jointVelocitiesQdot[i];
nu[i] = clientCmd.m_calculateInverseDynamicsArguments.m_jointAccelerations[i];
}
// Set the gravity to correspond to the world gravity
btInverseDynamics::vec3 id_grav(m_data->m_dynamicsWorld->getGravity());
if (-1 != tree->setGravityInWorldFrame(id_grav) &&
-1 != tree->calculateInverseDynamics(q, qdot, nu, &joint_force))
{
serverCmd.m_inverseDynamicsResultArgs.m_bodyUniqueId = clientCmd.m_calculateInverseDynamicsArguments.m_bodyUniqueId;
serverCmd.m_inverseDynamicsResultArgs.m_dofCount = num_dofs + baseDofQdot;
//inverse dynamics stores angular before linear, swap it to have a consistent PyBullet API.
if (baseDofQdot)
{
serverCmd.m_inverseDynamicsResultArgs.m_jointForces[0] = joint_force[3];
serverCmd.m_inverseDynamicsResultArgs.m_jointForces[1] = joint_force[4];
serverCmd.m_inverseDynamicsResultArgs.m_jointForces[2] = joint_force[5];
serverCmd.m_inverseDynamicsResultArgs.m_jointForces[3] = joint_force[0];
serverCmd.m_inverseDynamicsResultArgs.m_jointForces[4] = joint_force[1];
serverCmd.m_inverseDynamicsResultArgs.m_jointForces[5] = joint_force[2];
}
for (int i = baseDofQdot; i < num_dofs + baseDofQdot; i++)
{
serverCmd.m_inverseDynamicsResultArgs.m_jointForces[i] = joint_force[i];
}
serverCmd.m_type = CMD_CALCULATED_INVERSE_DYNAMICS_COMPLETED;
}
else
{
serverCmd.m_type = CMD_CALCULATED_INVERSE_DYNAMICS_FAILED;
}
serverCmd.m_type = CMD_CALCULATED_INVERSE_DYNAMICS_COMPLETED;
}
else
{
serverCmd.m_type = CMD_CALCULATED_INVERSE_DYNAMICS_FAILED;
}
}
}
@@ -8732,37 +8912,82 @@ bool PhysicsServerCommandProcessor::processCalculateMassMatrixCommand(const stru
InternalBodyHandle* bodyHandle = m_data->m_bodyHandles.getHandle(clientCmd.m_calculateMassMatrixArguments.m_bodyUniqueId);
if (bodyHandle && bodyHandle->m_multiBody)
{
btInverseDynamics::MultiBodyTree* tree = m_data->findOrCreateTree(bodyHandle->m_multiBody);
if (tree)
if (clientCmd.m_calculateMassMatrixArguments.m_flags & 1)
{
int baseDofs = bodyHandle->m_multiBody->hasFixedBase() ? 0 : 6;
const int numDofs = bodyHandle->m_multiBody->getNumDofs();
const int totDofs = numDofs + baseDofs;
btInverseDynamics::vecx q(totDofs);
btInverseDynamics::matxx massMatrix(totDofs, totDofs);
for (int i = 0; i < numDofs; i++)
#ifdef STATIC_LINK_SPD_PLUGIN
{
q[i + baseDofs] = clientCmd.m_calculateMassMatrixArguments.m_jointPositionsQ[i];
}
if (-1 != tree->calculateMassMatrix(q, &massMatrix))
{
serverCmd.m_massMatrixResultArgs.m_dofCount = totDofs;
// Fill in the result into the shared memory.
double* sharedBuf = (double*)bufferServerToClient;
int sizeInBytes = totDofs * totDofs * sizeof(double);
if (sizeInBytes < bufferSizeInBytes)
int posVal = bodyHandle->m_multiBody->getNumPosVars();
btAlignedObjectArray<double> zeroVel;
int dof = 7 + posVal;
zeroVel.resize(dof);
cRBDModel* rbdModel = m_data->findOrCreateRBDModel(bodyHandle->m_multiBody, clientCmd.m_calculateMassMatrixArguments.m_jointPositionsQ,
&zeroVel[0]);
if (rbdModel)
{
for (int i = 0; i < (totDofs); ++i)
Eigen::MatrixXd out_mass;
cRBDUtil::BuildMassMat(*rbdModel, out_mass);
int skipDofs = 0;// 7 - baseDofQ;
int totDofs = dof;
serverCmd.m_massMatrixResultArgs.m_dofCount = totDofs-skipDofs;
// Fill in the result into the shared memory.
double* sharedBuf = (double*)bufferServerToClient;
int sizeInBytes = totDofs * totDofs * sizeof(double);
if (sizeInBytes < bufferSizeInBytes)
{
for (int j = 0; j < (totDofs); ++j)
for (int i = skipDofs; i < (totDofs); ++i)
{
int element = (totDofs)*i + j;
sharedBuf[element] = massMatrix(i, j);
for (int j = skipDofs; j < (totDofs); ++j)
{
int element = (totDofs- skipDofs)*(i- skipDofs) + (j- skipDofs);
double v = out_mass(i, j);
if (i == j && v == 0)
{
v = 1;
}
sharedBuf[element] = v;
}
}
serverCmd.m_type = CMD_CALCULATED_MASS_MATRIX_COMPLETED;
}
}
}
#endif
}
else
{
btInverseDynamics::MultiBodyTree* tree = m_data->findOrCreateTree(bodyHandle->m_multiBody);
if (tree)
{
int baseDofs = bodyHandle->m_multiBody->hasFixedBase() ? 0 : 6;
const int numDofs = bodyHandle->m_multiBody->getNumDofs();
const int totDofs = numDofs + baseDofs;
btInverseDynamics::vecx q(totDofs);
btInverseDynamics::matxx massMatrix(totDofs, totDofs);
for (int i = 0; i < numDofs; i++)
{
q[i + baseDofs] = clientCmd.m_calculateMassMatrixArguments.m_jointPositionsQ[i];
}
if (-1 != tree->calculateMassMatrix(q, &massMatrix))
{
serverCmd.m_massMatrixResultArgs.m_dofCount = totDofs;
// Fill in the result into the shared memory.
double* sharedBuf = (double*)bufferServerToClient;
int sizeInBytes = totDofs * totDofs * sizeof(double);
if (sizeInBytes < bufferSizeInBytes)
{
for (int i = 0; i < (totDofs); ++i)
{
for (int j = 0; j < (totDofs); ++j)
{
int element = (totDofs)*i + j;
sharedBuf[element] = massMatrix(i, j);
}
}
serverCmd.m_type = CMD_CALCULATED_MASS_MATRIX_COMPLETED;
}
serverCmd.m_type = CMD_CALCULATED_MASS_MATRIX_COMPLETED;
}
}
}
@@ -9800,7 +10025,7 @@ bool PhysicsServerCommandProcessor::processCalculateInverseKinematicsCommand(con
numDofs, clientCmd.m_calculateInverseKinematicsArguments.m_endEffectorLinkIndex,
&q_new[0], ikMethod, &jacobian_linear[0], &jacobian_angular[0], jacSize * 2, targetDampCoeff);
}
serverCmd.m_inverseKinematicsResultArgs.m_bodyUniqueId = clientCmd.m_calculateInverseKinematicsArguments.m_bodyUniqueId;
serverCmd.m_inverseKinematicsResultArgs.m_bodyUniqueId = clientCmd.m_calculateInverseDynamicsArguments.m_bodyUniqueId;
for (int i = 0; i < numDofs; i++)
{
serverCmd.m_inverseKinematicsResultArgs.m_jointPositions[i] = q_new[i];

3
examples/SharedMemory/SharedMemoryCommands.h
View File

@@ -663,6 +663,7 @@ struct CalculateInverseDynamicsArgs
double m_jointPositionsQ[MAX_DEGREE_OF_FREEDOM];
double m_jointVelocitiesQdot[MAX_DEGREE_OF_FREEDOM];
double m_jointAccelerations[MAX_DEGREE_OF_FREEDOM];
int m_flags;
};
struct CalculateInverseDynamicsResultArgs
@@ -693,6 +694,8 @@ struct CalculateMassMatrixArgs
{
int m_bodyUniqueId;
double m_jointPositionsQ[MAX_DEGREE_OF_FREEDOM];
int m_dofCountQ;
int m_flags;
};
struct CalculateMassMatrixResultArgs

2
examples/SharedMemory/SharedMemoryPublic.h
View File

@@ -817,6 +817,7 @@ enum eCONNECT_METHOD
eCONNECT_DART = 10,
eCONNECT_MUJOCO = 11,
eCONNECT_GRPC = 12,
eCONNECT_PHYSX=13,
};
enum eURDF_Flags
@@ -836,6 +837,7 @@ enum eURDF_Flags
URDF_PARSE_SENSORS = 16384,
URDF_USE_MATERIAL_COLORS_FROM_MTL = 32768,
URDF_USE_MATERIAL_TRANSPARANCY_FROM_MTL = 65536,
URDF_MAINTAIN_LINK_ORDER = 131072,
};
enum eUrdfGeomTypes //sync with UrdfParser UrdfGeomTypes

15
examples/SharedMemory/physx/PhysXC_API.cpp Normal file
View File

@@ -0,0 +1,15 @@
#ifdef BT_ENABLE_PHYSX
#include "PhysXC_API.h"
#include "PhysXServerCommandProcessor.h"
#include "PhysXClient.h"
B3_SHARED_API b3PhysicsClientHandle b3ConnectPhysX()
{
PhysXServerCommandProcessor* sdk = new PhysXServerCommandProcessor;
PhysXClient* direct = new PhysXClient(sdk, true);
bool connected;
connected = direct->connect();
return (b3PhysicsClientHandle)direct;
}
#endif //BT_ENABLE_PHYSX

20
examples/SharedMemory/physx/PhysXC_API.h Normal file
View File

@@ -0,0 +1,20 @@
#ifndef PHYSX_C_API_H
#define PHYSX_C_API_H
#ifdef BT_ENABLE_PHYSX
#include "../PhysicsClientC_API.h"
#ifdef __cplusplus
extern "C"
{
#endif
B3_SHARED_API b3PhysicsClientHandle b3ConnectPhysX();
#ifdef __cplusplus
}
#endif
#endif //BT_ENABLE_PHYSX
#endif //PHYSX_C_API_H

1409
examples/SharedMemory/physx/PhysXClient.cpp Normal file
View File

File diff suppressed because it is too large Load Diff

135
examples/SharedMemory/physx/PhysXClient.h Normal file
View File

@@ -0,0 +1,135 @@
#ifndef PHYSX_CLIENT_H
#define PHYSX_CLIENT_H
#include "../PhysicsClient.h"
///PhysicsDirect executes the commands directly, without transporting them or having a separate server executing commands
class PhysXClient : public PhysicsClient
{
protected:
struct PhysXDirectInternalData* m_data;
bool processDebugLines(const struct SharedMemoryCommand& orgCommand);
bool processCamera(const struct SharedMemoryCommand& orgCommand);
bool processContactPointData(const struct SharedMemoryCommand& orgCommand);
bool processOverlappingObjects(const struct SharedMemoryCommand& orgCommand);
bool processVisualShapeData(const struct SharedMemoryCommand& orgCommand);
void processBodyJointInfo(int bodyUniqueId, const struct SharedMemoryStatus& serverCmd);
void processAddUserData(const struct SharedMemoryStatus& serverCmd);
void postProcessStatus(const struct SharedMemoryStatus& serverCmd);
void resetData();
void removeCachedBody(int bodyUniqueId);
public:
PhysXClient(class PhysicsCommandProcessorInterface* physSdk, bool passSdkOwnership);
virtual ~PhysXClient();
// return true if connection succesfull, can also check 'isConnected'
//it is OK to pass a null pointer for the gui helper
virtual bool connect();
////todo: rename to 'disconnect'
virtual void disconnectSharedMemory();
virtual bool isConnected() const;
// return non-null if there is a status, nullptr otherwise
virtual const SharedMemoryStatus* processServerStatus();
virtual SharedMemoryCommand* getAvailableSharedMemoryCommand();
virtual bool canSubmitCommand() const;
virtual bool submitClientCommand(const struct SharedMemoryCommand& command);
virtual int getNumBodies() const;
virtual int getBodyUniqueId(int serialIndex) const;
virtual bool getBodyInfo(int bodyUniqueId, struct b3BodyInfo& info) const;
virtual int getNumJoints(int bodyIndex) const;
virtual bool getJointInfo(int bodyIndex, int jointIndex, struct b3JointInfo& info) const;
virtual int getNumUserConstraints() const;
virtual int getUserConstraintInfo(int constraintUniqueId, struct b3UserConstraint& info) const;
virtual int getUserConstraintId(int serialIndex) const;
///todo: move this out of the
virtual void setSharedMemoryKey(int key);
void uploadBulletFileToSharedMemory(const char* data, int len);
virtual void uploadRaysToSharedMemory(struct SharedMemoryCommand& command, const double* rayFromWorldArray, const double* rayToWorldArray, int numRays);
virtual int getNumDebugLines() const;
virtual const float* getDebugLinesFrom() const;
virtual const float* getDebugLinesTo() const;
virtual const float* getDebugLinesColor() const;
virtual void getCachedCameraImage(b3CameraImageData* cameraData);
virtual void getCachedContactPointInformation(struct b3ContactInformation* contactPointData);
virtual void getCachedOverlappingObjects(struct b3AABBOverlapData* overlappingObjects);
virtual void getCachedVisualShapeInformation(struct b3VisualShapeInformation* visualShapesInfo);
virtual void getCachedCollisionShapeInformation(struct b3CollisionShapeInformation* collisionShapesInfo);
virtual void getCachedVREvents(struct b3VREventsData* vrEventsData);
virtual void getCachedKeyboardEvents(struct b3KeyboardEventsData* keyboardEventsData);
virtual void getCachedMouseEvents(struct b3MouseEventsData* mouseEventsData);
virtual void getCachedRaycastHits(struct b3RaycastInformation* raycastHits);
virtual void getCachedMassMatrix(int dofCountCheck, double* massMatrix);
//the following APIs are for internal use for visualization:
virtual bool connect(struct GUIHelperInterface* guiHelper);
virtual void renderScene();
virtual void debugDraw(int debugDrawMode);
virtual void setTimeOut(double timeOutInSeconds);
virtual double getTimeOut() const;
virtual bool getCachedUserData(int userDataId, struct b3UserDataValue& valueOut) const
{
return false;
}
virtual int getCachedUserDataId(int bodyUniqueId, int linkIndex, int visualShapeIndex, const char* key) const
{
return -1;
}
virtual int getNumUserData(int bodyUniqueId) const
{
return 0;
}
virtual void getUserDataInfo(int bodyUniqueId, int userDataIndex, const char** keyOut, int* userDataIdOut, int* linkIndexOut, int* visualShapeIndexOut) const
{
*keyOut = 0;
*userDataIdOut = -1;
return;
}
virtual void pushProfileTiming(const char* timingName);
virtual void popProfileTiming();
};
#endif //PHYSX_CLIENT_H

1639
examples/SharedMemory/physx/PhysXServerCommandProcessor.cpp Normal file
View File

File diff suppressed because it is too large Load Diff

47
examples/SharedMemory/physx/PhysXServerCommandProcessor.h Normal file
View File

@@ -0,0 +1,47 @@
#ifndef PHYSX_SERVER_COMMAND_PROCESSOR_H
#define PHYSX_SERVER_COMMAND_PROCESSOR_H
#include "../PhysicsCommandProcessorInterface.h"
class PhysXServerCommandProcessor : public PhysicsCommandProcessorInterface
{
struct PhysXServerCommandProcessorInternalData* m_data;
bool processSyncBodyInfoCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
bool processRequestInternalDataCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
bool processSyncUserDataCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
bool processLoadURDFCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
bool processRequestBodyInfoCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
bool processForwardDynamicsCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
bool processSendPhysicsParametersCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
bool processRequestActualStateCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
bool processResetSimulationCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
bool processCustomCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
void resetSimulation();
bool processStateLoggingCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
public:
PhysXServerCommandProcessor();
virtual ~PhysXServerCommandProcessor();
virtual bool connect();
virtual void disconnect();
virtual bool isConnected() const;
virtual bool processCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
virtual bool receiveStatus(struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
virtual void renderScene(int renderFlags) {}
virtual void physicsDebugDraw(int debugDrawFlags) {}
virtual void setGuiHelper(struct GUIHelperInterface* guiHelper) {}
virtual void setTimeOut(double timeOutInSeconds) {}
virtual void reportNotifications() {}
};
#endif //PHYSX_SERVER_COMMAND_PROCESSOR_H

1434
examples/SharedMemory/physx/PhysXUrdfImporter.cpp Normal file
View File

File diff suppressed because it is too large Load Diff

116
examples/SharedMemory/physx/PhysXUrdfImporter.h Normal file
View File

@@ -0,0 +1,116 @@
#ifndef PHYSX_URDF_IMPORTER_H
#define PHYSX_URDF_IMPORTER_H
#include "../../Importers/ImportURDFDemo/URDFImporterInterface.h"
#include "../../Importers/ImportURDFDemo/UrdfRenderingInterface.h"
struct PhysXURDFTexture
{
int m_width;
int m_height;
unsigned char* textureData1;
bool m_isCached;
};
///PhysXURDFImporter can deal with URDF and SDF files
class PhysXURDFImporter : public URDFImporterInterface
{
struct PhysXURDFInternalData* m_data;
public:
PhysXURDFImporter(struct CommonFileIOInterface* fileIO=0,double globalScaling=1, int flags=0);
virtual ~PhysXURDFImporter();
virtual bool loadURDF(const char* fileName, bool forceFixedBase = false);
//warning: some quick test to load SDF: we 'activate' a model, so we can re-use URDF code path
virtual bool loadSDF(const char* fileName, bool forceFixedBase = false);
virtual int getNumModels() const;
virtual void activateModel(int modelIndex);
virtual void setBodyUniqueId(int bodyId);
virtual int getBodyUniqueId() const;
const char* getPathPrefix();
void printTree(); //for debugging
virtual int getRootLinkIndex() const;
virtual void getLinkChildIndices(int linkIndex, btAlignedObjectArray<int>& childLinkIndices) const;
virtual std::string getBodyName() const;
virtual std::string getLinkName(int linkIndex) const;
virtual bool getLinkColor(int linkIndex, btVector4& colorRGBA) const;
virtual bool getLinkColor2(int linkIndex, UrdfMaterialColor& matCol) const;
virtual void setLinkColor2(int linkIndex, struct UrdfMaterialColor& matCol) const;
virtual bool getLinkContactInfo(int urdflinkIndex, URDFLinkContactInfo& contactInfo) const;
virtual bool getLinkAudioSource(int linkIndex, SDFAudioSource& audioSource) const;
virtual std::string getJointName(int linkIndex) const;
virtual void getMassAndInertia(int linkIndex, btScalar& mass, btVector3& localInertiaDiagonal, btTransform& inertialFrame) const;
virtual void getMassAndInertia2(int urdfLinkIndex, btScalar& mass, btVector3& localInertiaDiagonal, btTransform& inertialFrame, int flags) const;
virtual bool getJointInfo(int urdfLinkIndex, btTransform& parent2joint, btTransform& linkTransformInWorld, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit, btScalar& jointDamping, btScalar& jointFriction) const;
virtual bool getJointInfo2(int urdfLinkIndex, btTransform& parent2joint, btTransform& linkTransformInWorld, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit, btScalar& jointDamping, btScalar& jointFriction, btScalar& jointMaxForce, btScalar& jointMaxVelocity) const;
virtual bool getRootTransformInWorld(btTransform& rootTransformInWorld) const;
virtual void setRootTransformInWorld(const btTransform& rootTransformInWorld);
virtual int convertLinkVisualShapes3(
int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame,
const struct UrdfLink* linkPtr, const UrdfModel* model,
int collisionObjectUniqueId, int bodyUniqueId, struct CommonFileIOInterface* fileIO) const;
virtual void convertLinkVisualShapes2(int linkIndex, int urdfIndex, const char* pathPrefix, const btTransform& inertialFrame, class btCollisionObject* colObj, int bodyUniqueId) const;
class btCollisionShape* convertURDFToCollisionShape(const struct UrdfCollision* collision, const char* urdfPathPrefix) const
{
return 0;
}
virtual int getUrdfFromCollisionShape(const btCollisionShape* collisionShape, UrdfCollision& collision) const;
///todo(erwincoumans) refactor this convertLinkCollisionShapes/memory allocation
virtual class btCompoundShape* convertLinkCollisionShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame) const
{
return 0;
}
virtual const struct UrdfLink* getUrdfLink(int urdfLinkIndex) const;
virtual const struct UrdfModel* getUrdfModel() const;
virtual int getCollisionGroupAndMask(int linkIndex, int& colGroup, int& colMask) const;
virtual int getNumAllocatedCollisionShapes() const;
virtual class btCollisionShape* getAllocatedCollisionShape(int index);
virtual int getNumAllocatedMeshInterfaces() const;
virtual class btStridingMeshInterface* getAllocatedMeshInterface(int index);
virtual int getNumAllocatedTextures() const;
virtual int getAllocatedTexture(int index) const;
virtual void setEnableTinyRenderer(bool enable);
//void convertURDFToVisualShapeInternal(const struct UrdfVisual* visual, const char* urdfPathPrefix, const class btTransform& visualTransform, btAlignedObjectArray<struct GLInstanceVertex>& verticesOut, btAlignedObjectArray<int>& indicesOut, btAlignedObjectArray<struct PhysXURDFTexture>& texturesOut, struct b3ImportMeshData& meshData) const;
int getNumPhysXLinks() const;
};
#endif //PHYSX_URDF_IMPORTER_H

8
examples/SharedMemory/physx/PhysXUserData.h Normal file
View File

@@ -0,0 +1,8 @@
#ifndef PHYSX_USER_DATA_H
#define PHYSX_USER_DATA_H
struct MyPhysXUserData
{
int m_graphicsUniqueId;
};
#endif //PHYSX_USER_DATA_H

908
examples/SharedMemory/physx/URDF2PhysX.cpp Normal file
View File

@@ -0,0 +1,908 @@
#include "URDF2PhysX.h"
#include "PhysXUrdfImporter.h"
#include "Bullet3Common/b3Logging.h"
#include "PxArticulationReducedCoordinate.h"
#include "PxArticulationJointReducedCoordinate.h"
#include "PxRigidActorExt.h"
#include "PxArticulation.h"
#include "PxRigidBodyExt.h"
#include "PxRigidBody.h"
#include "LinearMath/btThreads.h"
#include "PxRigidActorExt.h"
#include "PxArticulationBase.h"
#include "PxArticulationLink.h"
#include "PxMaterial.h"
#include "PxCooking.h"
#include "PxScene.h"
#include "PxAggregate.h"
#include "PhysXUserData.h"
#include "../../CommonInterfaces/CommonFileIOInterface.h"
#include "Importers/ImportURDFDemo/UrdfParser.h"
struct URDF2PhysXCachedData
{
URDF2PhysXCachedData()
: m_currentMultiBodyLinkIndex(-1),
m_articulation(0),
m_totalNumJoints1(0)
{
}
//these arrays will be initialized in the 'InitURDF2BulletCache'
btAlignedObjectArray<int> m_urdfLinkParentIndices;
btAlignedObjectArray<int> m_urdfLinkIndices2BulletLinkIndices;
btAlignedObjectArray<class physx::PxArticulationLink*> m_urdfLink2physxLink;
btAlignedObjectArray<btTransform> m_urdfLinkLocalInertialFrames;
int m_currentMultiBodyLinkIndex;
physx::PxArticulationReducedCoordinate* m_articulation;
btAlignedObjectArray<physx::PxTransform> m_linkTransWorldSpace;
btAlignedObjectArray<int> m_urdfLinkIndex;
btAlignedObjectArray<int> m_parentUrdfLinkIndex;
btAlignedObjectArray<physx::PxReal> m_linkMasses;
btAlignedObjectArray<physx::PxArticulationJointType::Enum> m_jointTypes;
btAlignedObjectArray<physx::PxTransform> m_parentLocalPoses;
btAlignedObjectArray<physx::PxTransform> m_childLocalPoses;
btAlignedObjectArray<UrdfGeomTypes> m_geomTypes;
btAlignedObjectArray<physx::PxVec3> m_geomDimensions;
btAlignedObjectArray<physx::PxVec3> m_linkMaterials;
btAlignedObjectArray<physx::PxTransform> m_geomLocalPoses;
//this will be initialized in the constructor
int m_totalNumJoints1;
int getParentUrdfIndex(int linkIndex) const
{
return m_urdfLinkParentIndices[linkIndex];
}
int getMbIndexFromUrdfIndex(int urdfIndex) const
{
if (urdfIndex == -2)
return -2;
return m_urdfLinkIndices2BulletLinkIndices[urdfIndex];
}
void registerMultiBody(int urdfLinkIndex, class physx::PxArticulationLink* body, const btTransform& worldTransform, btScalar mass, const btVector3& localInertiaDiagonal, const btTransform& localInertialFrame)
{
m_urdfLink2physxLink[urdfLinkIndex] = body;
m_urdfLinkLocalInertialFrames[urdfLinkIndex] = localInertialFrame;
}
class physx::PxArticulationLink* getPhysxLinkFromLink(int urdfLinkIndex)
{
return m_urdfLink2physxLink[urdfLinkIndex];
}
void registerRigidBody(int urdfLinkIndex, class physx::PxArticulationLink* body, const btTransform& worldTransform, btScalar mass, const btVector3& localInertiaDiagonal, const class btCollisionShape* compound, const btTransform& localInertialFrame)
{
btAssert(m_urdfLink2physxLink[urdfLinkIndex] == 0);
m_urdfLink2physxLink[urdfLinkIndex] = body;
m_urdfLinkLocalInertialFrames[urdfLinkIndex] = localInertialFrame;
}
};
void ComputeParentIndices(const URDFImporterInterface& u2b, URDF2PhysXCachedData& cache, int urdfLinkIndex, int urdfParentIndex)
{
cache.m_urdfLinkParentIndices[urdfLinkIndex] = urdfParentIndex;
cache.m_urdfLinkIndices2BulletLinkIndices[urdfLinkIndex] = cache.m_currentMultiBodyLinkIndex++;
btAlignedObjectArray<int> childIndices;
u2b.getLinkChildIndices(urdfLinkIndex, childIndices);
for (int i = 0; i < childIndices.size(); i++)
{
ComputeParentIndices(u2b, cache, childIndices[i], urdfLinkIndex);
}
}
void ComputeTotalNumberOfJoints(const URDFImporterInterface& u2b, URDF2PhysXCachedData& cache, int linkIndex)
{
btAlignedObjectArray<int> childIndices;
u2b.getLinkChildIndices(linkIndex, childIndices);
cache.m_totalNumJoints1 += childIndices.size();
for (int i = 0; i < childIndices.size(); i++)
{
int childIndex = childIndices[i];
ComputeTotalNumberOfJoints(u2b, cache, childIndex);
}
}
void InitURDF2BulletCache(const URDFImporterInterface& u2b, URDF2PhysXCachedData& cache, int flags)
{
//compute the number of links, and compute parent indices array (and possibly other cached data?)
cache.m_totalNumJoints1 = 0;
int rootLinkIndex = u2b.getRootLinkIndex();
if (rootLinkIndex >= 0)
{
ComputeTotalNumberOfJoints(u2b, cache, rootLinkIndex);
int numTotalLinksIncludingBase = 1 + cache.m_totalNumJoints1;
cache.m_urdfLinkParentIndices.resize(numTotalLinksIncludingBase);
cache.m_urdfLinkIndices2BulletLinkIndices.resize(numTotalLinksIncludingBase);
cache.m_urdfLink2physxLink.resize(numTotalLinksIncludingBase);
cache.m_urdfLinkLocalInertialFrames.resize(numTotalLinksIncludingBase);
cache.m_currentMultiBodyLinkIndex = -1; //multi body base has 'link' index -1
bool maintainLinkOrder = (flags & CUF_MAINTAIN_LINK_ORDER) != 0;
if (maintainLinkOrder)
{
URDF2PhysXCachedData cache2 = cache;
ComputeParentIndices(u2b, cache2, rootLinkIndex, -2);
for (int j = 0; j<numTotalLinksIncludingBase; j++)
{
cache.m_urdfLinkParentIndices[j] = cache2.m_urdfLinkParentIndices[j];
cache.m_urdfLinkIndices2BulletLinkIndices[j] = j - 1;
}
}
else
{
ComputeParentIndices(u2b, cache, rootLinkIndex, -2);
}
}
}
struct ArticulationCreationInterface
{
physx::PxFoundation* m_foundation;
physx::PxPhysics* m_physics;
physx::PxCooking* m_cooking;
physx::PxScene* m_scene;
CommonFileIOInterface* m_fileIO;
b3AlignedObjectArray<int> m_mb2urdfLink;
void addLinkMapping(int urdfLinkIndex, int mbLinkIndex)
{
if (m_mb2urdfLink.size() < (mbLinkIndex + 1))
{
m_mb2urdfLink.resize((mbLinkIndex + 1), -2);
}
// m_urdf2mbLink[urdfLinkIndex] = mbLinkIndex;
m_mb2urdfLink[mbLinkIndex] = urdfLinkIndex;
}
};
static btVector4 colors4[4] =
{
btVector4(1, 0, 0, 1),
btVector4(0, 1, 0, 1),
btVector4(0, 1, 1, 1),
btVector4(1, 1, 0, 1),
};
static btVector4 selectColor4()
{
static btSpinMutex sMutex;
sMutex.lock();
static int curColor = 0;
btVector4 color = colors4[curColor];
curColor++;
curColor &= 3;
sMutex.unlock();
return color;
}
static physx::PxConvexMesh* createPhysXConvex(physx::PxU32 numVerts, const physx::PxVec3* verts, ArticulationCreationInterface& creation)
{
physx::PxCookingParams params = creation.m_cooking->getParams();
// Use the new (default) PxConvexMeshCookingType::eQUICKHULL
params.convexMeshCookingType = physx::PxConvexMeshCookingType::eQUICKHULL;
// If the gaussMapLimit is chosen higher than the number of output vertices, no gauss map is added to the convex mesh data (here 256).
// If the gaussMapLimit is chosen lower than the number of output vertices, a gauss map is added to the convex mesh data (here 16).
int gaussMapLimit = 256;
params.gaussMapLimit = gaussMapLimit;
creation.m_cooking->setParams(params);
// Setup the convex mesh descriptor
physx::PxConvexMeshDesc desc;
// We provide points only, therefore the PxConvexFlag::eCOMPUTE_CONVEX flag must be specified
desc.points.data = verts;
desc.points.count = numVerts;
desc.points.stride = sizeof(physx::PxVec3);
desc.flags = physx::PxConvexFlag::eCOMPUTE_CONVEX;
physx::PxU32 meshSize = 0;
// Directly insert mesh into PhysX
physx::PxConvexMesh* convex = creation.m_cooking->createConvexMesh(desc, creation.m_physics->getPhysicsInsertionCallback());
PX_ASSERT(convex);
return convex;
}
int convertLinkPhysXShapes(const URDFImporterInterface& u2b, URDF2PhysXCachedData& cache, ArticulationCreationInterface& creation, int urdfLinkIndex, const char* pathPrefix, const btTransform& localInertiaFrame,
physx::PxArticulationReducedCoordinate* articulation, int mbLinkIndex, physx::PxArticulationLink* linkPtr)
{
int numShapes = 0;
URDFLinkContactInfo contactInfo;
u2b.getLinkContactInfo(urdfLinkIndex, contactInfo);
//static friction, dynamic frictoin, restitution
cache.m_linkMaterials.push_back(physx::PxVec3(contactInfo.m_lateralFriction, contactInfo.m_lateralFriction, contactInfo.m_restitution));
physx::PxMaterial* material = creation.m_physics->createMaterial(contactInfo.m_lateralFriction, contactInfo.m_lateralFriction, contactInfo.m_restitution);
const UrdfLink* link = u2b.getUrdfLink(urdfLinkIndex);//.convertLinkCollisionShapes m_data->m_urdfParser.getModel().m_links.getAtIndex(linkIndex);
btAssert(linkPtr);
if (linkPtr)
{
for (int v = 0; v < link->m_collisionArray.size(); v++)
{
const UrdfCollision& col = link->m_collisionArray[v];
const UrdfCollision* collision = &col;
btTransform childTrans = col.m_linkLocalFrame;
btTransform localTrans = localInertiaFrame.inverse()*childTrans;
physx::PxTransform tr;
tr.p = physx::PxVec3(localTrans.getOrigin()[0], localTrans.getOrigin()[1], localTrans.getOrigin()[2]);
tr.q = physx::PxQuat(localTrans.getRotation()[0], localTrans.getRotation()[1], localTrans.getRotation()[2], localTrans.getRotation()[3]);
physx::PxShape* shape = 0;
cache.m_geomTypes.push_back(col.m_geometry.m_type);
switch (col.m_geometry.m_type)
{
case URDF_GEOM_PLANE:
{
btVector3 planeNormal = col.m_geometry.m_planeNormal;
btScalar planeConstant = 0; //not available?
//PxPlane(1, 0, 0, 0).
shape = physx::PxRigidActorExt::createExclusiveShape(*linkPtr, physx::PxPlaneGeometry(), *material);
//todo: compensate for plane axis
//physx::PxTransform localPose = tr*physx::PxTransform
//shape->setLocalPose(localPose);
numShapes++;
break;
}
case URDF_GEOM_CAPSULE:
{
btScalar radius = collision->m_geometry.m_capsuleRadius;
btScalar height = collision->m_geometry.m_capsuleHeight;
shape = physx::PxRigidActorExt::createExclusiveShape(*linkPtr, physx::PxCapsuleGeometry(radius, 0.5*height), *material);
btTransform childTrans = col.m_linkLocalFrame;
btTransform x2z;
x2z.setIdentity();
x2z.setRotation(btQuaternion(btVector3(0, 1, 0), SIMD_HALF_PI));
btTransform localTrans = localInertiaFrame.inverse()*childTrans*x2z;
physx::PxTransform tr;
tr.p = physx::PxVec3(localTrans.getOrigin()[0], localTrans.getOrigin()[1], localTrans.getOrigin()[2]);
tr.q = physx::PxQuat(localTrans.getRotation()[0], localTrans.getRotation()[1], localTrans.getRotation()[2], localTrans.getRotation()[3]);
shape->setLocalPose(tr);
cache.m_geomLocalPoses.push_back(tr);
numShapes++;
//btCapsuleShapeZ* capsuleShape = new btCapsuleShapeZ(radius, height);
//shape = capsuleShape;
//shape->setMargin(gUrdfDefaultCollisionMargin);
break;
}
case URDF_GEOM_CYLINDER:
{
btScalar cylRadius = collision->m_geometry.m_capsuleRadius;
btScalar cylHalfLength = 0.5 * collision->m_geometry.m_capsuleHeight;
cache.m_geomDimensions.push_back(physx::PxVec3(cylRadius, cylHalfLength,0));
//if (m_data->m_flags & CUF_USE_IMPLICIT_CYLINDER)
//{
// btVector3 halfExtents(cylRadius, cylRadius, cylHalfLength);
// btCylinderShapeZ* cylZShape = new btCylinderShapeZ(halfExtents);
// shape = cylZShape;
//}
//else
{
btAlignedObjectArray<physx::PxVec3> vertices;
int numSteps = 32;
for (int i = 0; i < numSteps; i++)
{
physx::PxVec3 vert(cylRadius * btSin(SIMD_2_PI * (float(i) / numSteps)), cylRadius * btCos(SIMD_2_PI * (float(i) / numSteps)), cylHalfLength);
vertices.push_back(vert);
vert[2] = -cylHalfLength;
vertices.push_back(vert);
}
physx::PxConvexMesh* convexMesh = createPhysXConvex(vertices.size(), &vertices[0], creation);
shape = physx::PxRigidActorExt::createExclusiveShape(*linkPtr,
physx::PxConvexMeshGeometry(convexMesh), *material);
shape->setLocalPose(tr);
cache.m_geomLocalPoses.push_back(tr);
numShapes++;
}
break;
}
case URDF_GEOM_BOX:
{
btVector3 extents = collision->m_geometry.m_boxSize;
shape = physx::PxRigidActorExt::createExclusiveShape(*linkPtr, physx::PxBoxGeometry(extents[0] * 0.5, extents[1] * 0.5, extents[2] * 0.5), *material);
cache.m_geomDimensions.push_back(physx::PxVec3(extents[0]*0.5, extents[1] * 0.5, extents[2] * 0.5));
shape->setLocalPose(tr);
cache.m_geomLocalPoses.push_back(tr);
numShapes++;
break;
}
case URDF_GEOM_SPHERE:
{
btScalar radius = collision->m_geometry.m_sphereRadius;
shape = physx::PxRigidActorExt::createExclusiveShape(*linkPtr, physx::PxSphereGeometry(radius), *material);
cache.m_geomDimensions.push_back(physx::PxVec3(radius,0,0));
shape->setLocalPose(tr);
cache.m_geomLocalPoses.push_back(tr);
numShapes++;
break;
}
default:
{
}
}
if (shape)
{
//see https://github.com/NVIDIAGameWorks/PhysX/issues/21
physx::PxReal contactOffset = shape->getContactOffset();
physx::PxReal restOffset = shape->getContactOffset();
//shape->setContactOffset(physx::PxReal(.03));
//shape->setRestOffset(physx::PxReal(.01)); //
}
}
}
return numShapes;
}
btTransform ConvertURDF2PhysXInternal(
const PhysXURDFImporter& u2b,
ArticulationCreationInterface& creation,
URDF2PhysXCachedData& cache, int urdfLinkIndex,
const btTransform& parentTransformInWorldSpace,
bool createMultiBody, const char* pathPrefix,
int flags,
UrdfVisualShapeCache2* cachedLinkGraphicsShapesIn,
UrdfVisualShapeCache2* cachedLinkGraphicsShapesOut,
bool recursive)
{
B3_PROFILE("ConvertURDF2PhysXInternal");
//b3Printf("start converting/extracting data from URDF interface\n");
btTransform linkTransformInWorldSpace;
linkTransformInWorldSpace.setIdentity();
int mbLinkIndex = cache.getMbIndexFromUrdfIndex(urdfLinkIndex);
int urdfParentIndex = cache.getParentUrdfIndex(urdfLinkIndex);
int mbParentIndex = cache.getMbIndexFromUrdfIndex(urdfParentIndex);
//b3Printf("mb link index = %d\n",mbLinkIndex);
btTransform parentLocalInertialFrame;
parentLocalInertialFrame.setIdentity();
btScalar parentMass(1);
btVector3 parentLocalInertiaDiagonal(1, 1, 1);
if (urdfParentIndex == -2)
{
//b3Printf("root link has no parent\n");
}
else
{
//b3Printf("urdf parent index = %d\n",urdfParentIndex);
//b3Printf("mb parent index = %d\n",mbParentIndex);
//parentRigidBody = cache.getRigidBodyFromLink(urdfParentIndex);
u2b.getMassAndInertia2(urdfParentIndex, parentMass, parentLocalInertiaDiagonal, parentLocalInertialFrame, flags);
}
btScalar mass = 0;
btTransform localInertialFrame;
localInertialFrame.setIdentity();
btVector3 localInertiaDiagonal(0, 0, 0);
u2b.getMassAndInertia2(urdfLinkIndex, mass, localInertiaDiagonal, localInertialFrame, flags);
btTransform parent2joint;
parent2joint.setIdentity();
int jointType;
btVector3 jointAxisInJointSpace;
btScalar jointLowerLimit;
btScalar jointUpperLimit;
btScalar jointDamping;
btScalar jointFriction;
btScalar jointMaxForce;
btScalar jointMaxVelocity;
bool hasParentJoint = u2b.getJointInfo2(urdfLinkIndex, parent2joint, linkTransformInWorldSpace, jointAxisInJointSpace, jointType, jointLowerLimit, jointUpperLimit, jointDamping, jointFriction, jointMaxForce, jointMaxVelocity);
btTransform axis2Reference;
axis2Reference.setIdentity();
switch (jointType)
{
case URDFContinuousJoint:
case URDFPrismaticJoint:
case URDFRevoluteJoint:
{
//rotate from revolute 'axis' to standard X axis
btVector3 refAxis(1, 0, 0);
btVector3 axis = jointAxisInJointSpace;
//btQuaternion axis2ReferenceRot(btVector3(0, 1, 0), SIMD_HALF_PI);// = shortestArcQuat(refAxis, btVector3(axis[0], axis[1], axis[2]));
btQuaternion axis2ReferenceRot = shortestArcQuat(refAxis, btVector3(axis[0], axis[1], axis[2]));
axis2Reference.setRotation(axis2ReferenceRot);
break;
}
default:
{
}
};
parent2joint = parent2joint*axis2Reference;
//localInertialFrame = axis2Reference.inverse()*localInertialFrame;
std::string linkName = u2b.getLinkName(urdfLinkIndex);
if (flags & CUF_USE_SDF)
{
parent2joint = parentTransformInWorldSpace.inverse() * linkTransformInWorldSpace;
}
else
{
if (flags & CUF_USE_MJCF)
{
linkTransformInWorldSpace = parentTransformInWorldSpace * linkTransformInWorldSpace;
}
else
{
linkTransformInWorldSpace = parentTransformInWorldSpace * parent2joint;
}
}
int graphicsIndex;
{
B3_PROFILE("convertLinkVisualShapes");
graphicsIndex = u2b.convertLinkVisualShapes3(urdfLinkIndex, pathPrefix, localInertialFrame, u2b.getUrdfLink(urdfLinkIndex), u2b.getUrdfModel(), -1, u2b.getBodyUniqueId(), creation.m_fileIO);
#if 0
if (cachedLinkGraphicsShapesIn && cachedLinkGraphicsShapesIn->m_cachedUrdfLinkVisualShapeIndices.size() > (mbLinkIndex + 1))
{
graphicsIndex = cachedLinkGraphicsShapesIn->m_cachedUrdfLinkVisualShapeIndices[mbLinkIndex + 1];
UrdfMaterialColor matColor = cachedLinkGraphicsShapesIn->m_cachedUrdfLinkColors[mbLinkIndex + 1];
u2b.setLinkColor2(urdfLinkIndex, matColor);
}
else
{
graphicsIndex =
if (cachedLinkGraphicsShapesOut)
{
cachedLinkGraphicsShapesOut->m_cachedUrdfLinkVisualShapeIndices.push_back(graphicsIndex);
UrdfMaterialColor matColor;
u2b.getLinkColor2(urdfLinkIndex, matColor);
cachedLinkGraphicsShapesOut->m_cachedUrdfLinkColors.push_back(matColor);
}
}
#endif
}
if (1)
{
UrdfMaterialColor matColor;
btVector4 color2 = selectColor4();
btVector3 specular(0.5, 0.5, 0.5);
if (u2b.getLinkColor2(urdfLinkIndex, matColor))
{
color2 = matColor.m_rgbaColor;
specular = matColor.m_specularColor;
}
/*
if (visual->material.get())
{
color.setValue(visual->material->color.r,visual->material->color.g,visual->material->color.b);//,visual->material->color.a);
}
*/
if (mass)
{
if (!(flags & CUF_USE_URDF_INERTIA))
{
//b3Assert(0);
//compoundShape->calculateLocalInertia(mass, localInertiaDiagonal);
btAssert(localInertiaDiagonal[0] < 1e10);
btAssert(localInertiaDiagonal[1] < 1e10);
btAssert(localInertiaDiagonal[2] < 1e10);
}
URDFLinkContactInfo contactInfo;
u2b.getLinkContactInfo(urdfLinkIndex, contactInfo);
//temporary inertia scaling until we load inertia from URDF
if (contactInfo.m_flags & URDF_CONTACT_HAS_INERTIA_SCALING)
{
localInertiaDiagonal *= contactInfo.m_inertiaScaling;
}
}
btTransform inertialFrameInWorldSpace = linkTransformInWorldSpace * localInertialFrame;
bool canSleep = (flags & CUF_ENABLE_SLEEPING) != 0;
physx::PxArticulationLink* linkPtr = 0;
if (!createMultiBody)
{
}
else
{
physx::PxTransform tr;
tr.p = physx::PxVec3(linkTransformInWorldSpace.getOrigin().x(), linkTransformInWorldSpace.getOrigin().y(), linkTransformInWorldSpace.getOrigin().z());
tr.q = physx::PxQuat(linkTransformInWorldSpace.getRotation().x(), linkTransformInWorldSpace.getRotation().y(), linkTransformInWorldSpace.getRotation().z(), linkTransformInWorldSpace.getRotation().w());
cache.m_linkTransWorldSpace.push_back(tr);
cache.m_urdfLinkIndex.push_back(urdfLinkIndex);
cache.m_parentUrdfLinkIndex.push_back(urdfParentIndex);
cache.m_linkMasses.push_back(mass);
if (cache.m_articulation == 0)
{
bool isFixedBase = (mass == 0); //todo: figure out when base is fixed
cache.m_articulation = creation.m_physics->createArticulationReducedCoordinate();
if (isFixedBase)
{
cache.m_articulation->setArticulationFlags(physx::PxArticulationFlag::eFIX_BASE);
}
physx::PxArticulationLink* base = cache.m_articulation->createLink(NULL,tr);
linkPtr = base;
//physx::PxRigidActorExt::createExclusiveShape(*base, PxBoxGeometry(0.5f, 0.25f, 1.5f), *gMaterial);
physx::PxRigidBody& body = *base;
//Now create the slider and fixed joints...
cache.m_articulation->setSolverIterationCounts(32);//todo: API?
cache.m_jointTypes.push_back(physx::PxArticulationJointType::eUNDEFINED);
cache.m_parentLocalPoses.push_back(physx::PxTransform());
cache.m_childLocalPoses.push_back(physx::PxTransform());
// Stabilization can create artefacts on jointed objects so we just disable it
//cache.m_articulation->setStabilizationThreshold(0.0f);
//cache.m_articulation->setMaxProjectionIterations(16);
//cache.m_articulation->setSeparationTolerance(0.001f);
#if 0
int totalNumJoints = cache.m_totalNumJoints1;
cache.m_bulletMultiBody = creation.allocateMultiBody(urdfLinkIndex, totalNumJoints, mass, localInertiaDiagonal, isFixedBase, canSleep);
if (flags & CUF_GLOBAL_VELOCITIES_MB)
{
cache.m_bulletMultiBody->useGlobalVelocities(true);
}
if (flags & CUF_USE_MJCF)
{
cache.m_bulletMultiBody->setBaseWorldTransform(linkTransformInWorldSpace);
}
#endif
}
else
{
physx::PxArticulationLink* parentLink = cache.getPhysxLinkFromLink(urdfParentIndex);
linkPtr = cache.m_articulation->createLink(parentLink, tr);
physx::PxArticulationJointReducedCoordinate* joint = static_cast<physx::PxArticulationJointReducedCoordinate*>(linkPtr->getInboundJoint());
switch (jointType)
{
case URDFFixedJoint:
{
joint->setJointType(physx::PxArticulationJointType::eFIX);
break;
}
case URDFSphericalJoint:
{
joint->setJointType(physx::PxArticulationJointType::eSPHERICAL);
joint->setMotion(physx::PxArticulationAxis::eTWIST, physx::PxArticulationMotion::eFREE);
joint->setMotion(physx::PxArticulationAxis::eSWING2, physx::PxArticulationMotion::eFREE);
joint->setMotion(physx::PxArticulationAxis::eSWING1, physx::PxArticulationMotion::eFREE);
break;
}
case URDFContinuousJoint:
case URDFRevoluteJoint:
{
joint->setJointType(physx::PxArticulationJointType::eREVOLUTE);
joint->setMotion(physx::PxArticulationAxis::eTWIST, physx::PxArticulationMotion::eFREE);
break;
}
case URDFPrismaticJoint:
{
joint->setJointType(physx::PxArticulationJointType::ePRISMATIC);
joint->setMotion(physx::PxArticulationAxis::eX, physx::PxArticulationMotion::eFREE);
break;
}
default:
{
joint->setJointType(physx::PxArticulationJointType::eFIX);
btAssert(0);
}
};
cache.m_jointTypes.push_back(joint->getJointType());
btTransform offsetInA, offsetInB;
offsetInA = parentLocalInertialFrame.inverse() * parent2joint;
offsetInB = (axis2Reference.inverse()*localInertialFrame).inverse();
physx::PxTransform parentPose(physx::PxVec3(offsetInA.getOrigin()[0], offsetInA.getOrigin()[1], offsetInA.getOrigin()[2]),
physx::PxQuat(offsetInA.getRotation()[0], offsetInA.getRotation()[1], offsetInA.getRotation()[2], offsetInA.getRotation()[3]));
physx::PxTransform childPose(physx::PxVec3(offsetInB.getOrigin()[0], offsetInB.getOrigin()[1], offsetInB.getOrigin()[2]),
physx::PxQuat(offsetInB.getRotation()[0], offsetInB.getRotation()[1], offsetInB.getRotation()[2], offsetInB.getRotation()[3]));
cache.m_parentLocalPoses.push_back(parentPose);
cache.m_childLocalPoses.push_back(childPose);
joint->setParentPose(parentPose);
joint->setChildPose(childPose);
}
cache.registerMultiBody(urdfLinkIndex, linkPtr, inertialFrameInWorldSpace, mass, localInertiaDiagonal, localInertialFrame);
if (linkPtr)
{
//todo: mem leaks
MyPhysXUserData* userData = new MyPhysXUserData();
userData->m_graphicsUniqueId = graphicsIndex;
linkPtr->userData = userData;
}
}
//create collision shapes
//physx::PxRigidActorExt::createExclusiveShape
convertLinkPhysXShapes(u2b, cache, creation, urdfLinkIndex, pathPrefix, localInertialFrame, cache.m_articulation, mbLinkIndex, linkPtr);
physx::PxRigidBodyExt::updateMassAndInertia(*linkPtr, mass);
//base->setMass(massOut);
//base->setMassSpaceInertiaTensor(diagTensor);
//base->setCMassLocalPose(PxTransform(com, orient));
//create a joint if necessary
if (hasParentJoint)
{
btTransform offsetInA, offsetInB;
offsetInA = parentLocalInertialFrame.inverse() * parent2joint;
offsetInB = localInertialFrame.inverse();
btQuaternion parentRotToThis = offsetInB.getRotation() * offsetInA.inverse().getRotation();
bool disableParentCollision = true;
if (createMultiBody && cache.m_articulation)
{
#if 0
cache.m_bulletMultiBody->getLink(mbLinkIndex).m_jointDamping = jointDamping;
cache.m_bulletMultiBody->getLink(mbLinkIndex).m_jointFriction = jointFriction;
cache.m_bulletMultiBody->getLink(mbLinkIndex).m_jointLowerLimit = jointLowerLimit;
cache.m_bulletMultiBody->getLink(mbLinkIndex).m_jointUpperLimit = jointUpperLimit;
cache.m_bulletMultiBody->getLink(mbLinkIndex).m_jointMaxForce = jointMaxForce;
cache.m_bulletMultiBody->getLink(mbLinkIndex).m_jointMaxVelocity = jointMaxVelocity;
#endif
}
}
if (createMultiBody)
{
}
else
{
}
}//was if 'shape/compountShape'
btAlignedObjectArray<int> urdfChildIndices;
u2b.getLinkChildIndices(urdfLinkIndex, urdfChildIndices);
int numChildren = urdfChildIndices.size();
if (recursive)
{
for (int i = 0; i < numChildren; i++)
{
int urdfChildLinkIndex = urdfChildIndices[i];
ConvertURDF2PhysXInternal(u2b, creation, cache, urdfChildLinkIndex, linkTransformInWorldSpace, createMultiBody, pathPrefix, flags, cachedLinkGraphicsShapesIn, cachedLinkGraphicsShapesOut, recursive);
}
}
return linkTransformInWorldSpace;
}
physx::PxArticulationReducedCoordinate* URDF2PhysX(physx::PxFoundation* foundation, physx::PxPhysics* physics, physx::PxCooking* cooking, physx::PxScene* scene, class PhysXURDFImporter& u2p, int flags, const char* pathPrefix, const btTransform& rootTransformInWorldSpace, struct CommonFileIOInterface* fileIO)
{
URDF2PhysXCachedData cache;
InitURDF2BulletCache(u2p, cache, flags);
int urdfLinkIndex = u2p.getRootLinkIndex();
int rootIndex = u2p.getRootLinkIndex();
B3_PROFILE("ConvertURDF2Bullet");
UrdfVisualShapeCache2 cachedLinkGraphicsShapesOut;
UrdfVisualShapeCache2 cachedLinkGraphicsShapes;
ArticulationCreationInterface creation;
creation.m_foundation = foundation;
creation.m_physics = physics;
creation.m_cooking = cooking;
creation.m_scene = scene;
creation.m_fileIO = fileIO;
bool createMultiBody = true;
bool recursive = (flags & CUF_MAINTAIN_LINK_ORDER) == 0;
if (recursive)
{
ConvertURDF2PhysXInternal(u2p, creation, cache, urdfLinkIndex, rootTransformInWorldSpace, createMultiBody, pathPrefix, flags, &cachedLinkGraphicsShapes, &cachedLinkGraphicsShapesOut, recursive);
}
else
{
#if 0
btAlignedObjectArray<btTransform> parentTransforms;
if (urdfLinkIndex >= parentTransforms.size())
{
parentTransforms.resize(urdfLinkIndex + 1);
}
parentTransforms[urdfLinkIndex] = rootTransformInWorldSpace;
btAlignedObjectArray<childParentIndex> allIndices;
GetAllIndices(u2b, cache, urdfLinkIndex, -1, allIndices);
allIndices.quickSort(MyIntCompareFunc);
for (int i = 0; i < allIndices.size(); i++)
{
int urdfLinkIndex = allIndices[i].m_index;
int parentIndex = allIndices[i].m_parentIndex;
btTransform parentTr = parentIndex >= 0 ? parentTransforms[parentIndex] : rootTransformInWorldSpace;
btTransform tr = ConvertURDF2BulletInternal(u2b, creation, cache, urdfLinkIndex, parentTr, world1, createMultiBody, pathPrefix, flags, cachedLinkGraphicsShapes, &cachedLinkGraphicsShapesOut, recursive);
if ((urdfLinkIndex + 1) >= parentTransforms.size())
{
parentTransforms.resize(urdfLinkIndex + 1);
}
parentTransforms[urdfLinkIndex] = tr;
}
#endif
}
#if 0
if (cachedLinkGraphicsShapes && cachedLinkGraphicsShapesOut.m_cachedUrdfLinkVisualShapeIndices.size() > cachedLinkGraphicsShapes->m_cachedUrdfLinkVisualShapeIndices.size())
{
*cachedLinkGraphicsShapes = cachedLinkGraphicsShapesOut;
}
#endif
if (scene && cache.m_articulation)
{
#ifdef DEBUG_ARTICULATIONS
printf("\n-----------------\n");
printf("m_linkTransWorldSpace\n");
for (int i = 0; i < cache.m_linkTransWorldSpace.size(); i++)
{
printf("PxTransform(PxVec3(%f,%f,%f), PxQuat(%f,%f,%f,%f),\n",
cache.m_linkTransWorldSpace[i].p.x, cache.m_linkTransWorldSpace[i].p.y, cache.m_linkTransWorldSpace[i].p.z,
cache.m_linkTransWorldSpace[i].q.x, cache.m_linkTransWorldSpace[i].q.y, cache.m_linkTransWorldSpace[i].q.z, cache.m_linkTransWorldSpace[i].q.w);
}
printf("m_parentLocalPoses\n");
for (int i = 0; i < cache.m_parentLocalPoses.size(); i++)
{
printf("PxTransform(PxVec3(%f,%f,%f), PxQuat(%f,%f,%f,%f),\n",
cache.m_parentLocalPoses[i].p.x, cache.m_parentLocalPoses[i].p.y, cache.m_parentLocalPoses[i].p.z,
cache.m_parentLocalPoses[i].q.x, cache.m_parentLocalPoses[i].q.y, cache.m_parentLocalPoses[i].q.z, cache.m_parentLocalPoses[i].q.w);
}
printf("m_childLocalPoses\n");
for (int i = 0; i < cache.m_childLocalPoses.size(); i++)
{
printf("PxTransform(PxVec3(%f,%f,%f), PxQuat(%f,%f,%f,%f),\n",
cache.m_childLocalPoses[i].p.x, cache.m_childLocalPoses[i].p.y, cache.m_childLocalPoses[i].p.z,
cache.m_childLocalPoses[i].q.x, cache.m_childLocalPoses[i].q.y, cache.m_childLocalPoses[i].q.z, cache.m_childLocalPoses[i].q.w);
}
printf("m_geomDimensions\n");
for (int i = 0; i < cache.m_geomDimensions.size(); i++)
{
printf("PxVec3(%f,%f,%f),\n",
cache.m_geomDimensions[i].x, cache.m_geomDimensions[i].y, cache.m_geomDimensions[i].z);
}
printf("m_geomLocalPoses\n");
for (int i = 0; i < cache.m_geomLocalPoses.size(); i++)
{
printf("PxTransform(PxVec3(%f,%f,%f), PxQuat(%f,%f,%f,%f),\n",
cache.m_geomLocalPoses[i].p.x, cache.m_geomLocalPoses[i].p.y, cache.m_geomLocalPoses[i].p.z,
cache.m_geomLocalPoses[i].q.x, cache.m_geomLocalPoses[i].q.y, cache.m_geomLocalPoses[i].q.z, cache.m_geomLocalPoses[i].q.w);
}
printf("m_linkMaterials\n");
for (int i = 0; i < cache.m_linkMaterials.size(); i++)
{
printf("PxVec3(%f,%f,%f),\n",
cache.m_linkMaterials[i].x, cache.m_linkMaterials[i].y, cache.m_linkMaterials[i].z);
}
#endif //DEBUG_ARTICULATIONS
//see also https://github.com/NVIDIAGameWorks/PhysX/issues/43
if ((flags & CUF_USE_SELF_COLLISION) == 0)
{
physx::PxU32 nbActors = cache.m_articulation->getNbLinks();; // Max number of actors expected in the aggregate
bool selfCollisions = false;
physx::PxAggregate* aggregate = physics->createAggregate(nbActors, selfCollisions);
aggregate->addArticulation(*cache.m_articulation);
scene->addAggregate(*aggregate);
}
else
{
scene->addArticulation(*cache.m_articulation);
}
}
return cache.m_articulation;
}

25
examples/SharedMemory/physx/URDF2PhysX.h Normal file
View File

@@ -0,0 +1,25 @@
#ifndef URDF2PHYSX_H
#define URDF2PHYSX_H
#include "Bullet3Common/b3AlignedObjectArray.h"
#include "Importers/ImportURDFDemo/URDFJointTypes.h"
namespace physx
{
class PxFoundation;
class PxPhysics;
class PxDefaultCpuDispatcher;
class PxScene;
class PxCooking;
class PxArticulationReducedCoordinate;
};
struct UrdfVisualShapeCache2
{
b3AlignedObjectArray<UrdfMaterialColor> m_cachedUrdfLinkColors;
b3AlignedObjectArray<int> m_cachedUrdfLinkVisualShapeIndices;
};
physx::PxArticulationReducedCoordinate* URDF2PhysX(physx::PxFoundation* foundation, physx::PxPhysics* physics, physx::PxCooking* cooking, physx::PxScene* scene, class PhysXURDFImporter& u2p, int flags, const char* pathPrefix, const class btTransform& rootTransformInWorldSpace,struct CommonFileIOInterface* fileIO);
#endif //URDF2PHYSX_H

407
examples/SharedMemory/plugins/eglPlugin/eglRendererVisualShapeConverter.cpp
View File

@@ -73,9 +73,9 @@ struct MyTexture3
struct EGLRendererObjectArray
{
btAlignedObjectArray<TinyRenderObjectData*> m_renderObjects;
btAlignedObjectArray<int> m_graphicsInstanceIds;
int m_objectUniqueId;
int m_linkIndex;
int m_graphicsInstanceId;
btTransform m_worldTransform;
btVector3 m_localScaling;
@@ -83,12 +83,15 @@ struct EGLRendererObjectArray
{
m_worldTransform.setIdentity();
m_localScaling.setValue(1, 1, 1);
m_graphicsInstanceId = -1;
}
};
#define START_WIDTH 2560
#define START_HEIGHT 2048
//#define START_WIDTH 2560
//#define START_HEIGHT 2048
#define START_WIDTH 1024
#define START_HEIGHT 768
struct EGLRendererVisualShapeConverterInternalData
{
@@ -105,6 +108,7 @@ struct EGLRendererVisualShapeConverterInternalData
btAlignedObjectArray<b3VisualShapeData> m_visualShapes;
int m_upAxis;
int m_swWidth;
int m_swHeight;
@@ -132,12 +136,22 @@ struct EGLRendererVisualShapeConverterInternalData
int m_flags;
SimpleCamera m_camera;
bool m_leftMouseButton;
bool m_middleMouseButton;
bool m_rightMouseButton;
float m_wheelMultiplier;
float m_mouseMoveMultiplier;
float m_mouseXpos;
float m_mouseYpos;
bool m_mouseInitialized;
int m_graphicsUniqueIdGenerator;
EGLRendererVisualShapeConverterInternalData()
: m_upAxis(2),
m_swWidth(START_WIDTH),
m_swHeight(START_HEIGHT),
m_rgbColorBuffer(START_WIDTH, START_HEIGHT, TGAImage::RGB),
m_lightDirection(btVector3(-5, 200, -40)),
m_lightDirection(btVector3(-5, -40, 200 )),
m_hasLightDirection(false),
m_lightColor(btVector3(1.0, 1.0, 1.0)),
m_hasLightColor(false),
@@ -150,7 +164,16 @@ struct EGLRendererVisualShapeConverterInternalData
m_lightSpecularCoeff(0.05),
m_hasLightSpecularCoeff(false),
m_hasShadow(false),
m_flags(0)
m_flags(0),
m_leftMouseButton(false),
m_middleMouseButton(false),
m_rightMouseButton(false),
m_wheelMultiplier(0.01f),
m_mouseMoveMultiplier(0.4f),
m_mouseXpos(0.f),
m_mouseYpos(0.f),
m_mouseInitialized(false),
m_graphicsUniqueIdGenerator(15)
{
m_depthBuffer.resize(m_swWidth * m_swHeight);
m_shadowBuffer.resize(m_swWidth * m_swHeight);
@@ -161,7 +184,7 @@ struct EGLRendererVisualShapeConverterInternalData
m_window = new DefaultOpenGLWindow();
m_window->setAllowRetina(allowRetina);
b3gWindowConstructionInfo ci;
ci.m_title = "Title";
ci.m_title = "PyBullet";
ci.m_width = m_swWidth;
ci.m_height = m_swHeight;
ci.m_renderDevice = 0;
@@ -207,21 +230,234 @@ struct EGLRendererVisualShapeConverterInternalData
}
};
static EGLRendererVisualShapeConverter* gWindow = 0;
static void SimpleResizeCallback(float widthf, float heightf)
{
int width = (int)widthf;
int height = (int)heightf;
if (gWindow && gWindow->m_data->m_instancingRenderer)
{
gWindow->m_data->m_instancingRenderer->resize(width, height);
}
//if (gApp && gApp->m_instancingRenderer)
// gApp->m_instancingRenderer->resize(width, height);
//
//if (gApp && gApp->m_primRenderer)
// gApp->m_primRenderer->setScreenSize(width, height);
}
static void SimpleKeyboardCallback(int key, int state)
{
if (key == B3G_ESCAPE) //&& gApp && gApp->m_window)
{
//gApp->m_window->setRequestExit();
}
else
{
//gApp->defaultKeyboardCallback(key,state);
}
}
static void SimpleMouseButtonCallback(int button, int state, float x, float y)
{
if (gWindow)
{
gWindow->mouseButtonCallback(button, state, x, y);
}
}
static void SimpleMouseMoveCallback(float x, float y)
{
if (gWindow)
{
gWindow->mouseMoveCallback(x, y);
}
}
static void SimpleWheelCallback(float deltax, float deltay)
{
float wheelMultiplier = 0.01f;
if (gWindow && gWindow->m_data->m_instancingRenderer)
{
class GLInstancingRenderer* renderer = gWindow->m_data->m_instancingRenderer;
b3Vector3 cameraTargetPosition, cameraPosition, cameraUp = b3MakeVector3(0, 0, 0);
int upAxis = renderer->getActiveCamera()->getCameraUpAxis();
cameraUp[upAxis] = 1;
CommonCameraInterface* camera = renderer->getActiveCamera();
camera->getCameraPosition(cameraPosition);
camera->getCameraTargetPosition(cameraTargetPosition);
bool m_leftMouseButton = false;
if (!m_leftMouseButton)
{
float cameraDistance = camera->getCameraDistance();
if (deltay < 0 || cameraDistance > 1)
{
cameraDistance -= deltay * 0.01f;
if (cameraDistance < 1)
cameraDistance = 1;
camera->setCameraDistance(cameraDistance);
}
else
{
b3Vector3 fwd = cameraTargetPosition - cameraPosition;
fwd.normalize();
cameraTargetPosition += fwd * deltay * wheelMultiplier; //todo: expose it in the GUI?
}
}
else
{
if (b3Fabs(deltax) > b3Fabs(deltay))
{
b3Vector3 fwd = cameraTargetPosition - cameraPosition;
b3Vector3 side = cameraUp.cross(fwd);
side.normalize();
cameraTargetPosition += side * deltax * wheelMultiplier;
}
else
{
cameraTargetPosition -= cameraUp * deltay * wheelMultiplier;
}
}
camera->setCameraTargetPosition(cameraTargetPosition[0], cameraTargetPosition[1], cameraTargetPosition[2]);
}
}
void defaultMouseButtonCallback(int button, int state, float x, float y)
{
if (gWindow)
{
gWindow->mouseButtonCallback(button, state, x, y);
}
}
void defaultMouseMoveCallback(float x, float y)
{
if (gWindow)
{
gWindow->mouseMoveCallback(x, y);
} //m_window && m_renderer
}
void EGLRendererVisualShapeConverter::mouseButtonCallback(int button, int state, float x, float y)
{
if (button == 0)
m_data->m_leftMouseButton = (state == 1);
if (button == 1)
m_data->m_middleMouseButton = (state == 1);
if (button == 2)
m_data->m_rightMouseButton = (state == 1);
m_data->m_mouseXpos = x;
m_data->m_mouseYpos = y;
m_data->m_mouseInitialized = true;
}
void EGLRendererVisualShapeConverter::mouseMoveCallback(float x, float y)
{
class GLInstancingRenderer* renderer = m_data->m_instancingRenderer;
if (renderer == 0)
return;
CommonCameraInterface* camera = renderer->getActiveCamera();
bool isAltPressed = m_data->m_window->isModifierKeyPressed(B3G_ALT);
bool isControlPressed = m_data->m_window->isModifierKeyPressed(B3G_CONTROL);
if (isAltPressed || isControlPressed)
{
float xDelta = x - m_data->m_mouseXpos;
float yDelta = y - m_data->m_mouseYpos;
float cameraDistance = camera->getCameraDistance();
float pitch = camera->getCameraPitch();
float yaw = camera->getCameraYaw();
float targPos[3];
float camPos[3];
camera->getCameraTargetPosition(targPos);
camera->getCameraPosition(camPos);
b3Vector3 cameraPosition = b3MakeVector3(b3Scalar(camPos[0]),
b3Scalar(camPos[1]),
b3Scalar(camPos[2]));
b3Vector3 cameraTargetPosition = b3MakeVector3(b3Scalar(targPos[0]),
b3Scalar(targPos[1]),
b3Scalar(targPos[2]));
b3Vector3 cameraUp = b3MakeVector3(0, 0, 0);
cameraUp[camera->getCameraUpAxis()] = 1.f;
if (m_data->m_leftMouseButton)
{
// if (b3Fabs(xDelta)>b3Fabs(yDelta))
// {
pitch -= yDelta * m_data->m_mouseMoveMultiplier;
// } else
// {
yaw -= xDelta * m_data->m_mouseMoveMultiplier;
// }
}
if (m_data->m_middleMouseButton)
{
cameraTargetPosition += cameraUp * yDelta * 0.01;
b3Vector3 fwd = cameraTargetPosition - cameraPosition;
b3Vector3 side = cameraUp.cross(fwd);
side.normalize();
cameraTargetPosition += side * xDelta * 0.01;
}
if (m_data->m_rightMouseButton)
{
cameraDistance -= xDelta * 0.01f;
cameraDistance -= yDelta * 0.01f;
if (cameraDistance < 1)
cameraDistance = 1;
if (cameraDistance > 1000)
cameraDistance = 1000;
}
camera->setCameraDistance(cameraDistance);
camera->setCameraPitch(pitch);
camera->setCameraYaw(yaw);
camera->setCameraTargetPosition(cameraTargetPosition[0], cameraTargetPosition[1], cameraTargetPosition[2]);
}
m_data->m_mouseXpos = x;
m_data->m_mouseYpos = y;
m_data->m_mouseInitialized = true;
}
EGLRendererVisualShapeConverter::EGLRendererVisualShapeConverter()
{
m_data = new EGLRendererVisualShapeConverterInternalData();
float dist = 1.5;
float pitch = -10;
float yaw = -80;
float targetPos[3] = {0, 0, 0};
m_data->m_camera.setCameraUpAxis(m_data->m_upAxis);
resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]);
gWindow = this;
m_data->m_window->setResizeCallback(SimpleResizeCallback);
m_data->m_window->setWheelCallback(SimpleWheelCallback);
m_data->m_window->setMouseButtonCallback(SimpleMouseButtonCallback);
m_data->m_window->setMouseMoveCallback(SimpleMouseMoveCallback);
}
EGLRendererVisualShapeConverter::~EGLRendererVisualShapeConverter()
{
gWindow = 0;
resetAll();
delete m_data;
}
void EGLRendererVisualShapeConverter::setLightDirection(float x, float y, float z)
@@ -649,11 +885,15 @@ static btVector4 sColors[4] =
// If you are getting segfaults in this function it may be ecause you are
// compliling the plugin with differently from pybullet, try complining the
// plugin with distutils too.
void EGLRendererVisualShapeConverter::convertVisualShapes(
int EGLRendererVisualShapeConverter::convertVisualShapes(
int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame,
const UrdfLink* linkPtr, const UrdfModel* model,
int collisionObjectUniqueId, int bodyUniqueId, struct CommonFileIOInterface* fileIO)
int orgGraphicsUniqueId, int bodyUniqueId, struct CommonFileIOInterface* fileIO)
{
if (orgGraphicsUniqueId< 0)
{
orgGraphicsUniqueId = m_data->m_graphicsUniqueIdGenerator++;
}
btAssert(linkPtr); // TODO: remove if (not doing it now, because diff will be 50+ lines)
if (linkPtr)
{
@@ -743,12 +983,12 @@ void EGLRendererVisualShapeConverter::convertVisualShapes(
}
}
EGLRendererObjectArray** visualsPtr = m_data->m_swRenderInstances[collisionObjectUniqueId];
EGLRendererObjectArray** visualsPtr = m_data->m_swRenderInstances[orgGraphicsUniqueId];
if (visualsPtr == 0)
{
m_data->m_swRenderInstances.insert(collisionObjectUniqueId, new EGLRendererObjectArray);
m_data->m_swRenderInstances.insert(orgGraphicsUniqueId, new EGLRendererObjectArray);
}
visualsPtr = m_data->m_swRenderInstances[collisionObjectUniqueId];
visualsPtr = m_data->m_swRenderInstances[orgGraphicsUniqueId];
btAssert(visualsPtr);
EGLRendererObjectArray* visuals = *visualsPtr;
@@ -808,13 +1048,14 @@ void EGLRendererVisualShapeConverter::convertVisualShapes(
int shapeIndex = m_data->m_instancingRenderer->registerShape(&vertices[0].xyzw[0], vertices.size(), &indices[0], indices.size(), B3_GL_TRIANGLES, textureIndex);
double scaling[3] = {1, 1, 1};
visuals->m_graphicsInstanceId = m_data->m_instancingRenderer->registerGraphicsInstance(shapeIndex, &visualShape.m_localVisualFrame[0], &visualShape.m_localVisualFrame[3], &visualShape.m_rgbaColor[0], scaling);
int graphicsIndex = m_data->m_instancingRenderer->registerGraphicsInstance(shapeIndex, &visualShape.m_localVisualFrame[0], &visualShape.m_localVisualFrame[3], &visualShape.m_rgbaColor[0], scaling);
int segmentationMask = bodyUniqueId + ((linkIndex + 1) << 24);
{
int graphicsIndex = visuals->m_graphicsInstanceId;
if (graphicsIndex >= 0)
{
visuals->m_graphicsInstanceIds.push_back(graphicsIndex);
if (m_data->m_graphicsIndexToSegmentationMask.size() < (graphicsIndex + 1))
{
m_data->m_graphicsIndexToSegmentationMask.resize(graphicsIndex + 1);
@@ -835,6 +1076,7 @@ void EGLRendererVisualShapeConverter::convertVisualShapes(
}
}
}
return orgGraphicsUniqueId;
}
int EGLRendererVisualShapeConverter::getNumVisualShapes(int bodyUniqueId)
@@ -1032,7 +1274,7 @@ void EGLRendererVisualShapeConverter::copyCameraImageDataGL(
int numRemainingPixels = numTotalPixels - startPixelIndex;
int numBytesPerPixel = 4; //RGBA
int numRequestedPixels = btMin(rgbaBufferSizeInPixels, numRemainingPixels);
if (numRequestedPixels)
if (1)
{
if (startPixelIndex == 0)
{
@@ -1049,9 +1291,13 @@ void EGLRendererVisualShapeConverter::copyCameraImageDataGL(
m_data->m_instancingRenderer->updateCamera(m_data->m_upAxis);
m_data->m_instancingRenderer->renderScene();
m_data->m_instancingRenderer->drawLine(b3MakeVector3(0, 0, 0), b3MakeVector3(1, 0, 0), b3MakeVector3(1, 0, 0), 3);
m_data->m_instancingRenderer->drawLine(b3MakeVector3(0, 0, 0), b3MakeVector3(0, 1, 0), b3MakeVector3(0, 1, 0), 3);
m_data->m_instancingRenderer->drawLine(b3MakeVector3(0, 0, 0), b3MakeVector3(0, 0, 1), b3MakeVector3(0, 0, 1), 3);
int numBytesPerPixel = 4; //RGBA
if (pixelsRGBA || depthBuffer)
{
{
BT_PROFILE("copy pixels");
@@ -1083,40 +1329,40 @@ void EGLRendererVisualShapeConverter::copyCameraImageDataGL(
}
}
}
}
m_data->m_rgbaPixelBuffer1.resize((*widthPtr) * (*heightPtr) * numBytesPerPixel);
m_data->m_depthBuffer1.resize((*widthPtr) * (*heightPtr));
//rescale and flip
{
BT_PROFILE("resize and flip");
for (int j = 0; j < *heightPtr; j++)
m_data->m_rgbaPixelBuffer1.resize((*widthPtr) * (*heightPtr) * numBytesPerPixel);
m_data->m_depthBuffer1.resize((*widthPtr) * (*heightPtr));
//rescale and flip
{
for (int i = 0; i < *widthPtr; i++)
BT_PROFILE("resize and flip");
for (int j = 0; j < *heightPtr; j++)
{
int xIndex = int(float(i) * (float(sourceWidth) / float(*widthPtr)));
int yIndex = int(float(*heightPtr - 1 - j) * (float(sourceHeight) / float(*heightPtr)));
btClamp(xIndex, 0, sourceWidth);
btClamp(yIndex, 0, sourceHeight);
int bytesPerPixel = 4; //RGBA
int sourcePixelIndex = (xIndex + yIndex * sourceWidth) * bytesPerPixel;
int sourceDepthIndex = xIndex + yIndex * sourceWidth;
#define COPY4PIXELS 1
#ifdef COPY4PIXELS
int* dst = (int*)&m_data->m_rgbaPixelBuffer1[(i + j * (*widthPtr)) * 4 + 0];
int* src = (int*)&m_data->m_sourceRgbaPixelBuffer[sourcePixelIndex + 0];
*dst = *src;
#else
m_data->m_rgbaPixelBuffer1[(i + j * widthPtr) * 4 + 0] = sourceRgbaPixelBuffer[sourcePixelIndex + 0];
m_data->m_rgbaPixelBuffer1[(i + j * widthPtr) * 4 + 1] = sourceRgbaPixelBuffer[sourcePixelIndex + 1];
m_data->m_rgbaPixelBuffer1[(i + j * widthPtr) * 4 + 2] = sourceRgbaPixelBuffer[sourcePixelIndex + 2];
m_data->m_rgbaPixelBuffer1[(i + j * widthPtr) * 4 + 3] = 255;
#endif
if (depthBuffer)
for (int i = 0; i < *widthPtr; i++)
{
m_data->m_depthBuffer1[i + j * (*widthPtr)] = m_data->m_sourceDepthBuffer[sourceDepthIndex];
int xIndex = int(float(i) * (float(sourceWidth) / float(*widthPtr)));
int yIndex = int(float(*heightPtr - 1 - j) * (float(sourceHeight) / float(*heightPtr)));
btClamp(xIndex, 0, sourceWidth);
btClamp(yIndex, 0, sourceHeight);
int bytesPerPixel = 4; //RGBA
int sourcePixelIndex = (xIndex + yIndex * sourceWidth) * bytesPerPixel;
int sourceDepthIndex = xIndex + yIndex * sourceWidth;
#define COPY4PIXELS 1
#ifdef COPY4PIXELS
int* dst = (int*)&m_data->m_rgbaPixelBuffer1[(i + j * (*widthPtr)) * 4 + 0];
int* src = (int*)&m_data->m_sourceRgbaPixelBuffer[sourcePixelIndex + 0];
*dst = *src;
#else
m_data->m_rgbaPixelBuffer1[(i + j * widthPtr) * 4 + 0] = sourceRgbaPixelBuffer[sourcePixelIndex + 0];
m_data->m_rgbaPixelBuffer1[(i + j * widthPtr) * 4 + 1] = sourceRgbaPixelBuffer[sourcePixelIndex + 1];
m_data->m_rgbaPixelBuffer1[(i + j * widthPtr) * 4 + 2] = sourceRgbaPixelBuffer[sourcePixelIndex + 2];
m_data->m_rgbaPixelBuffer1[(i + j * widthPtr) * 4 + 3] = 255;
#endif
if (depthBuffer)
{
m_data->m_depthBuffer1[i + j * (*widthPtr)] = m_data->m_sourceDepthBuffer[sourceDepthIndex];
}
}
}
}
@@ -1153,36 +1399,35 @@ void EGLRendererVisualShapeConverter::copyCameraImageDataGL(
}
}
}
}
m_data->m_segmentationMaskBuffer.resize(destinationWidth * destinationHeight, -1);
m_data->m_segmentationMaskBuffer.resize(destinationWidth * destinationHeight, -1);
//rescale and flip
{
BT_PROFILE("resize and flip");
for (int j = 0; j < destinationHeight; j++)
//rescale and flip
{
for (int i = 0; i < destinationWidth; i++)
BT_PROFILE("resize and flip");
for (int j = 0; j < destinationHeight; j++)
{
int xIndex = int(float(i) * (float(sourceWidth) / float(destinationWidth)));
int yIndex = int(float(destinationHeight - 1 - j) * (float(sourceHeight) / float(destinationHeight)));
btClamp(xIndex, 0, sourceWidth);
btClamp(yIndex, 0, sourceHeight);
int bytesPerPixel = 4; //RGBA
int sourcePixelIndex = (xIndex + yIndex * sourceWidth) * bytesPerPixel;
int sourceDepthIndex = xIndex + yIndex * sourceWidth;
if (segmentationMaskBuffer)
for (int i = 0; i < destinationWidth; i++)
{
float depth = m_data->m_segmentationMaskSourceDepthBuffer[sourceDepthIndex];
if (depth < 1)
int xIndex = int(float(i) * (float(sourceWidth) / float(destinationWidth)));
int yIndex = int(float(destinationHeight - 1 - j) * (float(sourceHeight) / float(destinationHeight)));
btClamp(xIndex, 0, sourceWidth);
btClamp(yIndex, 0, sourceHeight);
int bytesPerPixel = 4; //RGBA
int sourcePixelIndex = (xIndex + yIndex * sourceWidth) * bytesPerPixel;
int sourceDepthIndex = xIndex + yIndex * sourceWidth;
if (segmentationMaskBuffer)
{
int segMask = m_data->m_segmentationMaskSourceRgbaPixelBuffer[sourcePixelIndex + 0] + 256 * (m_data->m_segmentationMaskSourceRgbaPixelBuffer[sourcePixelIndex + 1]) + 256 * 256 * (m_data->m_segmentationMaskSourceRgbaPixelBuffer[sourcePixelIndex + 2]);
m_data->m_segmentationMaskBuffer[i + j * destinationWidth] = segMask;
}
else
{
m_data->m_segmentationMaskBuffer[i + j * destinationWidth] = -1;
float depth = m_data->m_segmentationMaskSourceDepthBuffer[sourceDepthIndex];
if (depth < 1)
{
int segMask = m_data->m_segmentationMaskSourceRgbaPixelBuffer[sourcePixelIndex + 0] + 256 * (m_data->m_segmentationMaskSourceRgbaPixelBuffer[sourcePixelIndex + 1]) + 256 * 256 * (m_data->m_segmentationMaskSourceRgbaPixelBuffer[sourcePixelIndex + 2]);
m_data->m_segmentationMaskBuffer[i + j * destinationWidth] = segMask;
}
else
{
m_data->m_segmentationMaskBuffer[i + j * destinationWidth] = -1;
}
}
}
}
@@ -1257,7 +1502,11 @@ void EGLRendererVisualShapeConverter::removeVisualShape(int collisionObjectUniqu
{
for (int o = 0; o < ptr->m_renderObjects.size(); o++)
{
m_data->m_instancingRenderer->removeGraphicsInstance(ptr->m_graphicsInstanceId);
for (int i = 0; i < ptr->m_graphicsInstanceIds.size(); i++)
{
m_data->m_instancingRenderer->removeGraphicsInstance(ptr->m_graphicsInstanceIds[i]);
}
delete ptr->m_renderObjects[o];
}
}
@@ -1387,11 +1636,15 @@ void EGLRendererVisualShapeConverter::syncTransform(int collisionObjectUniqueId,
EGLRendererObjectArray* renderObj = *renderObjPtr;
renderObj->m_worldTransform = worldTransform;
renderObj->m_localScaling = localScaling;
if (renderObj->m_graphicsInstanceId >= 0)
for (int i = 0; i < renderObj->m_graphicsInstanceIds.size(); i++)
{
btVector3 pos = worldTransform.getOrigin();
btQuaternion orn = worldTransform.getRotation();
m_data->m_instancingRenderer->writeSingleInstanceTransformToCPU(pos, orn, renderObj->m_graphicsInstanceId);
int graphicsInstanceId = renderObj->m_graphicsInstanceIds[i];
if (graphicsInstanceId >= 0)
{
btVector3 pos = worldTransform.getOrigin();
btQuaternion orn = worldTransform.getRotation();
m_data->m_instancingRenderer->writeSingleInstanceTransformToCPU(pos, orn, graphicsInstanceId);
}
}
}
}

6
examples/SharedMemory/plugins/eglPlugin/eglRendererVisualShapeConverter.h
View File

@@ -11,7 +11,7 @@ struct EGLRendererVisualShapeConverter : public UrdfRenderingInterface
virtual ~EGLRendererVisualShapeConverter();
virtual void convertVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame, const UrdfLink* linkPtr, const UrdfModel* model, int collisionObjectUniqueId, int bodyUniqueId, struct CommonFileIOInterface* fileIO);
virtual int convertVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame, const UrdfLink* linkPtr, const UrdfModel* model, int collisionObjectUniqueId, int bodyUniqueId, struct CommonFileIOInterface* fileIO);
virtual int getNumVisualShapes(int bodyUniqueId);
@@ -55,6 +55,10 @@ struct EGLRendererVisualShapeConverter : public UrdfRenderingInterface
virtual void setProjectiveTexture(bool useProjectiveTexture);
virtual void syncTransform(int shapeUid, const class btTransform& worldTransform, const class btVector3& localScaling);
virtual void mouseMoveCallback(float x, float y);
virtual void mouseButtonCallback(int button, int state, float x, float y);
};
#endif //EGL_RENDERER_VISUAL_SHAPE_CONVERTER_H

391
examples/SharedMemory/plugins/stablePDPlugin/BulletConversion.cpp Normal file
View File

@@ -0,0 +1,391 @@
#include "btBulletDynamicsCommon.h"
//for inverse dynamics, DeepMimic implementation
#include "RBDModel.h"
#include "RBDUtil.h"
#include "KinTree.h"
//for BulletInverseDynamics
//#include "BulletInverseDynamics/IDConfig.hpp"
//#include "../Extras/InverseDynamics/btMultiBodyTreeCreator.hpp"
//#include "BulletDynamics/MLCPSolvers/btDantzigSolver.h"
//#include "BulletDynamics/MLCPSolvers/btLemkeSolver.h"
//#include "BulletDynamics/MLCPSolvers/btSolveProjectedGaussSeidel.h"
#include "BulletDynamics/Featherstone/btMultiBody.h"
#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h"
#include "BulletDynamics/Featherstone/btMultiBodyMLCPConstraintSolver.h"
#include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h"
#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
#include "BulletDynamics/Featherstone/btMultiBodyLink.h"
#include "BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.h"
#include "BulletDynamics/Featherstone/btMultiBodyJointMotor.h"
#include "BulletDynamics/Featherstone/btMultiBodyPoint2Point.h"
#include "BulletDynamics/Featherstone/btMultiBodyFixedConstraint.h"
#include "BulletDynamics/Featherstone/btMultiBodySliderConstraint.h"
struct TempLink
{
int m_parentIndex;
const btCollisionObject* m_collider;
double m_mass;
int m_jointType;
int m_dofOffset;
int m_dofCount;
btVector3 m_dVector;
btVector3 m_eVector;
btQuaternion m_zeroRotParentToThis;
btQuaternion m_this_to_body1;
};
bool btExtractJointBodyFromTempLinks(btAlignedObjectArray<TempLink>& links, Eigen::MatrixXd& bodyDefs, Eigen::MatrixXd& jointMat)
{
bool result = true;
int num_joints = links.size();
btAlignedObjectArray<btVector3> bodyToLinkPositions;
btAlignedObjectArray<btQuaternion> bodyToLinkRotations;
btAlignedObjectArray<btQuaternion> dVectorRot;
bodyToLinkRotations.resize(num_joints);
bodyToLinkPositions.resize(num_joints);
dVectorRot.resize(num_joints);
jointMat.resize(num_joints, 19);
bodyDefs.resize(num_joints, 17);
for (int i = 0; i < num_joints * 19; i++)
{
jointMat(i) = SIMD_INFINITY;
}
for (int i = 0; i < num_joints * 17; i++)
{
bodyDefs(i) = SIMD_INFINITY;
}
for (int i = 0; i < num_joints * 17; i++)
{
bodyDefs(i) = SIMD_INFINITY;
}
btScalar unk = -12345;
int totalDofs = 0;
for (int j = 0; j < num_joints; ++j)
{
int i = j;
int parentIndex = links[j].m_parentIndex;
cShape::eShape shapeType = cShape::eShapeNull;
double param0 = 0, param1 = 0, param2 = 0;
if (links[j].m_collider)
{
const btCollisionShape* collisionShape = links[j].m_collider->getCollisionShape();
if (collisionShape->isCompound())
{
const btCompoundShape* compound = (const btCompoundShape*)collisionShape;
if (compound->getNumChildShapes() > 0)
{
collisionShape = compound->getChildShape(0);
}
}
switch (collisionShape->getShapeType())
{
case BOX_SHAPE_PROXYTYPE:
{
shapeType = cShape::eShapeBox;
btBoxShape* box = (btBoxShape*)collisionShape;
param0 = box->getHalfExtentsWithMargin()[0] * 2;
param1 = box->getHalfExtentsWithMargin()[1] * 2;
param2 = box->getHalfExtentsWithMargin()[2] * 2;
break;
}
case SPHERE_SHAPE_PROXYTYPE:
{
btSphereShape* sphere = (btSphereShape*)collisionShape;
param0 = sphere->getRadius() * 2;
param1 = sphere->getRadius() * 2;
param2 = sphere->getRadius() * 2;
shapeType = cShape::eShapeSphere;
break;
}
case CAPSULE_SHAPE_PROXYTYPE:
{
btCapsuleShape* caps = (btCapsuleShape*)collisionShape;
param0 = caps->getRadius() * 2;
param1 = caps->getHalfHeight() * 2;
param2 = caps->getRadius() * 2;
shapeType = cShape::eShapeCapsule;
break;
}
default:
{
btAssert(0);
}
}
}
btVector3 body_attach_pt1 = links[j].m_dVector;
//tQuaternion body_to_parent_body = parent_to_parent_body * this_to_parent * body_to_this;
//tQuaternion parent_to_parent_body = parent_body_to_parent.inverse();
bodyDefs(i, cKinTree::eBodyParam0) = param0;
bodyDefs(i, cKinTree::eBodyParam1) = param1;
bodyDefs(i, cKinTree::eBodyParam2) = param2;
bodyDefs(i, cKinTree::eBodyParamShape) = shapeType;
bodyDefs(i, cKinTree::eBodyParamMass) = links[j].m_mass;
bodyDefs(i, cKinTree::eBodyParamColGroup) = unk;
bodyDefs(i, cKinTree::eBodyParamEnableFallContact) = unk;
bodyDefs(i, cKinTree::eBodyColorR) = unk;
bodyDefs(i, cKinTree::eBodyColorG) = unk;
bodyDefs(i, cKinTree::eBodyColorB) = unk;
bodyDefs(i, cKinTree::eBodyColorA) = unk;
dVectorRot[j] = links[j].m_this_to_body1;
btVector3 body_attach_pt2 = quatRotate(links[j].m_this_to_body1.inverse(), body_attach_pt1);
bodyToLinkPositions[i] = body_attach_pt2;
bodyDefs(i, cKinTree::eBodyParamAttachX) = body_attach_pt2[0];
bodyDefs(i, cKinTree::eBodyParamAttachY) = body_attach_pt2[1];
bodyDefs(i, cKinTree::eBodyParamAttachZ) = body_attach_pt2[2];
btScalar bodyAttachThetaX = 0;
btScalar bodyAttachThetaY = 0;
btScalar bodyAttachThetaZ = 0;
btQuaternion body_to_this1 = links[j].m_this_to_body1.inverse();
body_to_this1.getEulerZYX(bodyAttachThetaZ, bodyAttachThetaY, bodyAttachThetaX);
bodyDefs(i, cKinTree::eBodyParamAttachThetaX) = bodyAttachThetaX;
bodyDefs(i, cKinTree::eBodyParamAttachThetaY) = bodyAttachThetaY;
bodyDefs(i, cKinTree::eBodyParamAttachThetaZ) = bodyAttachThetaZ;
jointMat(i, cKinTree::eJointDescType) = links[j].m_jointType;
jointMat(i, cKinTree::eJointDescParent) = parentIndex;
btVector3 jointAttachPointMy = links[j].m_eVector;
btVector3 jointAttachPointMyv0 = jointAttachPointMy;
btVector3 parentBodyAttachPtMy(0, 0, 0);
btQuaternion parentBodyToLink;
parentBodyToLink = btQuaternion::getIdentity();
btQuaternion linkToParentBody = btQuaternion::getIdentity();
int parent_joint = links[j].m_parentIndex;
if (parent_joint != gInvalidIdx)
{
parentBodyAttachPtMy = bodyToLinkPositions[parent_joint];
parentBodyToLink = bodyToLinkRotations[parent_joint];
linkToParentBody = parentBodyToLink.inverse();
}
parentBodyAttachPtMy = quatRotate(linkToParentBody, parentBodyAttachPtMy);
//bodyToLinkRotations
jointAttachPointMy += parentBodyAttachPtMy;
jointAttachPointMy = quatRotate(linkToParentBody.inverse(), jointAttachPointMy);
btVector3 parent_body_attach_pt1(0, 0, 0);
if (parentIndex >= 0)
{
parent_body_attach_pt1 = links[parentIndex].m_dVector;
}
btQuaternion myparent_body_to_body(0, 0, 0, 1);
btQuaternion mybody_to_parent_body(0, 0, 0, 1);
btQuaternion parent_body_to_body1 = links[i].m_zeroRotParentToThis;
btQuaternion body_to_parent_body1 = parent_body_to_body1.inverse();
bodyToLinkRotations[i] = body_to_this1;
jointMat(i, cKinTree::eJointDescAttachX) = jointAttachPointMy[0];
jointMat(i, cKinTree::eJointDescAttachY) = jointAttachPointMy[1];
jointMat(i, cKinTree::eJointDescAttachZ) = jointAttachPointMy[2];
btQuaternion parent2parent_body2(0, 0, 0, 1);
if (parent_joint >= 0)
{
//parent2parent_body2 = bulletMB->getLink(parent_joint).m_dVectorRot;
parent2parent_body2 = dVectorRot[parent_joint];
}
///btQuaternion this2bodyA = bulletMB->getLink(j).m_dVectorRot;
btQuaternion this2bodyA = dVectorRot[j];
btQuaternion parent_body_2_body = links[j].m_zeroRotParentToThis;
btQuaternion combined2 = parent_body_2_body.inverse();
btQuaternion recoverthis2parent = parent2parent_body2.inverse()*combined2*this2bodyA;// body2this.inverse();
btScalar eulZ, eulY, eulX;
recoverthis2parent.getEulerZYX(eulZ, eulY, eulX);
jointMat(i, cKinTree::eJointDescAttachThetaX) = eulX;
jointMat(i, cKinTree::eJointDescAttachThetaY) = eulY;
jointMat(i, cKinTree::eJointDescAttachThetaZ) = eulZ;
jointMat(i, cKinTree::eJointDescLimLow0) = unk;
jointMat(i, cKinTree::eJointDescLimLow1) = unk;
jointMat(i, cKinTree::eJointDescLimLow2) = unk;
jointMat(i, cKinTree::eJointDescLimHigh0) = unk;
jointMat(i, cKinTree::eJointDescLimHigh1) = unk;
jointMat(i, cKinTree::eJointDescLimHigh2) = unk;
jointMat(i, cKinTree::eJointDescTorqueLim) = unk;
jointMat(i, cKinTree::eJointDescForceLim) = unk;
jointMat(i, cKinTree::eJointDescIsEndEffector) = unk;
jointMat(i, cKinTree::eJointDescDiffWeight) = unk;
jointMat(i, cKinTree::eJointDescParamOffset) = totalDofs;
totalDofs += links[j].m_dofCount;
}
return result;
}
void btExtractJointBodyFromBullet(const btMultiBody* bulletMB, Eigen::MatrixXd& bodyDefs, Eigen::MatrixXd& jointMat)
{
btAlignedObjectArray<TempLink> links;
int numBaseShapes = 0;
if (bulletMB->getBaseCollider())
{
switch (bulletMB->getBaseCollider()->getCollisionShape()->getShapeType())
{
case CAPSULE_SHAPE_PROXYTYPE:
case SPHERE_SHAPE_PROXYTYPE:
case BOX_SHAPE_PROXYTYPE:
{
numBaseShapes++;
break;
}
case COMPOUND_SHAPE_PROXYTYPE:
{
btCompoundShape* compound = (btCompoundShape*)bulletMB->getBaseCollider()->getCollisionShape();
numBaseShapes += compound->getNumChildShapes();
break;
}
default:
{
}
}
}
//links include the 'base' and its childlinks
int baseLink = numBaseShapes? 1 : 0;
links.resize(bulletMB->getNumLinks() + baseLink);
for (int i = 0; i < links.size(); i++)
{
memset(&links[i], 0xffffffff, sizeof(TempLink));
}
int totalDofs = 0;
if (numBaseShapes)
{
//links[0] is the root/base
links[0].m_parentIndex = -1;
links[0].m_collider = bulletMB->getBaseCollider();
links[0].m_mass = bulletMB->getBaseMass();
links[0].m_jointType = (bulletMB->hasFixedBase()) ? cKinTree::eJointTypeFixed : cKinTree::eJointTypeNone;
links[0].m_dofOffset = 0;
links[0].m_dofCount = 7;
links[0].m_dVector.setValue(0, 0, 0);
links[0].m_eVector.setValue(0, 0, 0);
links[0].m_zeroRotParentToThis = btQuaternion(0, 0, 0, 1);
links[0].m_this_to_body1 = btQuaternion(0, 0, 0, 1);
totalDofs = 7;
}
for (int j = 0; j < bulletMB->getNumLinks(); ++j)
{
int parentIndex = bulletMB->getLink(j).m_parent;
links[j + baseLink].m_parentIndex = parentIndex + baseLink;
links[j + baseLink].m_collider = bulletMB->getLinkCollider(j);
links[j + baseLink].m_mass = bulletMB->getLink(j).m_mass;
int jointType = 0;
btQuaternion this_to_body1(0, 0, 0, 1);
int dofCount = 0;
if ((baseLink)==0 &&j == 0)//for 'root' either use fixed or none
{
dofCount = 7;
links[j].m_parentIndex = -1;
links[j].m_jointType = (bulletMB->hasFixedBase()) ? cKinTree::eJointTypeFixed : cKinTree::eJointTypeNone;
links[j].m_dofOffset = 0;
links[j].m_dofCount = dofCount;
links[j].m_zeroRotParentToThis = btQuaternion(0, 0, 0, 1);
//links[j].m_dVector.setValue(0, 0, 0);
links[j].m_dVector = bulletMB->getLink(j).m_dVector;
links[j].m_eVector.setValue(0, 0, 0);
//links[j].m_eVector = bulletMB->getLink(j).m_eVector;
links[j].m_zeroRotParentToThis = bulletMB->getLink(j).m_zeroRotParentToThis;
links[j].m_this_to_body1 = btQuaternion(0, 0, 0, 1);
totalDofs = 7;
}
else
{
switch (bulletMB->getLink(j).m_jointType)
{
case btMultibodyLink::eFixed:
{
jointType = cKinTree::eJointTypeFixed;
break;
}
case btMultibodyLink::ePrismatic:
{
dofCount = 1;
btVector3 refAxis(1, 0, 0);
btVector3 axis = bulletMB->getLink(j).getAxisTop(0);
this_to_body1 = shortestArcQuat(refAxis, btVector3(axis[0], axis[1], axis[2]));
jointType = cKinTree::eJointTypePrismatic;
break;
}
case btMultibodyLink::eSpherical:
{
dofCount = 4;//??
jointType = cKinTree::eJointTypeSpherical;
break;
}
case btMultibodyLink::eRevolute:
{
dofCount = 1;
btVector3 refAxis(0, 0, 1);
btVector3 axis = bulletMB->getLink(j).getAxisTop(0);
this_to_body1 = shortestArcQuat(refAxis, btVector3(axis[0], axis[1], axis[2]));
jointType = cKinTree::eJointTypeRevolute;
break;
}
default:
{
}
}
links[j + baseLink].m_jointType = jointType;
links[j + baseLink].m_dofOffset = totalDofs;
links[j + baseLink].m_dofCount = dofCount;
links[j + baseLink].m_dVector = bulletMB->getLink(j).m_dVector;
links[j + baseLink].m_eVector = bulletMB->getLink(j).m_eVector;
links[j + baseLink].m_zeroRotParentToThis = bulletMB->getLink(j).m_zeroRotParentToThis;
links[j + baseLink].m_this_to_body1 = this_to_body1;
}
totalDofs += dofCount;
}
btExtractJointBodyFromTempLinks(links, bodyDefs, jointMat);
}

9
examples/SharedMemory/plugins/stablePDPlugin/BulletConversion.h Normal file
View File

@@ -0,0 +1,9 @@
#ifndef BULLET_CONVERSION_H
#define BULLET_CONVERSION_H
class btMultiBody;
#include "MathUtil.h"
void btExtractJointBodyFromBullet(const btMultiBody* bulletMB, Eigen::MatrixXd& bodyDefs, Eigen::MatrixXd& jointMat);
#endif //BULLET_CONVERSION_H

1982
examples/SharedMemory/plugins/stablePDPlugin/KinTree.cpp Normal file
View File

File diff suppressed because it is too large Load Diff

271
examples/SharedMemory/plugins/stablePDPlugin/KinTree.h Normal file
View File

@@ -0,0 +1,271 @@
#pragma once
#include <vector>
#include <fstream>
#include "Shape.h"
#ifdef USE_JSON
#include "json/json.h"
#endif
#include "MathUtil.h"
class cKinTree
{
public:
// description of the joint tree representing an articulated figure
enum eJointType
{
eJointTypeRevolute,
eJointTypePlanar,
eJointTypePrismatic,
eJointTypeFixed,
eJointTypeSpherical,
eJointTypeNone,
eJointTypeMax
};
enum eJointDesc
{
eJointDescType,
eJointDescParent,
eJointDescAttachX,
eJointDescAttachY,
eJointDescAttachZ,
eJointDescAttachThetaX, // euler angles order rot(Z) * rot(Y) * rot(X)
eJointDescAttachThetaY,
eJointDescAttachThetaZ,
eJointDescLimLow0,
eJointDescLimLow1,
eJointDescLimLow2,
eJointDescLimHigh0,
eJointDescLimHigh1,
eJointDescLimHigh2,
eJointDescTorqueLim,
eJointDescForceLim,
eJointDescIsEndEffector,
eJointDescDiffWeight,
eJointDescParamOffset,
eJointDescMax
};
typedef Eigen::Matrix<double, 1, eJointDescMax> tJointDesc;
enum eBodyParam
{
eBodyParamShape,
eBodyParamMass,
eBodyParamColGroup, // 0 collides with nothing and 1 collides with everything
eBodyParamEnableFallContact,
eBodyParamAttachX,
eBodyParamAttachY,
eBodyParamAttachZ,
eBodyParamAttachThetaX, // Euler angles order XYZ
eBodyParamAttachThetaY,
eBodyParamAttachThetaZ,
eBodyParam0,
eBodyParam1,
eBodyParam2,
eBodyColorR,
eBodyColorG,
eBodyColorB,
eBodyColorA,
eBodyParamMax
};
typedef Eigen::Matrix<double, 1, eBodyParamMax> tBodyDef;
enum eDrawShape
{
eDrawShapeShape,
eDrawShapeParentJoint,
eDrawShapeAttachX,
eDrawShapeAttachY,
eDrawShapeAttachZ,
eDrawShapeAttachThetaX, // Euler angles order XYZ
eDrawShapeAttachThetaY,
eDrawShapeAttachThetaZ,
eDrawShapeParam0,
eDrawShapeParam1,
eDrawShapeParam2,
eDrawShapeColorR,
eDrawShapeColorG,
eDrawShapeColorB,
eDrawShapeColorA,
eDrawShapeMeshID,
eDrawShapeParamMax
};
typedef Eigen::Matrix<double, 1, eDrawShapeParamMax> tDrawShapeDef;
static const int gInvalidJointID;
static const int gPosDim;
static const int gRotDim;
static const int gRootDim;
static bool HasValidRoot(const Eigen::MatrixXd& joint_mat);
static tVector GetRootPos(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& state);
static void SetRootPos(const Eigen::MatrixXd& joint_mat, const tVector& pos, Eigen::VectorXd& out_state);
static tQuaternion GetRootRot(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& state);
static void SetRootRot(const Eigen::MatrixXd& joint_mat, const tQuaternion& rot, Eigen::VectorXd& out_state);
static tVector GetRootVel(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& vel);
static void SetRootVel(const Eigen::MatrixXd& joint_mat, const tVector& vel, Eigen::VectorXd& out_vel);
static tVector GetRootAngVel(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& vel);
static void SetRootAngVel(const Eigen::MatrixXd& joint_mat, const tVector& ang_vel, Eigen::VectorXd& out_vel);
static tVector CalcJointWorldPos(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& state, int joint_id);
static tVector LocalToWorldPos(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& state, int parent_id, const tVector& attach_pt);
static tQuaternion CalcJointWorldRot(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& state, int joint_id);
static void CalcJointWorldTheta(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& state, int joint_id, tVector& out_axis, double& out_theta);
static tVector CalcJointWorldVel(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& state, const Eigen::VectorXd& vel, int joint_id);
static tVector CalcWorldVel(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& state, const Eigen::VectorXd& vel, int parent_id, const tVector& attach_pt);
static tVector CalcJointWorldAngularVel(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& state, const Eigen::VectorXd& vel, int joint_id);
static tVector CalcWorldAngularVel(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& state, const Eigen::VectorXd& vel, int parent_id, const tVector& attach_pt);
static int GetNumDof(const Eigen::MatrixXd& joint_mat);
static void ApplyStep(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& step, Eigen::VectorXd& out_pose);
static Eigen::VectorXi FindJointChain(const Eigen::MatrixXd& joint_mat, int joint_beg, int joint_end);
static bool IsAncestor(const Eigen::MatrixXd& joint_mat, int child_joint, int ancestor_joint, int& out_len);
static double CalcChainLength(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXi& chain);
static void CalcAABB(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& state, tVector& out_min, tVector& out_max);
static int GetParamOffset(const Eigen::MatrixXd& joint_mat, int joint_id);
static int GetParamSize(const Eigen::MatrixXd& joint_mat, int joint_id);
static int GetJointParamSize(eJointType joint_type);
static void GetJointParams(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& state, int j, Eigen::VectorXd& out_params);
static void SetJointParams(const Eigen::MatrixXd& joint_mat, int j, const Eigen::VectorXd& params, Eigen::VectorXd& out_state);
static eJointType GetJointType(const Eigen::MatrixXd& joint_mat, int joint_id);
static int GetParent(const Eigen::MatrixXd& joint_mat, int joint_id);
static bool HasParent(const Eigen::MatrixXd& joint_mat, int joint_id);
static bool IsRoot(const Eigen::MatrixXd& joint_mat, int joint_id);
static bool IsJointActuated(const Eigen::MatrixXd& joint_mat, int joint_id);
static double GetTorqueLimit(const Eigen::MatrixXd& joint_mat, int joint_id);
static double GetForceLimit(const Eigen::MatrixXd& joint_mat, int joint_id);
static bool IsEndEffector(const Eigen::MatrixXd& joint_mat, int joint_id);
static tVector GetJointLimLow(const Eigen::MatrixXd& joint_mat, int joint_id);
static tVector GetJointLimHigh(const Eigen::MatrixXd& joint_mat, int joint_id);
static double GetJointDiffWeight(const Eigen::MatrixXd& joint_mat, int joint_id);
static double CalcLinkLength(const Eigen::MatrixXd& joint_mat, int joint_id);
static tVector GetAttachPt(const Eigen::MatrixXd& joint_mat, int joint_id);
static tVector GetAttachTheta(const Eigen::MatrixXd& joint_mat, int joint_id);
// calculates the longest chain in the subtree of each joint
static void CalcMaxSubChainLengths(const Eigen::MatrixXd& joint_mat, Eigen::VectorXd& out_lengths);
static void CalcSubTreeMasses(const Eigen::MatrixXd& joint_mat, const Eigen::MatrixXd& body_defs, Eigen::VectorXd& out_masses);
static tMatrix BuildAttachTrans(const Eigen::MatrixXd& joint_mat, int joint_id);
static tMatrix ChildParentTrans(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& state, int joint_id);
static tMatrix ParentChildTrans(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& state, int joint_id);
static tMatrix JointWorldTrans(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& state, int joint_id);
static tMatrix WorldJointTrans(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& state, int joint_id);
#ifdef USE_JSON
static bool Load(const Json::Value& root, Eigen::MatrixXd& out_joint_mat);
static bool ParseBodyDef(const Json::Value& root, tBodyDef& out_def);
static bool ParseDrawShapeDef(const Json::Value& root, tDrawShapeDef& out_def);
static std::string BuildJointMatJson(const Eigen::MatrixXd& joint_mat);
static std::string BuildJointJson(int id, const tJointDesc& joint_desc);
static bool ParseJoint(const Json::Value& root, tJointDesc& out_joint_desc);
#endif
static int GetNumJoints(const Eigen::MatrixXd& joint_mat);
static int GetRoot(const Eigen::MatrixXd& joint_mat);
static void FindChildren(const Eigen::MatrixXd& joint_mat, int joint_id, Eigen::VectorXi& out_children);
static bool LoadBodyDefs(const std::string& char_file, Eigen::MatrixXd& out_body_defs);
static bool LoadDrawShapeDefs(const std::string& char_file, Eigen::MatrixXd& out_draw_defs);
static cShape::eShape GetBodyShape(const Eigen::MatrixXd& body_defs, int part_id);
static tVector GetBodyAttachPt(const Eigen::MatrixXd& body_defs, int part_id);
static tVector GetBodyAttachTheta(const Eigen::MatrixXd& body_defs, int part_id);
static void GetBodyRotation(const Eigen::MatrixXd& body_defs, int part_id, tVector& out_axis, double& out_theta);
static double GetBodyMass(const Eigen::MatrixXd& body_defs, int part_id);
static int GetBodyColGroup(const Eigen::MatrixXd& body_defs, int part_id);
static bool GetBodyEnableFallContact(const Eigen::MatrixXd& body_defs, int part_id);
static void SetBodyEnableFallContact(int part_id, bool enable, Eigen::MatrixXd& out_body_defs);
static tVector GetBodySize(const Eigen::MatrixXd& body_defs, int part_id);
static tVector GetBodyColor(const Eigen::MatrixXd& body_defs, int part_id);
static double CalcTotalMass(const Eigen::MatrixXd& body_defs);
static bool IsValidBody(const Eigen::MatrixXd& body_defs, int part_id);
static tVector GetBodyLocalCoM(const Eigen::MatrixXd& body_defs, int part_id);
static int GetDrawShapeParentJoint(const tDrawShapeDef& shape);
static tVector GetDrawShapeAttachPt(const tDrawShapeDef& shape);
static tVector GetDrawShapeAttachTheta(const tDrawShapeDef& shape);
static void GetDrawShapeRotation(const tDrawShapeDef& shape, tVector& out_axis, double& out_theta);
static tVector GetDrawShapeColor(const tDrawShapeDef& shape);
static int GetDrawShapeMeshID(const tDrawShapeDef& shape);
static tVector CalcBodyPartPos(const Eigen::MatrixXd& joint_mat, const Eigen::MatrixXd& body_defs, const Eigen::VectorXd& state, int part_id);
static tVector CalcBodyPartVel(const Eigen::MatrixXd& joint_mat, const Eigen::MatrixXd& body_defs, const Eigen::VectorXd& state, const Eigen::VectorXd& vel, int part_id);
static void CalcBodyPartRotation(const Eigen::MatrixXd& joint_mat, const Eigen::MatrixXd& body_defs, const Eigen::VectorXd& state, int part_id, tVector& out_axis, double& out_theta);
static tMatrix BodyWorldTrans(const Eigen::MatrixXd& joint_mat, const Eigen::MatrixXd& body_defs, const Eigen::VectorXd& state, int part_id);
static tMatrix BodyJointTrans(const Eigen::MatrixXd& body_defs, int part_id);
static tJointDesc BuildJointDesc(eJointType joint_type, int parent_id, const tVector& attach_pt);
static tJointDesc BuildJointDesc();
static tBodyDef BuildBodyDef();
static tDrawShapeDef BuildDrawShapeDef();
static void BuildDefaultPose(const Eigen::MatrixXd& joint_mat, Eigen::VectorXd& out_pose);
static void BuildDefaultVel(const Eigen::MatrixXd& joint_mat, Eigen::VectorXd& out_vel);
static void CalcPoseDiff(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& pose0, const Eigen::VectorXd& pose1, Eigen::VectorXd& out_diff);
static tVector CalcRootPosDiff(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& pose0, const Eigen::VectorXd& pose1);
static tQuaternion CalcRootRotDiff(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& pose0, const Eigen::VectorXd& pose1);
static double CalcPoseErr(const Eigen::MatrixXd& joint_mat, int joint_id, const Eigen::VectorXd& pose0, const Eigen::VectorXd& pose1);
static double CalcRootPosErr(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& pose0, const Eigen::VectorXd& pose1);
static double CalcRootRotErr(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& pose0, const Eigen::VectorXd& pose1);
static void CalcVelDiff(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& vel0, const Eigen::VectorXd& vel1, Eigen::VectorXd& out_diff);
static tVector CalcRootVelDiff(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& pose0, const Eigen::VectorXd& pose1);
static tVector CalcRootAngVelDiff(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& pose0, const Eigen::VectorXd& pose1);
static double CalcVelErr(const Eigen::MatrixXd& joint_mat, int joint_id, const Eigen::VectorXd& vel0, const Eigen::VectorXd& vel1);
static double CalcRootVelErr(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& vel0, const Eigen::VectorXd& vel1);
static double CalcRootAngVelErr(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& vel0, const Eigen::VectorXd& vel1);
static void CalcVel(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& pose0, const Eigen::VectorXd& pose1, double dt, Eigen::VectorXd& out_vel);
static void PostProcessPose(const Eigen::MatrixXd& joint_mat, Eigen::VectorXd& out_pose);
static void LerpPoses(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& pose0, const Eigen::VectorXd& pose1, double lerp, Eigen::VectorXd& out_pose);
static void VelToPoseDiff(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& pose, const Eigen::VectorXd& vel, Eigen::VectorXd& out_pose_diff);
static double CalcHeading(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& pose);
static tQuaternion CalcHeadingRot(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& pose);
static tMatrix BuildHeadingTrans(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& pose);
static tMatrix BuildOriginTrans(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& pose);
static void NormalizePoseHeading(const Eigen::MatrixXd& joint_mat, Eigen::VectorXd& out_pose);
static void NormalizePoseHeading(const Eigen::MatrixXd& joint_mat, Eigen::VectorXd& out_pose, Eigen::VectorXd& out_vel);
static void MirrorPoseStance(const Eigen::MatrixXd& joint_mat, const std::vector<int> mirror_joints0, const std::vector<int> mirror_joints1, Eigen::VectorXd& out_pose);
protected:
static bool ParseJointType(const std::string& type_str, eJointType& out_joint_type);
static void PostProcessJointMat(Eigen::MatrixXd& out_joint_mat);
static tMatrix ChildParentTransRoot(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& state, int joint_id);
static tMatrix ChildParentTransRevolute(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& state, int joint_id);
static tMatrix ChildParentTransPlanar(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& state, int joint_id);
static tMatrix ChildParentTransFixed(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& state, int joint_id);
static tMatrix ChildParentTransPrismatic(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& state, int joint_id);
static tMatrix ChildParentTransSpherical(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& state, int joint_id);
static void BuildDefaultPoseRoot(const Eigen::MatrixXd& joint_mat, Eigen::VectorXd& out_pose);
static void BuildDefaultPoseRevolute(Eigen::VectorXd& out_pose);
static void BuildDefaultPosePrismatic(Eigen::VectorXd& out_pose);
static void BuildDefaultPosePlanar(Eigen::VectorXd& out_pose);
static void BuildDefaultPoseFixed(Eigen::VectorXd& out_pose);
static void BuildDefaultPoseSpherical(Eigen::VectorXd& out_pose);
static void BuildDefaultVelRoot(const Eigen::MatrixXd& joint_mat, Eigen::VectorXd& out_pose);
static void BuildDefaultVelRevolute(Eigen::VectorXd& out_pose);
static void BuildDefaultVelPrismatic(Eigen::VectorXd& out_pose);
static void BuildDefaultVelPlanar(Eigen::VectorXd& out_pose);
static void BuildDefaultVelFixed(Eigen::VectorXd& out_pose);
static void BuildDefaultVelSpherical(Eigen::VectorXd& out_pose);
static void CalcJointPoseDiff(const Eigen::MatrixXd& joint_mat, int joint_id, const Eigen::VectorXd& pose0, const Eigen::VectorXd& pose1, Eigen::VectorXd& out_diff);
static void CalcJointVelDiff(const Eigen::MatrixXd& joint_mat, int joint_id, const Eigen::VectorXd& vel0, const Eigen::VectorXd& vel1, Eigen::VectorXd& out_diff);
};

21
examples/SharedMemory/plugins/stablePDPlugin/LICENSE.txt Normal file
View File

@@ -0,0 +1,21 @@
MIT License
Copyright (c) 2018 Xue Bin Peng
Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:
The above copyright notice and this permission notice shall be included in all
copies or substantial portions of the Software.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
SOFTWARE.

904
examples/SharedMemory/plugins/stablePDPlugin/MathUtil.cpp Normal file
View File

@@ -0,0 +1,904 @@
#include "MathUtil.h"
#include <time.h>
#define _USE_MATH_DEFINES
#include <math.h>
cRand cMathUtil::gRand = cRand();
int cMathUtil::Clamp(int val, int min, int max)
{
return std::max(min, std::min(val, max));
}
void cMathUtil::Clamp(const Eigen::VectorXd& min, const Eigen::VectorXd& max, Eigen::VectorXd& out_vec)
{
out_vec = out_vec.cwiseMin(max).cwiseMax(min);
}
double cMathUtil::Clamp(double val, double min, double max)
{
return std::max(min, std::min(val, max));
}
double cMathUtil::Saturate(double val)
{
return Clamp(val, 0.0, 1.0);
}
double cMathUtil::Lerp(double t, double val0, double val1)
{
return (1 - t) * val0 + t * val1;
}
double cMathUtil::NormalizeAngle(double theta)
{
// normalizes theta to be between [-pi, pi]
double norm_theta = fmod(theta, 2 * M_PI);
if (norm_theta > M_PI)
{
norm_theta = -2 * M_PI + norm_theta;
}
else if (norm_theta < -M_PI)
{
norm_theta = 2 * M_PI + norm_theta;
}
return norm_theta;
}
double cMathUtil::RandDouble()
{
return RandDouble(0, 1);
}
double cMathUtil::RandDouble(double min, double max)
{
return gRand.RandDouble(min, max);
}
double cMathUtil::RandDoubleNorm(double mean, double stdev)
{
return gRand.RandDoubleNorm(mean, stdev);
}
double cMathUtil::RandDoubleExp(double lambda)
{
return gRand.RandDoubleExp(lambda);
}
double cMathUtil::RandDoubleSeed(double seed)
{
unsigned int int_seed = *reinterpret_cast<unsigned int*>(&seed);
std::default_random_engine rand_gen(int_seed);
std::uniform_real_distribution<double> dist;
return dist(rand_gen);
}
int cMathUtil::RandInt()
{
return gRand.RandInt();
}
int cMathUtil::RandInt(int min, int max)
{
return gRand.RandInt(min, max);
}
int cMathUtil::RandUint()
{
return gRand.RandUint();
}
int cMathUtil::RandUint(unsigned int min, unsigned int max)
{
return gRand.RandUint(min, max);
}
int cMathUtil::RandIntExclude(int min, int max, int exc)
{
return gRand.RandIntExclude(min, max, exc);
}
void cMathUtil::SeedRand(unsigned long int seed)
{
gRand.Seed(seed);
srand(gRand.RandInt());
}
int cMathUtil::RandSign()
{
return gRand.RandSign();
}
double cMathUtil::SmoothStep(double t)
{
double val = t * t * t * (t * (t * 6 - 15) + 10);
return val;
}
bool cMathUtil::FlipCoin(double p)
{
return gRand.FlipCoin(p);
}
tMatrix cMathUtil::TranslateMat(const tVector& trans)
{
tMatrix mat = tMatrix::Identity();
mat(0, 3) = trans[0];
mat(1, 3) = trans[1];
mat(2, 3) = trans[2];
return mat;
}
tMatrix cMathUtil::ScaleMat(double scale)
{
return ScaleMat(tVector::Ones() * scale);
}
tMatrix cMathUtil::ScaleMat(const tVector& scale)
{
tMatrix mat = tMatrix::Identity();
mat(0, 0) = scale[0];
mat(1, 1) = scale[1];
mat(2, 2) = scale[2];
return mat;
}
tMatrix cMathUtil::RotateMat(const tVector& euler)
{
double x = euler[0];
double y = euler[1];
double z = euler[2];
double x_s = std::sin(x);
double x_c = std::cos(x);
double y_s = std::sin(y);
double y_c = std::cos(y);
double z_s = std::sin(z);
double z_c = std::cos(z);
tMatrix mat = tMatrix::Identity();
mat(0, 0) = y_c * z_c;
mat(1, 0) = y_c * z_s;
mat(2, 0) = -y_s;
mat(0, 1) = x_s * y_s * z_c - x_c * z_s;
mat(1, 1) = x_s * y_s * z_s + x_c * z_c;
mat(2, 1) = x_s * y_c;
mat(0, 2) = x_c * y_s * z_c + x_s * z_s;
mat(1, 2) = x_c * y_s * z_s - x_s * z_c;
mat(2, 2) = x_c * y_c;
return mat;
}
tMatrix cMathUtil::RotateMat(const tVector& axis, double theta)
{
assert(std::abs(axis.squaredNorm() - 1) < 0.0001);
double c = std::cos(theta);
double s = std::sin(theta);
double x = axis[0];
double y = axis[1];
double z = axis[2];
tMatrix mat;
mat << c + x * x * (1 - c), x * y * (1 - c) - z * s, x * z * (1 - c) + y * s, 0,
y * x * (1 - c) + z * s, c + y * y * (1 - c), y * z * (1 - c) - x * s, 0,
z * x * (1 - c) - y * s, z * y * (1 - c) + x * s, c + z * z * (1 - c), 0,
0, 0, 0, 1;
return mat;
}
tMatrix cMathUtil::RotateMat(const tQuaternion& q)
{
tMatrix mat = tMatrix::Identity();
double sqw = q.w() * q.w();
double sqx = q.x()* q.x();
double sqy = q.y() * q.y();
double sqz = q.z() * q.z();
double invs = 1 / (sqx + sqy + sqz + sqw);
mat(0, 0) = (sqx - sqy - sqz + sqw) * invs;
mat(1, 1) = (-sqx + sqy - sqz + sqw) * invs;
mat(2, 2) = (-sqx - sqy + sqz + sqw) * invs;
double tmp1 = q.x()*q.y();
double tmp2 = q.z()*q.w();
mat(1, 0) = 2.0 * (tmp1 + tmp2) * invs;
mat(0, 1) = 2.0 * (tmp1 - tmp2) * invs;
tmp1 = q.x()*q.z();
tmp2 = q.y()*q.w();
mat(2, 0) = 2.0 * (tmp1 - tmp2) * invs;
mat(0, 2) = 2.0 * (tmp1 + tmp2) * invs;
tmp1 = q.y()*q.z();
tmp2 = q.x()*q.w();
mat(2, 1) = 2.0 * (tmp1 + tmp2) * invs;
mat(1, 2) = 2.0 * (tmp1 - tmp2) * invs;
return mat;
}
tMatrix cMathUtil::CrossMat(const tVector& a)
{
tMatrix m;
m << 0, -a[2], a[1], 0,
a[2], 0, -a[0], 0,
-a[1], a[0], 0, 0,
0, 0, 0, 1;
return m;
}
tMatrix cMathUtil::InvRigidMat(const tMatrix& mat)
{
tMatrix inv_mat = tMatrix::Zero();
inv_mat.block(0, 0, 3, 3) = mat.block(0, 0, 3, 3).transpose();
inv_mat.col(3) = -inv_mat * mat.col(3);
inv_mat(3, 3) = 1;
return inv_mat;
}
tVector cMathUtil::GetRigidTrans(const tMatrix& mat)
{
return tVector(mat(0, 3), mat(1, 3), mat(2, 3), 0);
}
tVector cMathUtil::InvEuler(const tVector& euler)
{
tMatrix inv_mat = cMathUtil::RotateMat(tVector(1, 0, 0, 0), -euler[0])
* cMathUtil::RotateMat(tVector(0, 1, 0, 0), -euler[1])
* cMathUtil::RotateMat(tVector(0, 0, 1, 0), -euler[2]);
tVector inv_euler = cMathUtil::RotMatToEuler(inv_mat);
return inv_euler;
}
void cMathUtil::RotMatToAxisAngle(const tMatrix& mat, tVector& out_axis, double& out_theta)
{
double c = (mat(0, 0) + mat(1, 1) + mat(2, 2) - 1) * 0.5;
c = cMathUtil::Clamp(c, -1.0, 1.0);
out_theta = std::acos(c);
if (std::abs(out_theta) < 0.00001)
{
out_axis = tVector(0, 0, 1, 0);
}
else
{
double m21 = mat(2, 1) - mat(1, 2);
double m02 = mat(0, 2) - mat(2, 0);
double m10 = mat(1, 0) - mat(0, 1);
double denom = std::sqrt(m21 * m21 + m02 * m02 + m10 * m10);
out_axis[0] = m21 / denom;
out_axis[1] = m02 / denom;
out_axis[2] = m10 / denom;
out_axis[3] = 0;
}
}
tVector cMathUtil::RotMatToEuler(const tMatrix& mat)
{
tVector euler;
euler[0] = std::atan2(mat(2, 1), mat(2, 2));
euler[1] = std::atan2(-mat(2, 0), std::sqrt(mat(2, 1) * mat(2, 1) + mat(2, 2) * mat(2, 2)));
euler[2] = std::atan2(mat(1, 0), mat(0, 0));
euler[3] = 0;
return euler;
}
tQuaternion cMathUtil::RotMatToQuaternion(const tMatrix& mat)
{
double tr = mat(0, 0) + mat(1, 1) + mat(2, 2);
tQuaternion q;
if (tr > 0) {
double S = sqrt(tr + 1.0) * 2; // S=4*qw
q.w() = 0.25 * S;
q.x() = (mat(2, 1) - mat(1, 2)) / S;
q.y() = (mat(0, 2) - mat(2, 0)) / S;
q.z() = (mat(1, 0) - mat(0, 1)) / S;
}
else if ((mat(0, 0) > mat(1, 1) && (mat(0, 0) > mat(2, 2)))) {
double S = sqrt(1.0 + mat(0, 0) - mat(1, 1) - mat(2, 2)) * 2; // S=4*qx
q.w() = (mat(2, 1) - mat(1, 2)) / S;
q.x() = 0.25 * S;
q.y() = (mat(0, 1) + mat(1, 0)) / S;
q.z() = (mat(0, 2) + mat(2, 0)) / S;
}
else if (mat(1, 1) > mat(2, 2)) {
double S = sqrt(1.0 + mat(1, 1) - mat(0, 0) - mat(2, 2)) * 2; // S=4*qy
q.w() = (mat(0, 2) - mat(2, 0)) / S;
q.x() = (mat(0, 1) + mat(1, 0)) / S;
q.y() = 0.25 * S;
q.z() = (mat(1, 2) + mat(2, 1)) / S;
}
else {
double S = sqrt(1.0 + mat(2, 2) - mat(0, 0) - mat(1, 1)) * 2; // S=4*qz
q.w() = (mat(1, 0) - mat(0, 1)) / S;
q.x() = (mat(0, 2) + mat(2, 0)) / S;
q.y() = (mat(1, 2) + mat(2, 1)) / S;
q.z() = 0.25 * S;
}
return q;
}
void cMathUtil::EulerToAxisAngle(const tVector& euler, tVector& out_axis, double& out_theta)
{
double x = euler[0];
double y = euler[1];
double z = euler[2];
double x_s = std::sin(x);
double x_c = std::cos(x);
double y_s = std::sin(y);
double y_c = std::cos(y);
double z_s = std::sin(z);
double z_c = std::cos(z);
double c = (y_c * z_c + x_s * y_s * z_s + x_c * z_c + x_c * y_c - 1) * 0.5;
c = Clamp(c, -1.0, 1.0);
out_theta = std::acos(c);
if (std::abs(out_theta) < 0.00001)
{
out_axis = tVector(0, 0, 1, 0);
}
else
{
double m21 = x_s * y_c - x_c * y_s * z_s + x_s * z_c;
double m02 = x_c * y_s * z_c + x_s * z_s + y_s;
double m10 = y_c * z_s - x_s * y_s * z_c + x_c * z_s;
double denom = std::sqrt(m21 * m21 + m02 * m02 + m10 * m10);
out_axis[0] = m21 / denom;
out_axis[1] = m02 / denom;
out_axis[2] = m10 / denom;
out_axis[3] = 0;
}
}
tVector cMathUtil::AxisAngleToEuler(const tVector& axis, double theta)
{
tQuaternion q = AxisAngleToQuaternion(axis, theta);
return QuaternionToEuler(q);
}
tMatrix cMathUtil::DirToRotMat(const tVector& dir, const tVector& up)
{
tVector x = up.cross3(dir);
double x_norm = x.norm();
if (x_norm == 0)
{
x_norm = 1;
x = (dir.dot(up) >= 0) ? tVector(1, 0, 0, 0) : tVector(-1, 0, 0, 0);
}
x /= x_norm;
tVector y = dir.cross3(x).normalized();
tVector z = dir;
tMatrix mat = tMatrix::Identity();
mat.block(0, 0, 3, 1) = x.segment(0, 3);
mat.block(0, 1, 3, 1) = y.segment(0, 3);
mat.block(0, 2, 3, 1) = z.segment(0, 3);
return mat;
}
void cMathUtil::DeltaRot(const tVector& axis0, double theta0, const tVector& axis1, double theta1,
tVector& out_axis, double& out_theta)
{
tMatrix R0 = RotateMat(axis0, theta0);
tMatrix R1 = RotateMat(axis1, theta1);
tMatrix M = DeltaRot(R0, R1);
RotMatToAxisAngle(M, out_axis, out_theta);
}
tMatrix cMathUtil::DeltaRot(const tMatrix& R0, const tMatrix& R1)
{
return R1 * R0.transpose();
}
tQuaternion cMathUtil::EulerToQuaternion(const tVector& euler)
{
tVector axis;
double theta;
EulerToAxisAngle(euler, axis, theta);
return AxisAngleToQuaternion(axis, theta);
}
tVector cMathUtil::QuaternionToEuler(const tQuaternion& q)
{
double sinr = 2.0 * (q.w() * q.x() + q.y() * q.z());
double cosr = 1.0 - 2.0 * (q.x() * q.x() + q.y() * q.y());
double x = std::atan2(sinr, cosr);
double sinp = 2.0 * (q.w() * q.y() - q.z() * q.x());
double y = 0;
if (fabs(sinp) >= 1)
{
y = copysign(M_PI / 2, sinp); // use 90 degrees if out of range
}
else
{
y = asin(sinp);
}
double siny = 2.0 * (q.w() * q.z() + q.x() * q.y());
double cosy = 1.0 - 2.0 * (q.y() * q.y() + q.z() * q.z());
double z = std::atan2(siny, cosy);
return tVector(x, y, z, 0);
}
tQuaternion cMathUtil::AxisAngleToQuaternion(const tVector& axis, double theta)
{
// axis must be normalized
double c = std::cos(theta / 2);
double s = std::sin(theta / 2);
tQuaternion q;
q.w() = c;
q.x() = s * axis[0];
q.y() = s * axis[1];
q.z() = s * axis[2];
return q;
}
void cMathUtil::QuaternionToAxisAngle(const tQuaternion& q, tVector& out_axis, double& out_theta)
{
out_theta = 0;
out_axis = tVector(0, 0, 1, 0);
tQuaternion q1 = q;
if (q1.w() > 1)
{
q1.normalize();
}
double sin_theta = std::sqrt(1 - q1.w() * q1.w());
if (sin_theta > 0.000001)
{
out_theta = 2 * std::acos(q1.w());
out_theta = cMathUtil::NormalizeAngle(out_theta);
out_axis = tVector(q1.x(), q1.y(), q1.z(), 0) / sin_theta;
}
}
tMatrix cMathUtil::BuildQuaternionDiffMat(const tQuaternion& q)
{
tMatrix mat;
mat << -0.5 * q.x(), -0.5 * q.y(), -0.5 * q.z(), 0,
0.5 * q.w(), -0.5 * q.z(), 0.5 * q.y(), 0,
0.5 * q.z(), 0.5 * q.w(), -0.5 * q.x(), 0,
-0.5 * q.y(), 0.5 * q.x(), 0.5 * q.w(), 0;
return mat;
}
tVector cMathUtil::CalcQuaternionVel(const tQuaternion& q0, const tQuaternion& q1, double dt)
{
tQuaternion q_diff = cMathUtil::QuatDiff(q0, q1);
tVector axis;
double theta;
QuaternionToAxisAngle(q_diff, axis, theta);
return (theta / dt) * axis;
}
tVector cMathUtil::CalcQuaternionVelRel(const tQuaternion& q0, const tQuaternion& q1, double dt)
{
// calculate relative rotational velocity in the coordinate frame of q0
tQuaternion q_diff = q0.conjugate() * q1;
tVector axis;
double theta;
QuaternionToAxisAngle(q_diff, axis, theta);
return (theta / dt) * axis;
}
tQuaternion cMathUtil::VecToQuat(const tVector& v)
{
return tQuaternion(v[0], v[1], v[2], v[3]);
}
tVector cMathUtil::QuatToVec(const tQuaternion& q)
{
return tVector(q.w(), q.x(), q.y(), q.z());
}
tQuaternion cMathUtil::QuatDiff(const tQuaternion& q0, const tQuaternion& q1)
{
return q1 * q0.conjugate();
}
double cMathUtil::QuatDiffTheta(const tQuaternion& q0, const tQuaternion& q1)
{
tQuaternion dq = QuatDiff(q0, q1);
return QuatTheta(dq);
}
double cMathUtil::QuatTheta(const tQuaternion& dq)
{
double theta = 0;
tQuaternion q1 = dq;
if (q1.w() > 1)
{
q1.normalize();
}
double sin_theta = std::sqrt(1 - q1.w() * q1.w());
if (sin_theta > 0.0001)
{
theta = 2 * std::acos(q1.w());
theta = cMathUtil::NormalizeAngle(theta);
}
return theta;
}
tQuaternion cMathUtil::VecDiffQuat(const tVector& v0, const tVector& v1)
{
return tQuaternion::FromTwoVectors(v0.segment(0, 3), v1.segment(0, 3));
}
tVector cMathUtil::QuatRotVec(const tQuaternion& q, const tVector& dir)
{
tVector rot_dir = tVector::Zero();
rot_dir.segment(0, 3) = q * dir.segment(0, 3);
return rot_dir;
}
tQuaternion cMathUtil::MirrorQuaternion(const tQuaternion& q, eAxis axis)
{
tQuaternion mirror_q;
mirror_q.w() = q.w();
mirror_q.x() = (axis == eAxisX) ? q.x() : -q.x();
mirror_q.y() = (axis == eAxisY) ? q.y() : -q.y();
mirror_q.z() = (axis == eAxisZ) ? q.z() : -q.z();
return mirror_q;
}
double cMathUtil::Sign(double val)
{
return SignAux<double>(val);
}
int cMathUtil::Sign(int val)
{
return SignAux<int>(val);
}
double cMathUtil::AddAverage(double avg0, int count0, double avg1, int count1)
{
double total = count0 + count1;
return (count0 / total) * avg0 + (count1 / total) * avg1;
}
tVector cMathUtil::AddAverage(const tVector& avg0, int count0, const tVector& avg1, int count1)
{
double total = count0 + count1;
return (count0 / total) * avg0 + (count1 / total) * avg1 ;
}
void cMathUtil::AddAverage(const Eigen::VectorXd& avg0, int count0, const Eigen::VectorXd& avg1, int count1, Eigen::VectorXd& out_result)
{
double total = count0 + count1;
out_result = (count0 / total) * avg0 + (count1 / total) * avg1;
}
void cMathUtil::CalcSoftmax(const Eigen::VectorXd& vals, double temp, Eigen::VectorXd& out_prob)
{
assert(out_prob.size() == vals.size());
int num_vals = static_cast<int>(vals.size());
double sum = 0;
double max_val = vals.maxCoeff();
for (int i = 0; i < num_vals; ++i)
{
double val = vals[i];
val = std::exp((val - max_val) / temp);
out_prob[i] = val;
sum += val;
}
out_prob /= sum;
}
double cMathUtil::EvalGaussian(const Eigen::VectorXd& mean, const Eigen::VectorXd& covar, const Eigen::VectorXd& sample)
{
assert(mean.size() == covar.size());
assert(sample.size() == covar.size());
Eigen::VectorXd diff = sample - mean;
double exp_val = diff.dot(diff.cwiseQuotient(covar));
double likelihood = std::exp(-0.5 * exp_val);
double partition = CalcGaussianPartition(covar);
likelihood /= partition;
return likelihood;
}
double cMathUtil::EvalGaussian(double mean, double covar, double sample)
{
double diff = sample - mean;
double exp_val = diff * diff / covar;
double norm = 1 / std::sqrt(2 * M_PI * covar);
double likelihood = norm * std::exp(-0.5 * exp_val);
return likelihood;
}
double cMathUtil::CalcGaussianPartition(const Eigen::VectorXd& covar)
{
int data_size = static_cast<int>(covar.size());
double det = covar.prod();
double partition = std::sqrt(std::pow(2 * M_PI, data_size) * det);
return partition;
}
double cMathUtil::EvalGaussianLogp(const Eigen::VectorXd& mean, const Eigen::VectorXd& covar, const Eigen::VectorXd& sample)
{
int data_size = static_cast<int>(covar.size());
Eigen::VectorXd diff = sample - mean;
double logp = -0.5 * diff.dot(diff.cwiseQuotient(covar));
double det = covar.prod();
logp += -0.5 * (data_size * std::log(2 * M_PI) + std::log(det));
return logp;
}
double cMathUtil::EvalGaussianLogp(double mean, double covar, double sample)
{
double diff = sample - mean;
double logp = -0.5 * diff * diff / covar;
logp += -0.5 * (std::log(2 * M_PI) + std::log(covar));
return logp;
}
double cMathUtil::Sigmoid(double x)
{
return Sigmoid(x, 1, 0);
}
double cMathUtil::Sigmoid(double x, double gamma, double bias)
{
double exp = -gamma * (x + bias);
double val = 1 / (1 + std::exp(exp));
return val;
}
int cMathUtil::SampleDiscreteProb(const Eigen::VectorXd& probs)
{
assert(std::abs(probs.sum() - 1) < 0.00001);
double rand = RandDouble();
int rand_idx = gInvalidIdx;
int num_probs = static_cast<int>(probs.size());
for (int i = 0; i < num_probs; ++i)
{
double curr_prob = probs[i];
rand -= curr_prob;
if (rand <= 0)
{
rand_idx = i;
break;
}
}
return rand_idx;
}
tVector cMathUtil::CalcBarycentric(const tVector& p, const tVector& a, const tVector& b, const tVector& c)
{
tVector v0 = b - a;
tVector v1 = c - a;
tVector v2 = p - a;
double d00 = v0.dot(v0);
double d01 = v0.dot(v1);
double d11 = v1.dot(v1);
double d20 = v2.dot(v0);
double d21 = v2.dot(v1);
double denom = d00 * d11 - d01 * d01;
double v = (d11 * d20 - d01 * d21) / denom;
double w = (d00 * d21 - d01 * d20) / denom;
double u = 1.0f - v - w;
return tVector(u, v, w, 0);
}
bool cMathUtil::ContainsAABB(const tVector& pt, const tVector& aabb_min, const tVector& aabb_max)
{
bool contains = pt[0] >= aabb_min[0] && pt[1] >= aabb_min[1] && pt[2] >= aabb_min[2]
&& pt[0] <= aabb_max[0] && pt[1] <= aabb_max[1] && pt[2] <= aabb_max[2];
return contains;
}
bool cMathUtil::ContainsAABB(const tVector& aabb_min0, const tVector& aabb_max0, const tVector& aabb_min1, const tVector& aabb_max1)
{
return ContainsAABB(aabb_min0, aabb_min1, aabb_max1) && ContainsAABB(aabb_max0, aabb_min1, aabb_max1);
}
bool cMathUtil::ContainsAABBXZ(const tVector& pt, const tVector& aabb_min, const tVector& aabb_max)
{
bool contains = pt[0] >= aabb_min[0] && pt[2] >= aabb_min[2]
&& pt[0] <= aabb_max[0] && pt[2] <= aabb_max[2];
return contains;
}
bool cMathUtil::ContainsAABBXZ(const tVector& aabb_min0, const tVector& aabb_max0, const tVector& aabb_min1, const tVector& aabb_max1)
{
return ContainsAABBXZ(aabb_min0, aabb_min1, aabb_max1) && ContainsAABBXZ(aabb_max0, aabb_min1, aabb_max1);
}
void cMathUtil::CalcAABBIntersection(const tVector& aabb_min0, const tVector& aabb_max0, const tVector& aabb_min1, const tVector& aabb_max1,
tVector& out_min, tVector& out_max)
{
out_min = aabb_min0.cwiseMax(aabb_min1);
out_max = aabb_max0.cwiseMin(aabb_max1);
if (out_min[0] > out_max[0])
{
out_min[0] = 0;
out_max[0] = 0;
}
if (out_min[1] > out_max[1])
{
out_min[1] = 0;
out_max[1] = 0;
}
if (out_min[2] > out_max[2])
{
out_min[2] = 0;
out_max[2] = 0;
}
}
void cMathUtil::CalcAABBUnion(const tVector& aabb_min0, const tVector& aabb_max0, const tVector& aabb_min1, const tVector& aabb_max1,
tVector& out_min, tVector& out_max)
{
out_min = aabb_min0.cwiseMin(aabb_min1);
out_max = aabb_max0.cwiseMax(aabb_max1);
}
bool cMathUtil::IntersectAABB(const tVector& aabb_min0, const tVector& aabb_max0, const tVector& aabb_min1, const tVector& aabb_max1)
{
tVector center0 = 0.5 * (aabb_max0 + aabb_min0);
tVector center1 = 0.5 * (aabb_max1 + aabb_min1);
tVector size0 = aabb_max0 - aabb_min0;
tVector size1 = aabb_max1 - aabb_min1;
tVector test_len = 0.5 * (size0 + size1);
tVector delta = center1 - center0;
bool overlap = (std::abs(delta[0]) <= test_len[0]) && (std::abs(delta[1]) <= test_len[1]) && (std::abs(delta[2]) <= test_len[2]);
return overlap;
}
bool cMathUtil::IntersectAABBXZ(const tVector& aabb_min0, const tVector& aabb_max0, const tVector& aabb_min1, const tVector& aabb_max1)
{
tVector center0 = 0.5 * (aabb_max0 + aabb_min0);
tVector center1 = 0.5 * (aabb_max1 + aabb_min1);
tVector size0 = aabb_max0 - aabb_min0;
tVector size1 = aabb_max1 - aabb_min1;
tVector test_len = 0.5 * (size0 + size1);
tVector delta = center1 - center0;
bool overlap = (std::abs(delta[0]) <= test_len[0]) && (std::abs(delta[2]) <= test_len[2]);
return overlap;
}
bool cMathUtil::CheckNextInterval(double delta, double curr_val, double int_size)
{
double pad = 0.001 * delta;
int curr_count = static_cast<int>(std::floor((curr_val + pad) / int_size));
int prev_count = static_cast<int>(std::floor((curr_val + pad - delta) / int_size));
bool new_action = (curr_count != prev_count);
return new_action;
}
tVector cMathUtil::SampleRandPt(const tVector& bound_min, const tVector& bound_max)
{
tVector pt = tVector(RandDouble(bound_min[0], bound_max[0]),
RandDouble(bound_min[1], bound_max[1]),
RandDouble(bound_min[2], bound_max[2]), 0);
return pt;
}
tVector cMathUtil::SampleRandPtBias(const tVector& bound_min, const tVector& bound_max)
{
return SampleRandPtBias(bound_min, bound_max, 0.5 * (bound_max + bound_min));
}
tVector cMathUtil::SampleRandPtBias(const tVector& bound_min, const tVector& bound_max, const tVector& focus)
{
double t = RandDouble(0, 1);
tVector size = bound_max - bound_min;
tVector new_min = focus + (t * 0.5) * size;
tVector new_max = focus - (t * 0.5) * size;
tVector offset = (bound_min - new_min).cwiseMax(0);
offset += (bound_max - new_max).cwiseMin(0);
new_min += offset;
new_max += offset;
return SampleRandPt(new_min, new_max);
}
void cMathUtil::QuatSwingTwistDecomposition(const tQuaternion& q, const tVector& dir, tQuaternion& out_swing, tQuaternion& out_twist)
{
assert(std::abs(dir.norm() - 1) < 0.000001);
assert(std::abs(q.norm() - 1) < 0.000001);
tVector q_axis = tVector(q.x(), q.y(), q.z(), 0);
double p = q_axis.dot(dir);
tVector twist_axis = p * dir;
out_twist = tQuaternion(q.w(), twist_axis[0], twist_axis[1], twist_axis[2]);
out_twist.normalize();
out_swing = q * out_twist.conjugate();
}
tQuaternion cMathUtil::ProjectQuat(const tQuaternion& q, const tVector& dir)
{
assert(std::abs(dir.norm() - 1) < 0.00001);
tVector ref_axis = tVector::Zero();
int min_idx = 0;
dir.cwiseAbs().minCoeff(&min_idx);
ref_axis[min_idx] = 1;
tVector rot_dir0 = dir.cross3(ref_axis);
tVector rot_dir1 = cMathUtil::QuatRotVec(q, rot_dir0);
rot_dir1 -= rot_dir1.dot(dir) * dir;
double dir1_norm = rot_dir1.norm();
tQuaternion p_rot = tQuaternion::Identity();
if (dir1_norm > 0.0001)
{
rot_dir1 /= dir1_norm;
p_rot = cMathUtil::VecDiffQuat(rot_dir0, rot_dir1);
}
return p_rot;
}
void cMathUtil::ButterworthFilter(double dt, double cutoff, Eigen::VectorXd& out_x)
{
double sampling_rate = 1 / dt;
int n = static_cast<int>(out_x.size());
double wc = std::tan(cutoff * M_PI / sampling_rate);
double k1 = std::sqrt(2) * wc;
double k2 = wc * wc;
double a = k2 / (1 + k1 + k2);
double b = 2 * a;
double c = a;
double k3 = b / k2;
double d = -2 * a + k3;
double e = 1 - (2 * a) - k3;
double xm2 = out_x[0];
double xm1 = out_x[0];
double ym2 = out_x[0];
double ym1 = out_x[0];
for (int s = 0; s < n; ++s)
{
double x = out_x[s];
double y = a * x + b * xm1 + c * xm2 + d * ym1 + e * ym2;
out_x[s] = y;
xm2 = xm1;
xm1 = x;
ym2 = ym1;
ym1 = y;
}
double yp2 = out_x[n - 1];
double yp1 = out_x[n - 1];
double zp2 = out_x[n - 1];
double zp1 = out_x[n - 1];
for (int t = n - 1; t >= 0; --t)
{
double y = out_x[t];
double z = a * y + b * yp1 + c * yp2 + d * zp1 + e * zp2;
out_x[t] = z;
yp2 = yp1;
yp1 = y;
zp2 = zp1;
zp1 = z;
}
}

155
examples/SharedMemory/plugins/stablePDPlugin/MathUtil.h Normal file
View File

@@ -0,0 +1,155 @@
#pragma once
#include <random>
#include "Eigen/Dense"
#include "Eigen/StdVector"
#include "Eigen/Geometry"
#include "Rand.h"
#define _USE_MATH_DEFINES
#include "math.h"
const int gInvalidIdx = -1;
// for convenience define standard vector for rendering
typedef Eigen::Vector4d tVector;
typedef Eigen::Vector4d tVector3;
typedef Eigen::Matrix4d tMatrix;
typedef Eigen::Matrix3d tMatrix3;
typedef Eigen::Quaterniond tQuaternion;
template <typename T>
using tEigenArr = std::vector<T, Eigen::aligned_allocator<T> >;
typedef tEigenArr<tVector> tVectorArr;
const double gRadiansToDegrees = 57.2957795;
const double gDegreesToRadians = 1.0 / gRadiansToDegrees;
const tVector gGravity = tVector(0, -9.8, 0, 0);
const double gInchesToMeters = 0.0254;
const double gFeetToMeters = 0.3048;
class cMathUtil
{
public:
enum eAxis
{
eAxisX,
eAxisY,
eAxisZ,
eAxisMax
};
static int Clamp(int val, int min, int max);
static void Clamp(const Eigen::VectorXd& min, const Eigen::VectorXd& max, Eigen::VectorXd& out_vec);
static double Clamp(double val, double min, double max);
static double Saturate(double val);
static double Lerp(double t, double val0, double val1);
static double NormalizeAngle(double theta);
// rand number
static double RandDouble();
static double RandDouble(double min, double max);
static double RandDoubleNorm(double mean, double stdev);
static double RandDoubleExp(double lambda);
static double RandDoubleSeed(double seed);
static int RandInt();
static int RandInt(int min, int max);
static int RandUint();
static int RandUint(unsigned int min, unsigned int max);
static int RandIntExclude(int min, int max, int exc);
static void SeedRand(unsigned long int seed);
static int RandSign();
static bool FlipCoin(double p = 0.5);
static double SmoothStep(double t);
// matrices
static tMatrix TranslateMat(const tVector& trans);
static tMatrix ScaleMat(double scale);
static tMatrix ScaleMat(const tVector& scale);
static tMatrix RotateMat(const tVector& euler); // euler angles order rot(Z) * rot(Y) * rot(X)
static tMatrix RotateMat(const tVector& axis, double theta);
static tMatrix RotateMat(const tQuaternion& q);
static tMatrix CrossMat(const tVector& a);
// inverts a transformation consisting only of rotations and translations
static tMatrix InvRigidMat(const tMatrix& mat);
static tVector GetRigidTrans(const tMatrix& mat);
static tVector InvEuler(const tVector& euler);
static void RotMatToAxisAngle(const tMatrix& mat, tVector& out_axis, double& out_theta);
static tVector RotMatToEuler(const tMatrix& mat);
static tQuaternion RotMatToQuaternion(const tMatrix& mat);
static void EulerToAxisAngle(const tVector& euler, tVector& out_axis, double& out_theta);
static tVector AxisAngleToEuler(const tVector& axis, double theta);
static tMatrix DirToRotMat(const tVector& dir, const tVector& up);
static void DeltaRot(const tVector& axis0, double theta0, const tVector& axis1, double theta1,
tVector& out_axis, double& out_theta);
static tMatrix DeltaRot(const tMatrix& R0, const tMatrix& R1);
static tQuaternion EulerToQuaternion(const tVector& euler);
static tVector QuaternionToEuler(const tQuaternion& q);
static tQuaternion AxisAngleToQuaternion(const tVector& axis, double theta);
static void QuaternionToAxisAngle(const tQuaternion& q, tVector& out_axis, double& out_theta);
static tMatrix BuildQuaternionDiffMat(const tQuaternion& q);
static tVector CalcQuaternionVel(const tQuaternion& q0, const tQuaternion& q1, double dt);
static tVector CalcQuaternionVelRel(const tQuaternion& q0, const tQuaternion& q1, double dt);
static tQuaternion VecToQuat(const tVector& v);
static tVector QuatToVec(const tQuaternion& q);
static tQuaternion QuatDiff(const tQuaternion& q0, const tQuaternion& q1);
static double QuatDiffTheta(const tQuaternion& q0, const tQuaternion& q1);
static double QuatTheta(const tQuaternion& dq);
static tQuaternion VecDiffQuat(const tVector& v0, const tVector& v1);
static tVector QuatRotVec(const tQuaternion& q, const tVector& dir);
static tQuaternion MirrorQuaternion(const tQuaternion& q, eAxis axis);
static double Sign(double val);
static int Sign(int val);
static double AddAverage(double avg0, int count0, double avg1, int count1);
static tVector AddAverage(const tVector& avg0, int count0, const tVector& avg1, int count1);
static void AddAverage(const Eigen::VectorXd& avg0, int count0, const Eigen::VectorXd& avg1, int count1, Eigen::VectorXd& out_result);
static void CalcSoftmax(const Eigen::VectorXd& vals, double temp, Eigen::VectorXd& out_prob);
static double EvalGaussian(const Eigen::VectorXd& mean, const Eigen::VectorXd& covar, const Eigen::VectorXd& sample);
static double EvalGaussian(double mean, double covar, double sample);
static double CalcGaussianPartition(const Eigen::VectorXd& covar);
static double EvalGaussianLogp(double mean, double covar, double sample);
static double EvalGaussianLogp(const Eigen::VectorXd& mean, const Eigen::VectorXd& covar, const Eigen::VectorXd& sample);
static double Sigmoid(double x);
static double Sigmoid(double x, double gamma, double bias);
static int SampleDiscreteProb(const Eigen::VectorXd& probs);
static tVector CalcBarycentric(const tVector& p, const tVector& a, const tVector& b, const tVector& c);
static bool ContainsAABB(const tVector& pt, const tVector& aabb_min, const tVector& aabb_max);
static bool ContainsAABB(const tVector& aabb_min0, const tVector& aabb_max0, const tVector& aabb_min1, const tVector& aabb_max1);
static bool ContainsAABBXZ(const tVector& pt, const tVector& aabb_min, const tVector& aabb_max);
static bool ContainsAABBXZ(const tVector& aabb_min0, const tVector& aabb_max0, const tVector& aabb_min1, const tVector& aabb_max1);
static void CalcAABBIntersection(const tVector& aabb_min0, const tVector& aabb_max0, const tVector& aabb_min1, const tVector& aabb_max1,
tVector& out_min, tVector& out_max);
static void CalcAABBUnion(const tVector& aabb_min0, const tVector& aabb_max0, const tVector& aabb_min1, const tVector& aabb_max1,
tVector& out_min, tVector& out_max);
static bool IntersectAABB(const tVector& aabb_min0, const tVector& aabb_max0, const tVector& aabb_min1, const tVector& aabb_max1);
static bool IntersectAABBXZ(const tVector& aabb_min0, const tVector& aabb_max0, const tVector& aabb_min1, const tVector& aabb_max1);
// check if curr_val and curr_val - delta belong to different intervals
static bool CheckNextInterval(double delta, double curr_val, double int_size);
static tVector SampleRandPt(const tVector& bound_min, const tVector& bound_max);
// samples a bound within the given bounds with a benter towards the focus pt
static tVector SampleRandPtBias(const tVector& bound_min, const tVector& bound_max);
static tVector SampleRandPtBias(const tVector& bound_min, const tVector& bound_max, const tVector& focus);
static void QuatSwingTwistDecomposition(const tQuaternion& q, const tVector& dir, tQuaternion& out_swing, tQuaternion& out_twist);
static tQuaternion ProjectQuat(const tQuaternion& q, const tVector& dir);
static void ButterworthFilter(double dt, double cutoff, Eigen::VectorXd& out_x);
private:
static cRand gRand;
template <typename T>
static T SignAux(T val)
{
return (T(0) < val) - (val < T(0));
}
};

245
examples/SharedMemory/plugins/stablePDPlugin/RBDModel.cpp Normal file
View File

@@ -0,0 +1,245 @@
#include "RBDModel.h"
#include "RBDUtil.h"
#include "KinTree.h"
cRBDModel::cRBDModel()
{
}
cRBDModel::~cRBDModel()
{
}
void cRBDModel::Init(const Eigen::MatrixXd& joint_mat, const Eigen::MatrixXd& body_defs, const tVector& gravity)
{
assert(joint_mat.rows() == body_defs.rows());
mGravity = gravity;
mJointMat = joint_mat;
mBodyDefs = body_defs;
int num_dofs = GetNumDof();
int num_joints = GetNumJoints();
const int svs = cSpAlg::gSpVecSize;
mPose = Eigen::VectorXd::Zero(num_dofs);
mVel = Eigen::VectorXd::Zero(num_dofs);
tMatrix trans_mat;
InitJointSubspaceArr();
mChildParentMatArr = Eigen::MatrixXd::Zero(num_joints * trans_mat.rows(), trans_mat.cols());
mSpWorldJointTransArr = Eigen::MatrixXd::Zero(num_joints * cSpAlg::gSVTransRows, cSpAlg::gSVTransCols);
mMassMat = Eigen::MatrixXd::Zero(num_dofs, num_dofs);
mBiasForce = Eigen::VectorXd::Zero(num_dofs);
mInertiaBuffer = Eigen::MatrixXd::Zero(num_joints * svs, svs);
}
void cRBDModel::Update(const Eigen::VectorXd& pose, const Eigen::VectorXd& vel)
{
SetPose(pose);
SetVel(vel);
UpdateJointSubspaceArr();
UpdateChildParentMatArr();
UpdateSpWorldTrans();
UpdateMassMat();
UpdateBiasForce();
}
int cRBDModel::GetNumDof() const
{
return cKinTree::GetNumDof(mJointMat);
}
int cRBDModel::GetNumJoints() const
{
return cKinTree::GetNumJoints(mJointMat);
}
void cRBDModel::SetGravity(const tVector& gravity)
{
mGravity = gravity;
}
const tVector& cRBDModel::GetGravity() const
{
return mGravity;
}
const Eigen::MatrixXd& cRBDModel::GetJointMat() const
{
return mJointMat;
}
const Eigen::MatrixXd& cRBDModel::GetBodyDefs() const
{
return mBodyDefs;
}
const Eigen::VectorXd& cRBDModel::GetPose() const
{
return mPose;
}
const Eigen::VectorXd& cRBDModel::GetVel() const
{
return mVel;
}
int cRBDModel::GetParent(int j) const
{
return cKinTree::GetParent(mJointMat, j);
}
const Eigen::MatrixXd& cRBDModel::GetMassMat() const
{
return mMassMat;
}
const Eigen::VectorXd& cRBDModel::GetBiasForce() const
{
return mBiasForce;
}
Eigen::MatrixXd& cRBDModel::GetInertiaBuffer()
{
return mInertiaBuffer;
}
tMatrix cRBDModel::GetChildParentMat(int j) const
{
assert(j >= 0 && j < GetNumJoints());
tMatrix trans;
int r = static_cast<int>(trans.rows());
int c = static_cast<int>(trans.cols());
trans = mChildParentMatArr.block(j * r, 0, r, c);
return trans;
}
tMatrix cRBDModel::GetParentChildMat(int j) const
{
tMatrix child_parent_trans = GetChildParentMat(j);
tMatrix parent_child_trans = cMathUtil::InvRigidMat(child_parent_trans);
return parent_child_trans;
}
cSpAlg::tSpTrans cRBDModel::GetSpChildParentTrans(int j) const
{
tMatrix mat = GetChildParentMat(j);
return cSpAlg::MatToTrans(mat);
}
cSpAlg::tSpTrans cRBDModel::GetSpParentChildTrans(int j) const
{
tMatrix mat = GetParentChildMat(j);
return cSpAlg::MatToTrans(mat);
}
tMatrix cRBDModel::GetWorldJointMat(int j) const
{
cSpAlg::tSpTrans trans = GetSpWorldJointTrans(j);
return cSpAlg::TransToMat(trans);
}
tMatrix cRBDModel::GetJointWorldMat(int j) const
{
cSpAlg::tSpTrans trans = GetSpJointWorldTrans(j);
return cSpAlg::TransToMat(trans);
}
cSpAlg::tSpTrans cRBDModel::GetSpWorldJointTrans(int j) const
{
assert(j >= 0 && j < GetNumJoints());
cSpAlg::tSpTrans trans = cSpAlg::GetTrans(mSpWorldJointTransArr, j);
return trans;
}
cSpAlg::tSpTrans cRBDModel::GetSpJointWorldTrans(int j) const
{
cSpAlg::tSpTrans world_joint_trans = GetSpWorldJointTrans(j);
return cSpAlg::InvTrans(world_joint_trans);
}
const Eigen::Block<const Eigen::MatrixXd> cRBDModel::GetJointSubspace(int j) const
{
assert(j >= 0 && j < GetNumJoints());
int offset = cKinTree::GetParamOffset(mJointMat, j);
int dim = cKinTree::GetParamSize(mJointMat, j);
int r = static_cast<int>(mJointSubspaceArr.rows());
return mJointSubspaceArr.block(0, offset, r, dim);
}
tVector cRBDModel::CalcJointWorldPos(int j) const
{
cSpAlg::tSpTrans world_joint_trans = GetSpWorldJointTrans(j);
tVector r = cSpAlg::GetRad(world_joint_trans);
return r;
}
void cRBDModel::SetPose(const Eigen::VectorXd& pose)
{
mPose = pose;
}
void cRBDModel::SetVel(const Eigen::VectorXd& vel)
{
mVel = vel;
}
void cRBDModel::InitJointSubspaceArr()
{
int num_dofs = GetNumDof();
int num_joints = GetNumJoints();
mJointSubspaceArr = Eigen::MatrixXd(cSpAlg::gSpVecSize, num_dofs);
for (int j = 0; j < num_joints; ++j)
{
int offset = cKinTree::GetParamOffset(mJointMat, j);
int dim = cKinTree::GetParamSize(mJointMat, j);
int r = static_cast<int>(mJointSubspaceArr.rows());
mJointSubspaceArr.block(0, offset, r, dim) = cRBDUtil::BuildJointSubspace(mJointMat, mPose, j);
}
}
void cRBDModel::UpdateJointSubspaceArr()
{
int num_joints = GetNumJoints();
for (int j = 0; j < num_joints; ++j)
{
bool const_subspace = cRBDUtil::IsConstJointSubspace(mJointMat, j);
if (!const_subspace)
{
int offset = cKinTree::GetParamOffset(mJointMat, j);
int dim = cKinTree::GetParamSize(mJointMat, j);
int r = static_cast<int>(mJointSubspaceArr.rows());
mJointSubspaceArr.block(0, offset, r, dim) = cRBDUtil::BuildJointSubspace(mJointMat, mPose, j);
}
}
}
void cRBDModel::UpdateChildParentMatArr()
{
int num_joints = GetNumJoints();
for (int j = 0; j < num_joints; ++j)
{
tMatrix child_parent_trans = cKinTree::ChildParentTrans(mJointMat, mPose, j);
int r = static_cast<int>(child_parent_trans.rows());
int c = static_cast<int>(child_parent_trans.cols());
mChildParentMatArr.block(j * r, 0, r, c) = child_parent_trans;
}
}
void cRBDModel::UpdateSpWorldTrans()
{
cRBDUtil::CalcWorldJointTransforms(*this, mSpWorldJointTransArr);
}
void cRBDModel::UpdateMassMat()
{
cRBDUtil::BuildMassMat(*this, mInertiaBuffer, mMassMat);
}
void cRBDModel::UpdateBiasForce()
{
cRBDUtil::BuildBiasForce(*this, mBiasForce);
}

71
examples/SharedMemory/plugins/stablePDPlugin/RBDModel.h Normal file
View File

@@ -0,0 +1,71 @@
#pragma once
#include "SpAlg.h"
#include "MathUtil.h"
// this class is mostly to help with efficiency by precomputing some useful
// quantities for RBD calculations
class cRBDModel
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
cRBDModel();
~cRBDModel();
virtual void Init(const Eigen::MatrixXd& joint_mat, const Eigen::MatrixXd& body_defs, const tVector& gravity);
virtual void Update(const Eigen::VectorXd& pose, const Eigen::VectorXd& vel);
virtual int GetNumDof() const;
virtual int GetNumJoints() const;
virtual const tVector& GetGravity() const;
virtual void SetGravity(const tVector& gravity);
virtual const Eigen::MatrixXd& GetJointMat() const;
virtual const Eigen::MatrixXd& GetBodyDefs() const;
virtual const Eigen::VectorXd& GetPose() const;
virtual const Eigen::VectorXd& GetVel() const;
virtual int GetParent(int j) const;
virtual const Eigen::MatrixXd& GetMassMat() const;
virtual const Eigen::VectorXd& GetBiasForce() const;
virtual Eigen::MatrixXd& GetInertiaBuffer();
virtual tMatrix GetChildParentMat(int j) const;
virtual tMatrix GetParentChildMat(int j) const;
virtual cSpAlg::tSpTrans GetSpChildParentTrans(int j) const;
virtual cSpAlg::tSpTrans GetSpParentChildTrans(int j) const;
virtual tMatrix GetWorldJointMat(int j) const;
virtual tMatrix GetJointWorldMat(int j) const;
virtual cSpAlg::tSpTrans GetSpWorldJointTrans(int j) const;
virtual cSpAlg::tSpTrans GetSpJointWorldTrans(int j) const;
virtual const Eigen::Block<const Eigen::MatrixXd> GetJointSubspace(int j) const;
virtual tVector CalcJointWorldPos(int j) const;
protected:
tVector mGravity;
Eigen::MatrixXd mJointMat;
Eigen::MatrixXd mBodyDefs;
Eigen::VectorXd mPose;
Eigen::VectorXd mVel;
Eigen::MatrixXd mJointSubspaceArr;
Eigen::MatrixXd mChildParentMatArr;
Eigen::MatrixXd mSpWorldJointTransArr;
Eigen::MatrixXd mMassMat;
Eigen::VectorXd mBiasForce;
Eigen::MatrixXd mInertiaBuffer;
virtual void SetPose(const Eigen::VectorXd& pose);
virtual void SetVel(const Eigen::VectorXd& vel);
virtual void InitJointSubspaceArr();
virtual void UpdateJointSubspaceArr();
virtual void UpdateChildParentMatArr();
virtual void UpdateSpWorldTrans();
virtual void UpdateMassMat();
virtual void UpdateBiasForce();
};

1050
examples/SharedMemory/plugins/stablePDPlugin/RBDUtil.cpp Normal file
View File

File diff suppressed because it is too large Load Diff

85
examples/SharedMemory/plugins/stablePDPlugin/RBDUtil.h Normal file
View File

@@ -0,0 +1,85 @@
#pragma once
#include <vector>
#include <fstream>
#include "RBDModel.h"
#include "KinTree.h"
class cRBDUtil
{
public:
static void SolveInvDyna(const cRBDModel& model, const Eigen::VectorXd& acc, Eigen::VectorXd& out_tau);
static void SolveForDyna(const cRBDModel& model, const Eigen::VectorXd& tau, Eigen::VectorXd& out_acc);
static void SolveForDyna(const cRBDModel& model, const Eigen::VectorXd& tau, const Eigen::VectorXd& total_force, Eigen::VectorXd& out_acc);
static void BuildMassMat(const cRBDModel& model, Eigen::MatrixXd& out_mass_mat);
static void BuildMassMat(const cRBDModel& model, Eigen::MatrixXd& inertia_buffer, Eigen::MatrixXd& out_mass_mat);
static void BuildBiasForce(const cRBDModel& model, Eigen::VectorXd& out_bias_force);
static void CalcGravityForce(const cRBDModel& model, Eigen::VectorXd& out_g_force);
static void BuildEndEffectorJacobian(const cRBDModel& model, int joint_id, Eigen::MatrixXd& out_J);
static void BuildEndEffectorJacobian(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& pose, int joint_id, Eigen::MatrixXd& out_J);
static Eigen::MatrixXd MultJacobianEndEff(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& vel, const Eigen::MatrixXd& J, int joint_id);
static void BuildJacobian(const cRBDModel& model, Eigen::MatrixXd& out_J);
static Eigen::MatrixXd ExtractEndEffJacobian(const Eigen::MatrixXd& joint_mat, const Eigen::MatrixXd& J, int joint_id);
static void BuildCOMJacobian(const cRBDModel& model, Eigen::MatrixXd& out_J);
static void BuildCOMJacobian(const cRBDModel& model, const Eigen::MatrixXd& J, Eigen::MatrixXd& out_J);
static void BuildJacobianDot(const cRBDModel& model, Eigen::MatrixXd& out_J_dot);
static cSpAlg::tSpVec BuildCOMVelProdAcc(const cRBDModel& model);
static cSpAlg::tSpVec BuildCOMVelProdAccAux(const cRBDModel& model, const Eigen::MatrixXd& Jd);
static cSpAlg::tSpVec CalcVelProdAcc(const cRBDModel& model, const Eigen::MatrixXd& Jd, int joint_id);
static cSpAlg::tSpVec CalcJointWorldVel(const cRBDModel& model, int joint_id);
static cSpAlg::tSpVec CalcJointWorldVel(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& state, const Eigen::VectorXd& vel, int joint_id);
static cSpAlg::tSpVec CalcWorldVel(const cRBDModel& model, int joint_id);
static cSpAlg::tSpVec CalcWorldVel(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& state, const Eigen::VectorXd& vel, int joint_id);
static tVector CalcCoMPos(const cRBDModel& model);
static tVector CalcCoMVel(const cRBDModel& model);
static tVector CalcCoMVel(const Eigen::MatrixXd& joint_mat, const Eigen::MatrixXd& body_defs, const Eigen::VectorXd& pose, const Eigen::VectorXd& vel);
static void CalcCoM(const cRBDModel& model, tVector& out_com, tVector& out_vel);
static void CalcCoM(const Eigen::MatrixXd& joint_mat, const Eigen::MatrixXd& body_defs, const Eigen::VectorXd& pose, const Eigen::VectorXd& vel,
tVector& out_com, tVector& out_vel);
static cSpAlg::tSpTrans BuildParentChildTransform(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& pose, int j);
static cSpAlg::tSpTrans BuildChildParentTransform(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& pose, int j);
// extract a cSpAlg::tSpTrans from a matrix presentating a stack of transforms
static void CalcWorldJointTransforms(const cRBDModel& model, Eigen::MatrixXd& out_trans_arr);
static bool IsConstJointSubspace(const Eigen::MatrixXd& joint_mat, int j);
static Eigen::MatrixXd BuildJointSubspace(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& pose, int j);
static cSpAlg::tSpMat BuildMomentInertia(const Eigen::MatrixXd& body_defs, int part_id);
protected:
static Eigen::MatrixXd BuildJointSubspaceRoot(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& pose);
static Eigen::MatrixXd BuildJointSubspaceRevolute(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& pose, int j);
static Eigen::MatrixXd BuildJointSubspacePrismatic(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& pose, int j);
static Eigen::MatrixXd BuildJointSubspacePlanar(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& pose, int j);
static Eigen::MatrixXd BuildJointSubspaceFixed(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& pose, int j);
static Eigen::MatrixXd BuildJointSubspaceSpherical(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& pose, int j);
static cSpAlg::tSpVec BuildCj(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& q, const Eigen::VectorXd& q_dot, int j);
static cSpAlg::tSpVec BuildCjRoot(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& q, const Eigen::VectorXd& q_dot, int j);
static cSpAlg::tSpVec BuildCjRevolute(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& q_dot, int j);
static cSpAlg::tSpVec BuildCjPrismatic(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& q_dot, int j);
static cSpAlg::tSpVec BuildCjPlanar(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& q_dot, int j);
static cSpAlg::tSpVec BuildCjFixed(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& q_dot, int j);
static cSpAlg::tSpVec BuildCjSpherical(const Eigen::MatrixXd& joint_mat, const Eigen::VectorXd& q_dot, int j);
static cSpAlg::tSpMat BuildMomentInertiaBox(const Eigen::MatrixXd& body_defs, int part_id);
static cSpAlg::tSpMat BuildMomentInertiaCapsule(const Eigen::MatrixXd& body_defs, int part_id);
static cSpAlg::tSpMat BuildMomentInertiaSphere(const Eigen::MatrixXd& body_defs, int part_id);
static cSpAlg::tSpMat BuildMomentInertiaCylinder(const Eigen::MatrixXd& body_defs, int part_id);
// builds the spatial inertial matrix in the coordinate frame of the parent joint
static cSpAlg::tSpMat BuildInertiaSpatialMat(const Eigen::MatrixXd& body_defs, int part_id);
};

140
examples/SharedMemory/plugins/stablePDPlugin/Rand.cpp Normal file
View File

@@ -0,0 +1,140 @@
#include "Rand.h"
#include <time.h>
#include <assert.h>
#include <algorithm>
cRand::cRand()
{
unsigned long int seed = static_cast<unsigned long int>(time(NULL));
mRandGen = std::default_random_engine(seed);
mRandDoubleDist = std::uniform_real_distribution<double>(0, 1);
mRandDoubleDistNorm = std::normal_distribution<double>(0, 1);
mRandIntDist = std::uniform_int_distribution<int>(std::numeric_limits<int>::min() + 1, std::numeric_limits<int>::max()); // + 1 since there is one more neg int than pos int
mRandUintDist = std::uniform_int_distribution<unsigned int>(std::numeric_limits<unsigned int>::min(), std::numeric_limits<unsigned int>::max());
}
cRand::cRand(unsigned long int seed)
{
Seed(seed);
}
cRand::~cRand()
{
}
double cRand::RandDouble()
{
return mRandDoubleDist(mRandGen);
}
double cRand::RandDouble(double min, double max)
{
if (min == max)
{
return min;
}
// generate random double in [min, max]
double rand_double = mRandDoubleDist(mRandGen);
rand_double = min + (rand_double * (max - min));
return rand_double;
}
double cRand::RandDoubleExp(double lambda)
{
std::exponential_distribution<double> dist(lambda);
double rand_double = dist(mRandGen);
return rand_double;
}
double cRand::RandDoubleNorm(double mean, double stdev)
{
double rand_double = mRandDoubleDistNorm(mRandGen);
rand_double = mean + stdev * rand_double;
return rand_double;
}
int cRand::RandInt()
{
return mRandIntDist(mRandGen);
}
int cRand::RandInt(int min, int max)
{
if (min == max)
{
return min;
}
// generate random double in [min, max)
int delta = max - min;
int rand_int = std::abs(RandInt());
rand_int = min + rand_int % delta;
return rand_int;
}
int cRand::RandUint()
{
return mRandUintDist(mRandGen);
}
int cRand::RandUint(unsigned int min, unsigned int max)
{
if (min == max)
{
return min;
}
// generate random double in [min, max)
int delta = max - min;
int rand_int = RandUint();
rand_int = min + rand_int % delta;
return rand_int;
}
int cRand::RandIntExclude(int min, int max, int exc)
{
int rand_int = 0;
if (exc < min || exc >= max)
{
rand_int = RandInt(min, max);
}
else
{
int new_max = max - 1;
if (new_max <= min)
{
rand_int = min;
}
else
{
rand_int = RandInt(min, new_max);
if (rand_int >= exc)
{
++rand_int;
}
}
}
return rand_int;
}
void cRand::Seed(unsigned long int seed)
{
mRandGen.seed(seed);
mRandDoubleDist.reset();
mRandDoubleDistNorm.reset();
mRandIntDist.reset();
mRandUintDist.reset();
}
int cRand::RandSign()
{
return FlipCoin() ? -1 : 1;
}
bool cRand::FlipCoin(double p)
{
return (RandDouble(0, 1) < p);
}

31
examples/SharedMemory/plugins/stablePDPlugin/Rand.h Normal file
View File

@@ -0,0 +1,31 @@
#pragma once
#include <random>
class cRand
{
public:
cRand();
cRand(unsigned long int seed);
virtual ~cRand();
virtual double RandDouble();
virtual double RandDouble(double min, double max);
virtual double RandDoubleExp(double lambda);
virtual double RandDoubleNorm(double mean, double stdev);
virtual int RandInt();
virtual int RandInt(int min, int max);
virtual int RandUint();
virtual int RandUint(unsigned int min, unsigned int max);
virtual int RandIntExclude(int min, int max, int exc);
virtual void Seed(unsigned long int seed);
virtual int RandSign();
virtual bool FlipCoin(double p = 0.5);
private:
std::default_random_engine mRandGen;
std::uniform_real_distribution<double> mRandDoubleDist;
std::normal_distribution<double> mRandDoubleDistNorm;
std::uniform_int_distribution<int> mRandIntDist;
std::uniform_int_distribution<unsigned int> mRandUintDist;
};

37
examples/SharedMemory/plugins/stablePDPlugin/Shape.cpp Normal file
View File

@@ -0,0 +1,37 @@
#include "Shape.h"
#include <assert.h>
bool cShape::ParseShape(const std::string& str, eShape& out_shape)
{
bool succ = true;
if (str == "null")
{
out_shape = eShapeNull;
}
else if (str == "box")
{
out_shape = eShapeBox;
}
else if (str == "capsule")
{
out_shape = eShapeCapsule;
}
else if (str == "sphere")
{
out_shape = eShapeSphere;
}
else if (str == "cylinder")
{
out_shape = eShapeCylinder;
}
else if (str == "plane")
{
out_shape = eShapePlane;
}
else
{
printf("Unsupported body shape %s\n", str.c_str());
assert(false);
}
return succ;
}

20
examples/SharedMemory/plugins/stablePDPlugin/Shape.h Normal file
View File

@@ -0,0 +1,20 @@
#pragma once
#include <string>
class cShape
{
public:
enum eShape
{
eShapeNull,
eShapeBox,
eShapeCapsule,
eShapeSphere,
eShapeCylinder,
eShapePlane,
eShapeMax,
};
static bool ParseShape(const std::string& str, cShape::eShape& out_shape);
};

356
examples/SharedMemory/plugins/stablePDPlugin/SpAlg.cpp Normal file
View File

@@ -0,0 +1,356 @@
#include "SpAlg.h"
#include <iostream>
const int cSpAlg::gSpVecSize;
const int cSpAlg::gSVTransRows;
const int cSpAlg::gSVTransCols;
cSpAlg::tSpVec cSpAlg::ConvertCoordM(const tSpVec& m0, const tVector& origin0, const tVector& origin1)
{
return ConvertCoordM(m0, origin0, origin1, tMatrix::Identity());
}
cSpAlg::tSpVec cSpAlg::ConvertCoordM(const tSpVec& m0, const tVector& origin0, const tMatrix& R0,
const tVector& origin1, const tMatrix& R1)
{
tMatrix R = R1 * R0.transpose();
return ConvertCoordM(m0, origin0, origin1, R);
}
cSpAlg::tSpVec cSpAlg::ConvertCoordM(const tSpVec& m0, const tVector& origin0, const tVector& origin1, const tMatrix& R)
{
tSpTrans X = BuildTrans(R, origin1 - origin0);
return ApplyTransM(X, m0);
}
cSpAlg::tSpVec cSpAlg::ConvertCoordF(const tSpVec& f0, const tVector& origin0, const tVector& origin1)
{
return ConvertCoordF(f0, origin0, origin1, tMatrix::Identity());
}
cSpAlg::tSpVec cSpAlg::ConvertCoordF(const tSpVec& f0, const tVector& origin0, const tMatrix& R0,
const tVector& origin1, const tMatrix& R1)
{
tMatrix R = R1 * R0.transpose();
return ConvertCoordF(f0, origin0, origin1, R);
}
cSpAlg::tSpVec cSpAlg::ConvertCoordF(const tSpVec& f0, const tVector& origin0,
const tVector& origin1, const tMatrix& R)
{
tSpTrans X = BuildTrans(R, origin1 - origin0);
return ApplyTransF(X, f0);
}
cSpAlg::tSpVec cSpAlg::CrossM(const tSpVec& sv, const tSpVec& m)
{
tVector sv_o = GetOmega(sv);
tVector sv_v = GetV(sv);
tVector m_o = GetOmega(m);
tVector m_v = GetV(m);
tVector o = sv_o.cross3(m_o);
tVector v = sv_v.cross3(m_o) + sv_o.cross3(m_v);
return BuildSV(o, v);
}
Eigen::MatrixXd cSpAlg::CrossMs(const tSpVec& sv, const Eigen::MatrixXd& ms)
{
assert(ms.rows() == gSpVecSize);
Eigen::MatrixXd result(gSpVecSize, ms.cols());
for (int i = 0; i < ms.cols(); ++i)
{
const tSpVec& curr_m = ms.col(i);
result.col(i) = CrossM(sv, curr_m);
}
return result;
}
cSpAlg::tSpVec cSpAlg::CrossF(const tSpVec& sv, const tSpVec& f)
{
tVector sv_o = GetOmega(sv);
tVector sv_v = GetV(sv);
tVector f_o = GetOmega(f);
tVector f_v = GetV(f);
tVector o = sv_o.cross3(f_o) + sv_v.cross3(f_v);
tVector v = sv_o.cross3(f_v);
return BuildSV(o, v);
}
Eigen::MatrixXd cSpAlg::CrossFs(const tSpVec& sv, const Eigen::MatrixXd& fs)
{
assert(fs.rows() == gSpVecSize);
Eigen::MatrixXd result(gSpVecSize, fs.cols());
for (int i = 0; i < fs.cols(); ++i)
{
const tSpVec& curr_f = fs.col(i);
result.col(i) = CrossF(sv, curr_f);
}
return result;
}
cSpAlg::tSpVec cSpAlg::BuildSV(const tVector& v)
{
return BuildSV(tVector::Zero(), v);
}
cSpAlg::tSpVec cSpAlg::BuildSV(const tVector& o, const tVector& v)
{
tSpVec sv;
SetOmega(o, sv);
SetV(v, sv);
return sv;
}
tVector cSpAlg::GetOmega(const tSpVec& sv)
{
tVector o = tVector::Zero();
o.block(0, 0, 3, 1) = sv.block(0, 0, 3, 1);
return o;
}
void cSpAlg::SetOmega(const tVector& o, tSpVec& out_sv)
{
out_sv.block(0, 0, 3, 1) = o.block(0, 0, 3, 1);
}
tVector cSpAlg::GetV(const tSpVec& sv)
{
tVector v = tVector::Zero();
v.block(0, 0, 3, 1) = sv.block(3, 0, 3, 1);
return v;
}
void cSpAlg::SetV(const tVector& v, tSpVec& out_sv)
{
out_sv.block(3, 0, 3, 1) = v.block(0, 0, 3, 1);
}
cSpAlg::tSpTrans cSpAlg::BuildTrans()
{
return BuildTrans(tMatrix::Identity(), tVector::Zero());
}
cSpAlg::tSpTrans cSpAlg::BuildTrans(const tMatrix& E, const tVector& r)
{
tSpTrans X;
SetRad(r, X);
SetRot(E, X);
return X;
}
cSpAlg::tSpTrans cSpAlg::BuildTrans(const tVector& r)
{
return BuildTrans(tMatrix::Identity(), r);
}
cSpAlg::tSpTrans cSpAlg::MatToTrans(const tMatrix& mat)
{
tMatrix E = mat;
tVector r = mat.col(3);
r[3] = 0;
r = -E.transpose() * r;
return BuildTrans(E, r);
}
tMatrix cSpAlg::TransToMat(const tSpTrans& X)
{
tMatrix E = GetRot(X);
tVector r = GetRad(X);
tMatrix m = E;
m.col(3) = -E * r;
m(3, 3) = 1;
return m;
}
cSpAlg::tSpMat cSpAlg::BuildSpatialMatM(const tSpTrans& X)
{
tSpMat m = tSpMat::Zero();
tMatrix E = GetRot(X);
tVector r = GetRad(X);
tMatrix Er = E * cMathUtil::CrossMat(r);
m.block(0, 0, 3, 3) = E.block(0, 0, 3, 3);
m.block(3, 3, 3, 3) = E.block(0, 0, 3, 3);
m.block(3, 0, 3, 3) = -Er.block(0, 0, 3, 3);
return m;
}
cSpAlg::tSpMat cSpAlg::BuildSpatialMatF(const tSpTrans& X)
{
tSpMat m = tSpMat::Zero();
tMatrix E = GetRot(X);
tVector r = GetRad(X);
tMatrix Er = E * cMathUtil::CrossMat(r);
m.block(0, 0, 3, 3) = E.block(0, 0, 3, 3);
m.block(3, 3, 3, 3) = E.block(0, 0, 3, 3);
m.block(0, 3, 3, 3) = -Er.block(0, 0, 3, 3);
return m;
}
cSpAlg::tSpTrans cSpAlg::InvTrans(const tSpTrans& X)
{
tMatrix E = GetRot(X);
tVector r = GetRad(X);
tSpTrans inv_X = BuildTrans(E.transpose(), -E * r);
return inv_X;
}
tMatrix cSpAlg::GetRot(const tSpTrans& X)
{
tMatrix E = tMatrix::Zero();
E.block(0, 0, 3, 3) = X.block(0, 0, 3, 3);
return E;
}
void cSpAlg::SetRot(const tMatrix& E, tSpTrans& out_X)
{
out_X.block(0, 0, 3, 3) = E.block(0, 0, 3, 3);
}
tVector cSpAlg::GetRad(const tSpTrans& X)
{
tVector r = tVector::Zero();
r.block(0, 0, 3, 1) = X.block(0, 3, 3, 1);
return r;
}
void cSpAlg::SetRad(const tVector& r, tSpTrans& out_X)
{
out_X.block(0, 3, 3, 1) = r.block(0, 0, 3, 1);
}
cSpAlg::tSpVec cSpAlg::ApplyTransM(const tSpTrans& X, const tSpVec& sv)
{
tMatrix E = GetRot(X);
tVector r = GetRad(X);
tVector o0 = GetOmega(sv);
tVector v0 = GetV(sv);
tVector o1 = E * o0;
tVector v1 = E * (v0 - r.cross3(o0));
tSpVec new_vec = BuildSV(o1, v1);
return new_vec;
}
cSpAlg::tSpVec cSpAlg::ApplyTransF(const tSpTrans& X, const tSpVec& sv)
{
tMatrix E = GetRot(X);
tVector r = GetRad(X);
tVector o0 = GetOmega(sv);
tVector v0 = GetV(sv);
tVector o1 = E * (o0 - r.cross3(v0));
tVector v1 = E * v0;
tSpVec new_vec = BuildSV(o1, v1);
return new_vec;
}
Eigen::MatrixXd cSpAlg::ApplyTransM(const tSpTrans& X, const Eigen::MatrixXd& sm)
{
assert(sm.rows() == gSpVecSize);
Eigen::MatrixXd result(gSpVecSize, sm.cols());
for (int i = 0; i < sm.cols(); ++i)
{
const tSpVec& curr_sv = sm.col(i);
result.col(i) = ApplyTransM(X, curr_sv);
}
return result;
}
Eigen::MatrixXd cSpAlg::ApplyTransF(const tSpTrans& X, const Eigen::MatrixXd& sm)
{
assert(sm.rows() == gSpVecSize);
Eigen::MatrixXd result(gSpVecSize, sm.cols());
for (int i = 0; i < sm.cols(); ++i)
{
const tSpVec& curr_sv = sm.col(i);
result.col(i) = ApplyTransF(X, curr_sv);
}
return result;
}
cSpAlg::tSpVec cSpAlg::ApplyInvTransM(const tSpTrans& X, const tSpVec& sv)
{
tMatrix E = GetRot(X);
tVector r = GetRad(X);
tVector o0 = GetOmega(sv);
tVector v0 = GetV(sv);
tVector o1 = E.transpose() * o0;
tVector v1 = E.transpose() * v0 + r.cross3(E.transpose() * o0);
tSpVec new_vec = BuildSV(o1, v1);
return new_vec;
}
cSpAlg::tSpVec cSpAlg::ApplyInvTransF(const tSpTrans& X, const tSpVec& sv)
{
tMatrix E = GetRot(X);
tVector r = GetRad(X);
tVector o0 = GetOmega(sv);
tVector v0 = GetV(sv);
tVector o1 = E.transpose() * o0 + r.cross3(E.transpose() * v0);
tVector v1 = E.transpose() * v0;
tSpVec new_vec = BuildSV(o1, v1);
return new_vec;
}
Eigen::MatrixXd cSpAlg::ApplyInvTransM(const tSpTrans& X, const Eigen::MatrixXd& sm)
{
assert(sm.rows() == gSpVecSize);
Eigen::MatrixXd result(sm.rows(), sm.cols());
for (int i = 0; i < sm.cols(); ++i)
{
const tSpVec& curr_sv = sm.col(i);
result.col(i) = ApplyInvTransM(X, curr_sv);
}
return result;
}
Eigen::MatrixXd cSpAlg::ApplyInvTransF(const tSpTrans& X, const Eigen::MatrixXd& sm)
{
assert(sm.rows() == gSpVecSize);
Eigen::MatrixXd result(sm.rows(), sm.cols());
for (int i = 0; i < sm.cols(); ++i)
{
const tSpVec& curr_sv = sm.col(i);
result.col(i) = ApplyInvTransF(X, curr_sv);
}
return result;
}
cSpAlg::tSpTrans cSpAlg::CompTrans(const tSpTrans& X0, const tSpTrans& X1)
{
tMatrix E0 = GetRot(X0);
tMatrix E1 = GetRot(X1);
tVector r0 = GetRad(X0);
tVector r1 = GetRad(X1);
tSpTrans X = BuildTrans(E0 * E1, r1 + E1.transpose() * r0);
return X;
}
cSpAlg::tSpTrans cSpAlg::GetTrans(const Eigen::MatrixXd& trans_arr, int j)
{
assert(trans_arr.rows() >= gSVTransRows);
assert((trans_arr.rows() % gSVTransRows) == 0);
assert(trans_arr.cols() == gSVTransCols);
int row_idx = j * gSVTransRows;
assert(row_idx <= trans_arr.rows() - gSVTransRows);
tSpTrans X = trans_arr.block(row_idx, 0, gSVTransRows, gSVTransCols);
return X;
}

68
examples/SharedMemory/plugins/stablePDPlugin/SpAlg.h Normal file
View File

@@ -0,0 +1,68 @@
#pragma once
#include "MathUtil.h"
// spatial algebra util
class cSpAlg
{
public:
const static int gSpVecSize = 6;
const static int gSVTransRows = 3;
const static int gSVTransCols = 4;
typedef Eigen::Matrix<double, gSVTransRows, gSVTransCols> tSpTrans;
typedef Eigen::Matrix<double, gSpVecSize, 1> tSpVec;
typedef Eigen::Matrix<double, gSpVecSize, gSpVecSize> tSpMat;
static tSpVec ConvertCoordM(const tSpVec& m0, const tVector& origin0, const tVector& origin1);
// rows of R shoold be the basis for the coordinate frame centered ar origin
static tSpVec ConvertCoordM(const tSpVec& m0, const tVector& origin0, const tMatrix& R0, const tVector& origin1, const tMatrix& R1);
// R is a rotation from coord frame 0 to coord frame 1
static tSpVec ConvertCoordM(const tSpVec& m0, const tVector& origin0, const tVector& origin1, const tMatrix& R);
static tSpVec ConvertCoordF(const tSpVec& m0, const tVector& origin0, const tVector& origin1);
static tSpVec ConvertCoordF(const tSpVec& m0, const tVector& origin0, const tMatrix& R0, const tVector& origin1, const tMatrix& R1);
static tSpVec ConvertCoordF(const tSpVec& m0, const tVector& origin0, const tVector& origin1, const tMatrix& R);
static tSpVec CrossM(const tSpVec& sv, const tSpVec& m);
static Eigen::MatrixXd CrossMs(const tSpVec& sv, const Eigen::MatrixXd& ms);
static tSpVec CrossF(const tSpVec& sv, const tSpVec& f);
static Eigen::MatrixXd CrossFs(const tSpVec& sv, const Eigen::MatrixXd& fs);
// Spatial Vector methods
static tSpVec BuildSV(const tVector& v);
static tSpVec BuildSV(const tVector& o, const tVector& v);
static tVector GetOmega(const tSpVec& sv);
static void SetOmega(const tVector& o, tSpVec& out_sv);
static tVector GetV(const tSpVec& sv);
static void SetV(const tVector& v, tSpVec& out_sv);
// tSpTrans methods
static tSpTrans BuildTrans();
static tSpTrans BuildTrans(const tMatrix& E, const tVector& r);
static tSpTrans BuildTrans(const tVector& r);
static tSpTrans MatToTrans(const tMatrix& mat);
static tMatrix TransToMat(const tSpTrans& X);
static tSpMat BuildSpatialMatM(const tSpTrans& X);
static tSpMat BuildSpatialMatF(const tSpTrans& X);
static tSpTrans InvTrans(const tSpTrans& X);
static tMatrix GetRot(const tSpTrans& X);
static void SetRot(const tMatrix& E, tSpTrans& out_X);
static tVector GetRad(const tSpTrans& X);
static void SetRad(const tVector& r, tSpTrans& out_X);
static tSpVec ApplyTransM(const tSpTrans& X, const tSpVec& sv);
static tSpVec ApplyTransF(const tSpTrans& X, const tSpVec& sv);
static Eigen::MatrixXd ApplyTransM(const tSpTrans& X, const Eigen::MatrixXd& sm);
static Eigen::MatrixXd ApplyTransF(const tSpTrans& X, const Eigen::MatrixXd& sm);
static tSpVec ApplyInvTransM(const tSpTrans& X, const tSpVec& sv);
static tSpVec ApplyInvTransF(const tSpTrans& X, const tSpVec& sv);
static Eigen::MatrixXd ApplyInvTransM(const tSpTrans& X, const Eigen::MatrixXd& sm);
static Eigen::MatrixXd ApplyInvTransF(const tSpTrans& X, const Eigen::MatrixXd& sm);
static tSpTrans CompTrans(const tSpTrans& X0, const tSpTrans& X1);
// extract a tSpTrans from a matrix presentating a stack of transforms
static tSpTrans GetTrans(const Eigen::MatrixXd& trans_arr, int j);
};

15
examples/SharedMemory/plugins/tinyRendererPlugin/TinyRendererVisualShapeConverter.cpp
View File

@@ -66,6 +66,7 @@ struct TinyRendererVisualShapeConverterInternalData
// Maps bodyUniqueId to a list of visual shapes belonging to the body.
btHashMap<btHashInt, btAlignedObjectArray<b3VisualShapeData> > m_visualShapesMap;
int m_uidGenerator;
int m_upAxis;
int m_swWidth;
int m_swHeight;
@@ -92,6 +93,7 @@ struct TinyRendererVisualShapeConverterInternalData
TinyRendererVisualShapeConverterInternalData()
: m_upAxis(2),
m_uidGenerator(1),
m_swWidth(START_WIDTH),
m_swHeight(START_HEIGHT),
m_rgbColorBuffer(START_WIDTH, START_HEIGHT, TGAImage::RGB),
@@ -606,12 +608,13 @@ static btVector4 sColors[4] =
//btVector4(1,1,0,1),
};
void TinyRendererVisualShapeConverter::convertVisualShapes(
int TinyRendererVisualShapeConverter::convertVisualShapes(
int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame,
const UrdfLink* linkPtr, const UrdfModel* model, int collisionObjectUniqueId,
const UrdfLink* linkPtr, const UrdfModel* model, int unused,
int bodyUniqueId, struct CommonFileIOInterface* fileIO)
{
int uniqueId = m_data->m_uidGenerator++;
btAssert(linkPtr); // TODO: remove if (not doing it now, because diff will be 50+ lines)
if (linkPtr)
{
@@ -699,12 +702,12 @@ void TinyRendererVisualShapeConverter::convertVisualShapes(
}
}
TinyRendererObjectArray** visualsPtr = m_data->m_swRenderInstances[collisionObjectUniqueId];
TinyRendererObjectArray** visualsPtr = m_data->m_swRenderInstances[uniqueId];
if (visualsPtr == 0)
{
m_data->m_swRenderInstances.insert(collisionObjectUniqueId, new TinyRendererObjectArray);
m_data->m_swRenderInstances.insert(uniqueId, new TinyRendererObjectArray);
}
visualsPtr = m_data->m_swRenderInstances[collisionObjectUniqueId];
visualsPtr = m_data->m_swRenderInstances[uniqueId];
btAssert(visualsPtr);
TinyRendererObjectArray* visuals = *visualsPtr;
@@ -778,6 +781,8 @@ void TinyRendererVisualShapeConverter::convertVisualShapes(
shapes->push_back(visualShape);
}
}
return uniqueId;
}
int TinyRendererVisualShapeConverter::getNumVisualShapes(int bodyUniqueId)

2
examples/SharedMemory/plugins/tinyRendererPlugin/TinyRendererVisualShapeConverter.h
View File

@@ -11,7 +11,7 @@ struct TinyRendererVisualShapeConverter : public UrdfRenderingInterface
virtual ~TinyRendererVisualShapeConverter();
virtual void convertVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame, const UrdfLink* linkPtr, const UrdfModel* model, int collisionObjectUniqueId, int bodyUniqueId, struct CommonFileIOInterface* fileIO);
virtual int convertVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame, const UrdfLink* linkPtr, const UrdfModel* model, int collisionObjectUniqueId, int bodyUniqueId, struct CommonFileIOInterface* fileIO);
virtual int getNumVisualShapes(int bodyUniqueId);