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

Merge branch 'master' of https://github.com/erwincoumans/bullet3

Browse Source 
# Conflicts:
#	examples/SharedMemory/PhysicsServerCommandProcessor.cpp
This commit is contained in:
erwincoumans
2018-06-01 09:35:15 -07:00
parent 2d40a18315 00c8f8f68e
commit 92579f9196
18 changed files with 296 additions and 119 deletions
Download Patch File Download Diff File Expand all files Collapse all files

106
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@@ -1516,7 +1516,7 @@ B3_SHARED_API int b3CreateBoxCommandSetStartOrientation(b3SharedMemoryCommandHan
return 0;
}
B3_SHARED_API b3SharedMemoryCommandHandle b3CreatePoseCommandInit(b3PhysicsClientHandle physClient, int bodyIndex)
B3_SHARED_API b3SharedMemoryCommandHandle b3CreatePoseCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId)
{
PhysicsClient* cl = (PhysicsClient* ) physClient;
@@ -1526,7 +1526,7 @@ B3_SHARED_API b3SharedMemoryCommandHandle b3CreatePoseCommandInit(b3PhysicsClien
b3Assert(command);
command->m_type = CMD_INIT_POSE;
command->m_updateFlags =0;
command->m_initPoseArgs.m_bodyUniqueId = bodyIndex;
command->m_initPoseArgs.m_bodyUniqueId = bodyUniqueId;
//a bit slow, initialing the full range to zero...
for (int i=0;i<MAX_DEGREE_OF_FREEDOM;i++)
{
@@ -2037,16 +2037,54 @@ B3_SHARED_API int b3GetBodyInfo(b3PhysicsClientHandle physClient, int bodyUnique
B3_SHARED_API int b3GetNumJoints(b3PhysicsClientHandle physClient, int bodyId)
B3_SHARED_API int b3GetNumJoints(b3PhysicsClientHandle physClient, int bodyUniqueId)
{
PhysicsClient* cl = (PhysicsClient* ) physClient;
return cl->getNumJoints(bodyId);
return cl->getNumJoints(bodyUniqueId);
}
B3_SHARED_API int b3GetJointInfo(b3PhysicsClientHandle physClient, int bodyIndex, int jointIndex, struct b3JointInfo* info)
B3_SHARED_API int b3ComputeDofCount(b3PhysicsClientHandle physClient, int bodyUniqueId)
{
int nj = b3GetNumJoints(physClient, bodyUniqueId);
int j=0;
int dofCountOrg = 0;
for (j=0;j<nj;j++)
{
struct b3JointInfo info;
b3GetJointInfo(physClient, bodyUniqueId, j, &info);
switch (info.m_jointType)
{
case eRevoluteType:
{
dofCountOrg+=1;
break;
}
case ePrismaticType:
{
dofCountOrg+=1;
break;
}
case eSphericalType:
{
return -1;
}
case ePlanarType:
{
return -2;
}
default:
{
//fixed joint has 0-dof and at the moment, we don't deal with planar, spherical etc
}
}
}
return dofCountOrg;
}
B3_SHARED_API int b3GetJointInfo(b3PhysicsClientHandle physClient, int bodyUniqueId, int jointIndex, struct b3JointInfo* info)
{
PhysicsClient* cl = (PhysicsClient* ) physClient;
return cl->getJointInfo(bodyIndex, jointIndex, *info);
return cl->getJointInfo(bodyUniqueId, jointIndex, *info);
}
@@ -2389,7 +2427,7 @@ B3_SHARED_API int b3ChangeDynamicsInfoSetContactProcessingThreshold(b3SharedMemo
B3_SHARED_API b3SharedMemoryCommandHandle b3InitCreateUserConstraintCommand(b3PhysicsClientHandle physClient, int parentBodyIndex, int parentJointIndex, int childBodyIndex, int childJointIndex, struct b3JointInfo* info)
B3_SHARED_API b3SharedMemoryCommandHandle b3InitCreateUserConstraintCommand(b3PhysicsClientHandle physClient, int parentBodyUniqueId, int parentJointIndex, int childBodyUniqueId, int childJointIndex, struct b3JointInfo* info)
{
PhysicsClient* cl = (PhysicsClient* ) physClient;
b3Assert(cl);
@@ -2400,9 +2438,9 @@ B3_SHARED_API b3SharedMemoryCommandHandle b3InitCreateUserConstraintCommand(b3Ph
command->m_type = CMD_USER_CONSTRAINT;
command->m_updateFlags = USER_CONSTRAINT_ADD_CONSTRAINT;
command->m_userConstraintArguments.m_parentBodyIndex = parentBodyIndex;
command->m_userConstraintArguments.m_parentBodyIndex = parentBodyUniqueId;
command->m_userConstraintArguments.m_parentJointIndex = parentJointIndex;
command->m_userConstraintArguments.m_childBodyIndex = childBodyIndex;
command->m_userConstraintArguments.m_childBodyIndex = childBodyUniqueId;
command->m_userConstraintArguments.m_childJointIndex = childJointIndex;
for (int i = 0; i < 7; ++i) {
command->m_userConstraintArguments.m_parentFrame[i] = info->m_parentFrame[i];
@@ -3742,7 +3780,7 @@ B3_SHARED_API void b3ApplyExternalTorque(b3SharedMemoryCommandHandle commandHand
///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,
B3_SHARED_API b3SharedMemoryCommandHandle b3CalculateInverseDynamicsCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId,
const double* jointPositionsQ, const double* jointVelocitiesQdot, const double* jointAccelerations)
{
PhysicsClient* cl = (PhysicsClient*)physClient;
@@ -3753,8 +3791,8 @@ B3_SHARED_API b3SharedMemoryCommandHandle b3CalculateInverseDynamicsCommandInit(
command->m_type = CMD_CALCULATE_INVERSE_DYNAMICS;
command->m_updateFlags = 0;
command->m_calculateInverseDynamicsArguments.m_bodyUniqueId = bodyIndex;
int numJoints = cl->getNumJoints(bodyIndex);
command->m_calculateInverseDynamicsArguments.m_bodyUniqueId = bodyUniqueId;
int numJoints = cl->getNumJoints(bodyUniqueId);
for (int i = 0; i < numJoints;i++)
{
command->m_calculateInverseDynamicsArguments.m_jointPositionsQ[i] = jointPositionsQ[i];
@@ -3799,7 +3837,7 @@ B3_SHARED_API int b3GetStatusInverseDynamicsJointForces(b3SharedMemoryStatusHand
return true;
}
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 b3SharedMemoryCommandHandle b3CalculateJacobianCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId, int linkIndex, const double* localPosition, const double* jointPositionsQ, const double* jointVelocitiesQdot, const double* jointAccelerations)
{
PhysicsClient* cl = (PhysicsClient*)physClient;
b3Assert(cl);
@@ -3809,12 +3847,12 @@ B3_SHARED_API b3SharedMemoryCommandHandle b3CalculateJacobianCommandInit(b3Physi
command->m_type = CMD_CALCULATE_JACOBIAN;
command->m_updateFlags = 0;
command->m_calculateJacobianArguments.m_bodyUniqueId = bodyIndex;
command->m_calculateJacobianArguments.m_bodyUniqueId = bodyUniqueId;
command->m_calculateJacobianArguments.m_linkIndex = linkIndex;
command->m_calculateJacobianArguments.m_localPosition[0] = localPosition[0];
command->m_calculateJacobianArguments.m_localPosition[1] = localPosition[1];
command->m_calculateJacobianArguments.m_localPosition[2] = localPosition[2];
int numJoints = cl->getNumJoints(bodyIndex);
int numJoints = cl->getNumJoints(bodyUniqueId);
for (int i = 0; i < numJoints;i++)
{
command->m_calculateJacobianArguments.m_jointPositionsQ[i] = jointPositionsQ[i];
@@ -3859,7 +3897,7 @@ B3_SHARED_API int b3GetStatusJacobian(b3SharedMemoryStatusHandle statusHandle, i
return true;
}
B3_SHARED_API b3SharedMemoryCommandHandle b3CalculateMassMatrixCommandInit(b3PhysicsClientHandle physClient, int bodyIndex, const double* jointPositionsQ)
B3_SHARED_API b3SharedMemoryCommandHandle b3CalculateMassMatrixCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId, const double* jointPositionsQ)
{
PhysicsClient* cl = (PhysicsClient*)physClient;
b3Assert(cl);
@@ -3869,7 +3907,7 @@ B3_SHARED_API b3SharedMemoryCommandHandle b3CalculateMassMatrixCommandInit(b3Phy
command->m_type = CMD_CALCULATE_MASS_MATRIX;
command->m_updateFlags = 0;
int numJoints = cl->getNumJoints(bodyIndex);
int numJoints = cl->getNumJoints(bodyUniqueId);
for (int i = 0; i < numJoints; i++)
{
command->m_calculateMassMatrixArguments.m_jointPositionsQ[i] = jointPositionsQ[i];
@@ -3905,7 +3943,7 @@ B3_SHARED_API int b3GetStatusMassMatrix(b3PhysicsClientHandle physClient, b3Shar
}
///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 b3SharedMemoryCommandHandle b3CalculateInverseKinematicsCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId)
{
PhysicsClient* cl = (PhysicsClient*)physClient;
b3Assert(cl);
@@ -3915,7 +3953,7 @@ B3_SHARED_API b3SharedMemoryCommandHandle b3CalculateInverseKinematicsCommandIni
command->m_type = CMD_CALCULATE_INVERSE_KINEMATICS;
command->m_updateFlags = 0;
command->m_calculateInverseKinematicsArguments.m_bodyUniqueId = bodyIndex;
command->m_calculateInverseKinematicsArguments.m_bodyUniqueId = bodyUniqueId;
return (b3SharedMemoryCommandHandle)command;
@@ -4008,6 +4046,36 @@ B3_SHARED_API void b3CalculateInverseKinematicsPosOrnWithNullSpaceVel(b3SharedMe
}
B3_SHARED_API void b3CalculateInverseKinematicsSetCurrentPositions(b3SharedMemoryCommandHandle commandHandle, int numDof, const double* currentJointPositions)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_CALCULATE_INVERSE_KINEMATICS);
command->m_updateFlags |= IK_HAS_CURRENT_JOINT_POSITIONS;
for (int i = 0; i < numDof; ++i)
{
command->m_calculateInverseKinematicsArguments.m_currentPositions[i] = currentJointPositions[i];
}
}
B3_SHARED_API void b3CalculateInverseKinematicsSetMaxNumIterations(b3SharedMemoryCommandHandle commandHandle, int maxNumIterations)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_CALCULATE_INVERSE_KINEMATICS);
command->m_updateFlags |= IK_HAS_MAX_ITERATIONS;
command->m_calculateInverseKinematicsArguments.m_maxNumIterations = maxNumIterations;
}
B3_SHARED_API void b3CalculateInverseKinematicsSetResidualThreshold(b3SharedMemoryCommandHandle commandHandle, double residualThreshold)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_CALCULATE_INVERSE_KINEMATICS);
command->m_updateFlags |= IK_HAS_RESIDUAL_THRESHOLD;
command->m_calculateInverseKinematicsArguments.m_residualThreshold = residualThreshold;
}
B3_SHARED_API void b3CalculateInverseKinematicsSetJointDamping(b3SharedMemoryCommandHandle commandHandle, int numDof, const double* jointDampingCoeff)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;

25
examples/SharedMemory/PhysicsClientC_API.h
View File

@@ -104,10 +104,14 @@ B3_SHARED_API int b3GetBodyUniqueId(b3PhysicsClientHandle physClient, int serial
B3_SHARED_API int b3GetBodyInfo(b3PhysicsClientHandle physClient, int bodyUniqueId, struct b3BodyInfo* info);
///give a unique body index (after loading the body) return the number of joints.
B3_SHARED_API int b3GetNumJoints(b3PhysicsClientHandle physClient, int bodyId);
B3_SHARED_API int b3GetNumJoints(b3PhysicsClientHandle physClient, int bodyUniqueId);
///compute the number of degrees of freedom for this body.
///Return -1 for unsupported spherical joint, -2 for unsupported planar joint.
B3_SHARED_API int b3ComputeDofCount(b3PhysicsClientHandle physClient, int bodyUniqueId);
///given a body and joint index, return the joint information. See b3JointInfo in SharedMemoryPublic.h
B3_SHARED_API int b3GetJointInfo(b3PhysicsClientHandle physClient, int bodyIndex, int jointIndex, struct b3JointInfo* info);
B3_SHARED_API int b3GetJointInfo(b3PhysicsClientHandle physClient, int bodyUniqueId, int jointIndex, struct b3JointInfo* info);
B3_SHARED_API b3SharedMemoryCommandHandle b3GetDynamicsInfoCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId, int linkIndex);
///given a body unique id and link index, return the dynamics information. See b3DynamicsInfo in SharedMemoryPublic.h
@@ -128,7 +132,7 @@ B3_SHARED_API int b3ChangeDynamicsInfoSetFrictionAnchor(b3SharedMemoryCommandHan
B3_SHARED_API int b3ChangeDynamicsInfoSetCcdSweptSphereRadius(b3SharedMemoryCommandHandle commandHandle,int bodyUniqueId,int linkIndex, double ccdSweptSphereRadius);
B3_SHARED_API int b3ChangeDynamicsInfoSetContactProcessingThreshold(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double contactProcessingThreshold);
B3_SHARED_API b3SharedMemoryCommandHandle b3InitCreateUserConstraintCommand(b3PhysicsClientHandle physClient, int parentBodyIndex, int parentJointIndex, int childBodyIndex, int childJointIndex, struct b3JointInfo* info);
B3_SHARED_API b3SharedMemoryCommandHandle b3InitCreateUserConstraintCommand(b3PhysicsClientHandle physClient, int parentBodyUniqueId, int parentJointIndex, int childBodyUniqueId, int childJointIndex, struct b3JointInfo* info);
///return a unique id for the user constraint, after successful creation, or -1 for an invalid constraint id
B3_SHARED_API int b3GetStatusUserConstraintUniqueId(b3SharedMemoryStatusHandle statusHandle);
@@ -350,26 +354,26 @@ 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,
B3_SHARED_API b3SharedMemoryCommandHandle b3CalculateInverseDynamicsCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId,
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 b3SharedMemoryCommandHandle b3CalculateJacobianCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId, 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 b3SharedMemoryCommandHandle b3CalculateMassMatrixCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId, const double* jointPositionsQ);
///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);
///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 b3SharedMemoryCommandHandle b3CalculateInverseKinematicsCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId);
B3_SHARED_API void b3CalculateInverseKinematicsAddTargetPurePosition(b3SharedMemoryCommandHandle commandHandle, int endEffectorLinkIndex, const double targetPosition[/*3*/]);
B3_SHARED_API void b3CalculateInverseKinematicsAddTargetPositionWithOrientation(b3SharedMemoryCommandHandle commandHandle, int endEffectorLinkIndex, const double targetPosition[/*3*/], const double targetOrientation[/*4*/]);
B3_SHARED_API void b3CalculateInverseKinematicsPosWithNullSpaceVel(b3SharedMemoryCommandHandle commandHandle, int numDof, int endEffectorLinkIndex, const double targetPosition[/*3*/], const double* lowerLimit, const double* upperLimit, const double* jointRange, const double* restPose);
@@ -381,6 +385,11 @@ B3_SHARED_API int b3GetStatusInverseKinematicsJointPositions(b3SharedMemoryStatu
int* dofCount,
double* jointPositions);
B3_SHARED_API void b3CalculateInverseKinematicsSetCurrentPositions(b3SharedMemoryCommandHandle commandHandle, int numDof, const double* currentJointPositions);
B3_SHARED_API void b3CalculateInverseKinematicsSetMaxNumIterations(b3SharedMemoryCommandHandle commandHandle, int maxNumIterations);
B3_SHARED_API void b3CalculateInverseKinematicsSetResidualThreshold(b3SharedMemoryCommandHandle commandHandle, double residualThreshold);
B3_SHARED_API b3SharedMemoryCommandHandle b3LoadSdfCommandInit(b3PhysicsClientHandle physClient, const char* sdfFileName);
B3_SHARED_API int b3LoadSdfCommandSetUseMultiBody(b3SharedMemoryCommandHandle commandHandle, int useMultiBody);
B3_SHARED_API int b3LoadSdfCommandSetUseGlobalScaling(b3SharedMemoryCommandHandle commandHandle, double globalScaling);
@@ -476,7 +485,7 @@ B3_SHARED_API int b3CreateBoxCommandSetColorRGBA(b3SharedMemoryCommandHandle com
///b3CreatePoseCommandInit will initialize (teleport) the pose of a body/robot. You can individually set the base position,
///base orientation and joint angles. This will set all velocities of base and joints to zero.
///This is not a robot control command using actuators/joint motors, but manual repositioning the robot.
B3_SHARED_API b3SharedMemoryCommandHandle b3CreatePoseCommandInit(b3PhysicsClientHandle physClient, int bodyIndex);
B3_SHARED_API b3SharedMemoryCommandHandle b3CreatePoseCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId);
B3_SHARED_API int b3CreatePoseCommandSetBasePosition(b3SharedMemoryCommandHandle commandHandle, double startPosX,double startPosY,double startPosZ);
B3_SHARED_API int b3CreatePoseCommandSetBaseOrientation(b3SharedMemoryCommandHandle commandHandle, double startOrnX,double startOrnY,double startOrnZ, double startOrnW);
B3_SHARED_API int b3CreatePoseCommandSetBaseLinearVelocity(b3SharedMemoryCommandHandle commandHandle, double linVel[/*3*/]);

3
examples/SharedMemory/SharedMemoryCommands.h
View File

@@ -695,6 +695,9 @@ struct CalculateInverseKinematicsArgs
double m_jointRange[MAX_DEGREE_OF_FREEDOM];
double m_restPose[MAX_DEGREE_OF_FREEDOM];
double m_jointDamping[MAX_DEGREE_OF_FREEDOM];
double m_currentPositions[MAX_DEGREE_OF_FREEDOM];
int m_maxNumIterations;
double m_residualThreshold;
};
struct CalculateInverseKinematicsResultArgs

4
examples/SharedMemory/SharedMemoryPublic.h
View File

@@ -628,7 +628,9 @@ enum EnumCalculateInverseKinematicsFlags
IK_HAS_TARGET_ORIENTATION=32,
IK_HAS_NULL_SPACE_VELOCITY=64,
IK_HAS_JOINT_DAMPING=128,
//IK_HAS_CURRENT_JOINT_POSITIONS=256,//not used yet
IK_HAS_CURRENT_JOINT_POSITIONS=256,
IK_HAS_MAX_ITERATIONS=512,
IK_HAS_RESIDUAL_THRESHOLD = 1024,
};
enum b3ConfigureDebugVisualizerEnum