AuroraMiddleware/bullet3
1
0
Fork 0
mirror of https://github.com/bulletphysics/bullet3 synced 2025-01-18 21:10:05 +00:00
Code Issues Releases Wiki Activity

Add calculateMass to shared memory interface.

Browse Source
This commit is contained in:
Jeffrey Bingham 2017-09-30 13:39:11 -07:00
parent 31ee8bf8bc
commit 7dfed65287
4 changed files with 68 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

44
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@ -3377,6 +3377,50 @@ B3_SHARED_API int b3GetStatusJacobian(b3SharedMemoryStatusHandle statusHandle, i
return true;
}
B3_SHARED_API b3SharedMemoryCommandHandle b3CalculateMassMatrixCommandInit(b3PhysicsClientHandle physClient, int bodyIndex, const double* jointPositionsQ)
{
PhysicsClient* cl = (PhysicsClient*)physClient;
b3Assert(cl);
b3Assert(cl->canSubmitCommand());
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
b3Assert(command);
command->m_type = CMD_CALCULATE_MASS_MATRIX;
command->m_updateFlags = 0;
int numJoints = cl->getNumJoints(bodyIndex);
for (int i = 0; i < numJoints; i++)
{
command->m_calculateMassMatrixArguments.m_jointPositionsQ[i] = jointPositionsQ[i];
}
return (b3SharedMemoryCommandHandle)command;
}
B3_SHARED_API int b3GetStatusMassMatrix(b3SharedMemoryStatusHandle statusHandle, int* dofCount, double* massMatrix)
{
const SharedMemoryStatus* status = (const SharedMemoryStatus*)statusHandle;
btAssert(status->m_type == CMD_CALCULATED_MASS_MATRIX_COMPLETED);
if (status->m_type != CMD_CALCULATED_MASS_MATRIX_COMPLETED)
return false;
if (dofCount)
{
*dofCount = status->m_massMatrixResultArgs.m_dofCount;
}
if (massMatrix)
{
int numElements = status->m_massMatrixResultArgs.m_dofCount * status->m_massMatrixResultArgs.m_dofCount;
for (int i = 0; i < numElements; i++)
{
massMatrix[i] = status->m_massMatrixResultArgs.m_massMatrix[i];
}
}
return true;
}
///compute the joint positions to move the end effector to a desired target using inverse kinematics
B3_SHARED_API b3SharedMemoryCommandHandle b3CalculateInverseKinematicsCommandInit(b3PhysicsClientHandle physClient, int bodyIndex)
{

9
examples/SharedMemory/PhysicsClientC_API.h
View File

@ -310,19 +310,24 @@ B3_SHARED_API void b3LoadMJCFCommandSetFlags(b3SharedMemoryCommandHandle command
///compute the forces to achieve an acceleration, given a state q and qdot using inverse dynamics
B3_SHARED_API b3SharedMemoryCommandHandle b3CalculateInverseDynamicsCommandInit(b3PhysicsClientHandle physClient, int bodyIndex,
const double* jointPositionsQ, const double* jointVelocitiesQdot, const double* jointAccelerations);
B3_SHARED_API int b3GetStatusInverseDynamicsJointForces(b3SharedMemoryStatusHandle statusHandle,
int* bodyUniqueId,
int* dofCount,
double* jointForces);
B3_SHARED_API b3SharedMemoryCommandHandle b3CalculateJacobianCommandInit(b3PhysicsClientHandle physClient, int bodyIndex, int linkIndex, const double* localPosition, const double* jointPositionsQ, const double* jointVelocitiesQdot, const double* jointAccelerations);
B3_SHARED_API int b3GetStatusJacobian(b3SharedMemoryStatusHandle statusHandle,
int* dofCount,
double* linearJacobian,
double* angularJacobian);
B3_SHARED_API b3SharedMemoryCommandHandle b3CalculateMassMatrixCommandInit(b3PhysicsClientHandle physClient, int bodyIndex, const double* jointPositionsQ);
B3_SHARED_API int b3GetStatusMassMatrix(b3SharedMemoryStatusHandle statusHandle,
int* dofCount,
double* massMatrix);
///compute the joint positions to move the end effector to a desired target using inverse kinematics
B3_SHARED_API b3SharedMemoryCommandHandle b3CalculateInverseKinematicsCommandInit(b3PhysicsClientHandle physClient, int bodyIndex);
B3_SHARED_API void b3CalculateInverseKinematicsAddTargetPurePosition(b3SharedMemoryCommandHandle commandHandle, int endEffectorLinkIndex, const double targetPosition[/*3*/]);

14
examples/SharedMemory/SharedMemoryCommands.h
View File

@ -638,6 +638,18 @@ struct CalculateJacobianResultArgs
double m_angularJacobian[3*MAX_DEGREE_OF_FREEDOM];
};
struct CalculateMassMatrixArgs
{
int m_bodyUniqueId;
double m_jointPositionsQ[MAX_DEGREE_OF_FREEDOM];
};
struct CalculateMassMatrixResultArgs
{
int m_dofCount;
double m_massMatrix[MAX_DEGREE_OF_FREEDOM * MAX_DEGREE_OF_FREEDOM];
};
enum EnumCalculateInverseKinematicsFlags
{
IK_HAS_TARGET_POSITION=1,
@ -969,6 +981,7 @@ struct SharedMemoryCommand
struct ExternalForceArgs m_externalForceArguments;
struct CalculateInverseDynamicsArgs m_calculateInverseDynamicsArguments;
struct CalculateJacobianArgs m_calculateJacobianArguments;
struct CalculateMassMatrixArgs m_calculateMassMatrixArguments;
struct b3UserConstraint m_userConstraintArguments;
struct RequestContactDataArgs m_requestContactPointArguments;
struct RequestOverlappingObjectsArgs m_requestOverlappingObjectsArgs;
@ -1043,6 +1056,7 @@ struct SharedMemoryStatus
struct RigidBodyCreateArgs m_rigidBodyCreateArgs;
struct CalculateInverseDynamicsResultArgs m_inverseDynamicsResultArgs;
struct CalculateJacobianResultArgs m_jacobianResultArgs;
struct CalculateMassMatrixResultArgs m_massMatrixResultArgs;
struct SendContactDataArgs m_sendContactPointArgs;
struct SendOverlappingObjectsArgs m_sendOverlappingObjectsArgs;
struct CalculateInverseKinematicsResultArgs m_inverseKinematicsResultArgs;

3
examples/SharedMemory/SharedMemoryPublic.h
View File

@ -43,6 +43,7 @@ enum EnumSharedMemoryClientCommand
CMD_CALCULATE_INVERSE_DYNAMICS,
CMD_CALCULATE_INVERSE_KINEMATICS,
CMD_CALCULATE_JACOBIAN,
CMD_CALCULATE_MASS_MATRIX,
CMD_USER_CONSTRAINT,
CMD_REQUEST_CONTACT_POINT_INFORMATION,
CMD_REQUEST_RAY_CAST_INTERSECTIONS,
@ -120,6 +121,8 @@ enum EnumSharedMemoryServerStatus
CMD_CALCULATED_INVERSE_DYNAMICS_FAILED,
CMD_CALCULATED_JACOBIAN_COMPLETED,
CMD_CALCULATED_JACOBIAN_FAILED,
CMD_CALCULATED_MASS_MATRIX_COMPLETED,
CMD_CALCULATED_MASS_MATRIX_FAILED,
CMD_CONTACT_POINT_INFORMATION_COMPLETED,
CMD_CONTACT_POINT_INFORMATION_FAILED,
CMD_REQUEST_AABB_OVERLAP_COMPLETED,