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

implement accurate inverse kinematics in C++. PyBullet.calculateInverseKinematics gets "maxNumIterations=20", "residualThreshold=1.04" to tune

Browse Source 
allow to provide current joint positions in IK, overriding the body joint positions, also IK target will be in local coordinates.
expose b3ComputeDofCount in C-API
This commit is contained in:
erwincoumans
2018-05-31 16:06:15 -07:00
parent 11d8f069f0
commit edc70582dd
7 changed files with 429 additions and 247 deletions
Download Patch File Download Diff File Expand all files Collapse all files

108
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@@ -1516,7 +1516,7 @@ B3_SHARED_API int b3CreateBoxCommandSetStartOrientation(b3SharedMemoryCommandHan
return 0; return 0;
} }
B3_SHARED_API b3SharedMemoryCommandHandle b3CreatePoseCommandInit(b3PhysicsClientHandle physClient, int bodyIndex) B3_SHARED_API b3SharedMemoryCommandHandle b3CreatePoseCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId)
{ {
PhysicsClient* cl = (PhysicsClient* ) physClient; PhysicsClient* cl = (PhysicsClient* ) physClient;
@@ -1526,7 +1526,7 @@ B3_SHARED_API b3SharedMemoryCommandHandle b3CreatePoseCommandInit(b3PhysicsClien
b3Assert(command); b3Assert(command);
command->m_type = CMD_INIT_POSE; command->m_type = CMD_INIT_POSE;
command->m_updateFlags =0; 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... //a bit slow, initialing the full range to zero...
for (int i=0;i<MAX_DEGREE_OF_FREEDOM;i++) 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; 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; 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; PhysicsClient* cl = (PhysicsClient* ) physClient;
b3Assert(cl); b3Assert(cl);
@@ -2400,9 +2438,9 @@ B3_SHARED_API b3SharedMemoryCommandHandle b3InitCreateUserConstraintCommand(b3Ph
command->m_type = CMD_USER_CONSTRAINT; command->m_type = CMD_USER_CONSTRAINT;
command->m_updateFlags = USER_CONSTRAINT_ADD_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_parentJointIndex = parentJointIndex;
command->m_userConstraintArguments.m_childBodyIndex = childBodyIndex; command->m_userConstraintArguments.m_childBodyIndex = childBodyUniqueId;
command->m_userConstraintArguments.m_childJointIndex = childJointIndex; command->m_userConstraintArguments.m_childJointIndex = childJointIndex;
for (int i = 0; i < 7; ++i) { for (int i = 0; i < 7; ++i) {
command->m_userConstraintArguments.m_parentFrame[i] = info->m_parentFrame[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 ///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) const double* jointPositionsQ, const double* jointVelocitiesQdot, const double* jointAccelerations)
{ {
PhysicsClient* cl = (PhysicsClient*)physClient; PhysicsClient* cl = (PhysicsClient*)physClient;
@@ -3753,8 +3791,8 @@ B3_SHARED_API b3SharedMemoryCommandHandle b3CalculateInverseDynamicsCommandInit(
command->m_type = CMD_CALCULATE_INVERSE_DYNAMICS; command->m_type = CMD_CALCULATE_INVERSE_DYNAMICS;
command->m_updateFlags = 0; command->m_updateFlags = 0;
command->m_calculateInverseDynamicsArguments.m_bodyUniqueId = bodyIndex; command->m_calculateInverseDynamicsArguments.m_bodyUniqueId = bodyUniqueId;
int numJoints = cl->getNumJoints(bodyIndex); int numJoints = cl->getNumJoints(bodyUniqueId);
for (int i = 0; i < numJoints;i++) for (int i = 0; i < numJoints;i++)
{ {
command->m_calculateInverseDynamicsArguments.m_jointPositionsQ[i] = jointPositionsQ[i]; command->m_calculateInverseDynamicsArguments.m_jointPositionsQ[i] = jointPositionsQ[i];
@@ -3799,7 +3837,7 @@ B3_SHARED_API int b3GetStatusInverseDynamicsJointForces(b3SharedMemoryStatusHand
return true; 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; PhysicsClient* cl = (PhysicsClient*)physClient;
b3Assert(cl); b3Assert(cl);
@@ -3809,12 +3847,12 @@ B3_SHARED_API b3SharedMemoryCommandHandle b3CalculateJacobianCommandInit(b3Physi
command->m_type = CMD_CALCULATE_JACOBIAN; command->m_type = CMD_CALCULATE_JACOBIAN;
command->m_updateFlags = 0; 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_linkIndex = linkIndex;
command->m_calculateJacobianArguments.m_localPosition[0] = localPosition[0]; command->m_calculateJacobianArguments.m_localPosition[0] = localPosition[0];
command->m_calculateJacobianArguments.m_localPosition[1] = localPosition[1]; command->m_calculateJacobianArguments.m_localPosition[1] = localPosition[1];
command->m_calculateJacobianArguments.m_localPosition[2] = localPosition[2]; 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++) for (int i = 0; i < numJoints;i++)
{ {
command->m_calculateJacobianArguments.m_jointPositionsQ[i] = jointPositionsQ[i]; command->m_calculateJacobianArguments.m_jointPositionsQ[i] = jointPositionsQ[i];
@@ -3859,7 +3897,7 @@ B3_SHARED_API int b3GetStatusJacobian(b3SharedMemoryStatusHandle statusHandle, i
return true; 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; PhysicsClient* cl = (PhysicsClient*)physClient;
b3Assert(cl); b3Assert(cl);
@@ -3869,7 +3907,7 @@ B3_SHARED_API b3SharedMemoryCommandHandle b3CalculateMassMatrixCommandInit(b3Phy
command->m_type = CMD_CALCULATE_MASS_MATRIX; command->m_type = CMD_CALCULATE_MASS_MATRIX;
command->m_updateFlags = 0; command->m_updateFlags = 0;
int numJoints = cl->getNumJoints(bodyIndex); int numJoints = cl->getNumJoints(bodyUniqueId);
for (int i = 0; i < numJoints; i++) for (int i = 0; i < numJoints; i++)
{ {
command->m_calculateMassMatrixArguments.m_jointPositionsQ[i] = jointPositionsQ[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 ///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; PhysicsClient* cl = (PhysicsClient*)physClient;
b3Assert(cl); b3Assert(cl);
@@ -3915,7 +3953,7 @@ B3_SHARED_API b3SharedMemoryCommandHandle b3CalculateInverseKinematicsCommandIni
command->m_type = CMD_CALCULATE_INVERSE_KINEMATICS; command->m_type = CMD_CALCULATE_INVERSE_KINEMATICS;
command->m_updateFlags = 0; command->m_updateFlags = 0;
command->m_calculateInverseKinematicsArguments.m_bodyUniqueId = bodyIndex; command->m_calculateInverseKinematicsArguments.m_bodyUniqueId = bodyUniqueId;
return (b3SharedMemoryCommandHandle)command; 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) B3_SHARED_API void b3CalculateInverseKinematicsSetJointDamping(b3SharedMemoryCommandHandle commandHandle, int numDof, const double* jointDampingCoeff)
{ {
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
@@ -4513,7 +4581,7 @@ B3_SHARED_API double b3GetTimeOut(b3PhysicsClientHandle physClient)
return -1; return -1;
} }
B3_SHARED_API b3SharedMemoryCommandHandle b3SetAdditionalSearchPath(b3PhysicsClientHandle physClient, char* path) B3_SHARED_API b3SharedMemoryCommandHandle b3SetAdditionalSearchPath(b3PhysicsClientHandle physClient, const char* path)
{ {
PhysicsClient* cl = (PhysicsClient*)physClient; PhysicsClient* cl = (PhysicsClient*)physClient;
b3Assert(cl); b3Assert(cl);

27
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); 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. ///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 ///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); 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 ///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 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 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 ///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); 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 ///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); const double* jointPositionsQ, const double* jointVelocitiesQdot, const double* jointAccelerations);
B3_SHARED_API int b3GetStatusInverseDynamicsJointForces(b3SharedMemoryStatusHandle statusHandle, B3_SHARED_API int b3GetStatusInverseDynamicsJointForces(b3SharedMemoryStatusHandle statusHandle,
int* bodyUniqueId, int* bodyUniqueId,
int* dofCount, int* dofCount,
double* jointForces); 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, B3_SHARED_API int b3GetStatusJacobian(b3SharedMemoryStatusHandle statusHandle,
int* dofCount, int* dofCount,
double* linearJacobian, double* linearJacobian,
double* angularJacobian); 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 ///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); 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 ///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 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 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); 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, int* dofCount,
double* jointPositions); 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 b3SharedMemoryCommandHandle b3LoadSdfCommandInit(b3PhysicsClientHandle physClient, const char* sdfFileName);
B3_SHARED_API int b3LoadSdfCommandSetUseMultiBody(b3SharedMemoryCommandHandle commandHandle, int useMultiBody); B3_SHARED_API int b3LoadSdfCommandSetUseMultiBody(b3SharedMemoryCommandHandle commandHandle, int useMultiBody);
B3_SHARED_API int b3LoadSdfCommandSetUseGlobalScaling(b3SharedMemoryCommandHandle commandHandle, double globalScaling); 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, ///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. ///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. ///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 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 b3CreatePoseCommandSetBaseOrientation(b3SharedMemoryCommandHandle commandHandle, double startOrnX,double startOrnY,double startOrnZ, double startOrnW);
B3_SHARED_API int b3CreatePoseCommandSetBaseLinearVelocity(b3SharedMemoryCommandHandle commandHandle, double linVel[/*3*/]); B3_SHARED_API int b3CreatePoseCommandSetBaseLinearVelocity(b3SharedMemoryCommandHandle commandHandle, double linVel[/*3*/]);
@@ -575,7 +584,7 @@ B3_SHARED_API void b3SetProfileTimingDuractionInMicroSeconds(b3SharedMemoryComma
B3_SHARED_API void b3SetTimeOut(b3PhysicsClientHandle physClient, double timeOutInSeconds); B3_SHARED_API void b3SetTimeOut(b3PhysicsClientHandle physClient, double timeOutInSeconds);
B3_SHARED_API double b3GetTimeOut(b3PhysicsClientHandle physClient); B3_SHARED_API double b3GetTimeOut(b3PhysicsClientHandle physClient);
B3_SHARED_API b3SharedMemoryCommandHandle b3SetAdditionalSearchPath(b3PhysicsClientHandle physClient, char* path); B3_SHARED_API b3SharedMemoryCommandHandle b3SetAdditionalSearchPath(b3PhysicsClientHandle physClient, const char* path);
B3_SHARED_API void b3MultiplyTransforms(const double posA[/*3*/], const double ornA[/*4*/], const double posB[/*3*/], const double ornB[/*4*/], double outPos[/*3*/], double outOrn[/*4*/]); B3_SHARED_API void b3MultiplyTransforms(const double posA[/*3*/], const double ornA[/*4*/], const double posB[/*3*/], const double ornB[/*4*/], double outPos[/*3*/], double outOrn[/*4*/]);
B3_SHARED_API void b3InvertTransform(const double pos[/*3*/], const double orn[/*4*/], double outPos[/*3*/], double outOrn[/*4*/]); B3_SHARED_API void b3InvertTransform(const double pos[/*3*/], const double orn[/*4*/], double outPos[/*3*/], double outOrn[/*4*/]);

435
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -6904,7 +6904,8 @@ bool PhysicsServerCommandProcessor::processInitPoseCommand(const struct SharedMe
} }
} }
btAlignedObjectArray<btQuaternion> scratch_q; btAlignedObjectArray<btQuaternion> scratch_q;
btAlignedObjectArray<btVector3> scratch_m; btAlignedObjectArray<btVector3> scratch_m;
@@ -8201,201 +8202,295 @@ bool PhysicsServerCommandProcessor::processCalculateInverseKinematicsCommand(con
int endEffectorLinkIndex = clientCmd.m_calculateInverseKinematicsArguments.m_endEffectorLinkIndex; int endEffectorLinkIndex = clientCmd.m_calculateInverseKinematicsArguments.m_endEffectorLinkIndex;
btAlignedObjectArray<double> startingPositions;
if (ikHelperPtr && (endEffectorLinkIndex<bodyHandle->m_multiBody->getNumLinks())) startingPositions.reserve(bodyHandle->m_multiBody->getNumLinks());
btVector3 targetPosWorld(clientCmd.m_calculateInverseKinematicsArguments.m_targetPosition[0],
clientCmd.m_calculateInverseKinematicsArguments.m_targetPosition[1],
clientCmd.m_calculateInverseKinematicsArguments.m_targetPosition[2]);
btQuaternion targetOrnWorld(clientCmd.m_calculateInverseKinematicsArguments.m_targetOrientation[0],
clientCmd.m_calculateInverseKinematicsArguments.m_targetOrientation[1],
clientCmd.m_calculateInverseKinematicsArguments.m_targetOrientation[2],
clientCmd.m_calculateInverseKinematicsArguments.m_targetOrientation[3]);
btTransform targetBaseCoord;
if (clientCmd.m_updateFlags& IK_HAS_CURRENT_JOINT_POSITIONS)
{ {
const int numDofs = bodyHandle->m_multiBody->getNumDofs(); targetBaseCoord.setOrigin(targetPosWorld);
int baseDofs = bodyHandle->m_multiBody->hasFixedBase() ? 0 : 6; targetBaseCoord.setRotation(targetOrnWorld);
b3AlignedObjectArray<double> jacobian_linear; } else
jacobian_linear.resize(3*numDofs); {
b3AlignedObjectArray<double> jacobian_angular; btTransform targetWorld;
jacobian_angular.resize(3*numDofs); targetWorld.setOrigin(targetPosWorld);
int jacSize = 0; targetWorld.setRotation(targetOrnWorld);
btTransform tr = bodyHandle->m_multiBody->getBaseWorldTransform();
btInverseDynamics::MultiBodyTree* tree = m_data->findOrCreateTree(bodyHandle->m_multiBody); targetBaseCoord = tr.inverse()*targetWorld;
}
{
int DofIndex = 0;
for (int i = 0; i < bodyHandle->m_multiBody->getNumLinks(); ++i)
{
if (bodyHandle->m_multiBody->getLink(i).m_jointType >= 0 && bodyHandle->m_multiBody->getLink(i).m_jointType <= 2)
{
// 0, 1, 2 represent revolute, prismatic, and spherical joint types respectively. Skip the fixed joints.
double curPos = 0;
if (clientCmd.m_updateFlags& IK_HAS_CURRENT_JOINT_POSITIONS)
{
curPos = clientCmd.m_calculateInverseKinematicsArguments.m_currentPositions[DofIndex];
} else
{
curPos = bodyHandle->m_multiBody->getJointPos(i);
}
startingPositions.push_back(curPos);
DofIndex++;
}
}
}
int numIterations = 20;
if (clientCmd.m_updateFlags& IK_HAS_MAX_ITERATIONS)
{
numIterations = clientCmd.m_calculateInverseKinematicsArguments.m_maxNumIterations;
}
double residualThreshold = 1e-4;
if (clientCmd.m_updateFlags& IK_HAS_RESIDUAL_THRESHOLD)
{
residualThreshold = clientCmd.m_calculateInverseKinematicsArguments.m_residualThreshold;
}
btScalar currentDiff = 1e30f;
b3AlignedObjectArray<double> jacobian_linear;
b3AlignedObjectArray<double> jacobian_angular;
btAlignedObjectArray<double> q_current;
btAlignedObjectArray<double> q_new;
btAlignedObjectArray<double> lower_limit;
btAlignedObjectArray<double> upper_limit;
btAlignedObjectArray<double> joint_range;
btAlignedObjectArray<double> rest_pose;
const int numDofs = bodyHandle->m_multiBody->getNumDofs();
int baseDofs = bodyHandle->m_multiBody->hasFixedBase() ? 0 : 6;
btInverseDynamics::vecx nu(numDofs+baseDofs), qdot(numDofs + baseDofs), q(numDofs + baseDofs), joint_force(numDofs + baseDofs);
for (int i=0;i<numIterations && currentDiff > residualThreshold;i++)
{
BT_PROFILE("InverseKinematics1Step");
if (ikHelperPtr && (endEffectorLinkIndex<bodyHandle->m_multiBody->getNumLinks()))
{
jacobian_linear.resize(3*numDofs);
jacobian_angular.resize(3*numDofs);
int jacSize = 0;
btInverseDynamics::MultiBodyTree* tree = m_data->findOrCreateTree(bodyHandle->m_multiBody);
btAlignedObjectArray<double> q_current; q_current.resize(numDofs);
q_current.resize(numDofs);
if (tree && ((numDofs+ baseDofs) == tree->numDoFs()))
{
if (tree && ((numDofs+ baseDofs) == tree->numDoFs())) btInverseDynamics::vec3 world_origin;
{ btInverseDynamics::mat33 world_rot;
jacSize = jacobian_linear.size();
// Set jacobian value jacSize = jacobian_linear.size();
// Set jacobian value
int DofIndex = 0;
btInverseDynamics::vecx nu(numDofs+baseDofs), qdot(numDofs + baseDofs), q(numDofs + baseDofs), joint_force(numDofs + baseDofs); for (int i = 0; i < bodyHandle->m_multiBody->getNumLinks(); ++i)
int DofIndex = 0;
for (int i = 0; i < bodyHandle->m_multiBody->getNumLinks(); ++i) {
if (bodyHandle->m_multiBody->getLink(i).m_jointType >= 0 && bodyHandle->m_multiBody->getLink(i).m_jointType <= 2) { // 0, 1, 2 represent revolute, prismatic, and spherical joint types respectively. Skip the fixed joints.
q_current[DofIndex] = bodyHandle->m_multiBody->getJointPos(i);
q[DofIndex+baseDofs] = bodyHandle->m_multiBody->getJointPos(i);
qdot[DofIndex+baseDofs] = 0;
nu[DofIndex+baseDofs] = 0;
DofIndex++;
}
} // 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))
{
tree->calculateJacobians(q);
btInverseDynamics::mat3x jac_t(3, numDofs+ baseDofs);
btInverseDynamics::mat3x jac_r(3,numDofs + baseDofs);
// Note that inverse dynamics uses zero-based indexing of bodies, not starting from -1 for the base link.
tree->getBodyJacobianTrans(endEffectorLinkIndex+1, &jac_t);
tree->getBodyJacobianRot(endEffectorLinkIndex+1, &jac_r);
for (int i = 0; i < 3; ++i)
{
for (int j = 0; j < numDofs; ++j)
{
jacobian_linear[i*numDofs+j] = jac_t(i,(baseDofs+j));
jacobian_angular[i*numDofs+j] = jac_r(i,(baseDofs+j));
}
}
}
btAlignedObjectArray<double> q_new;
q_new.resize(numDofs);
int ikMethod = 0;
if ((clientCmd.m_updateFlags& IK_HAS_TARGET_ORIENTATION)&&(clientCmd.m_updateFlags&IK_HAS_NULL_SPACE_VELOCITY))
{
//Nullspace task only works with DLS now. TODO: add nullspace task to SDLS.
ikMethod = IK2_VEL_DLS_WITH_ORIENTATION_NULLSPACE;
}
else if (clientCmd.m_updateFlags& IK_HAS_TARGET_ORIENTATION)
{
if (clientCmd.m_updateFlags & IK_SDLS)
{ {
ikMethod = IK2_VEL_SDLS_WITH_ORIENTATION; if (bodyHandle->m_multiBody->getLink(i).m_jointType >= 0 && bodyHandle->m_multiBody->getLink(i).m_jointType <= 2)
{
// 0, 1, 2 represent revolute, prismatic, and spherical joint types respectively. Skip the fixed joints.
double curPos = startingPositions[DofIndex];
q_current[DofIndex] = curPos;
q[DofIndex+baseDofs] = curPos;
qdot[DofIndex+baseDofs] = 0;
nu[DofIndex+baseDofs] = 0;
DofIndex++;
}
} // Set the gravity to correspond to the world gravity
btInverseDynamics::vec3 id_grav(m_data->m_dynamicsWorld->getGravity());
{
BT_PROFILE("calculateInverseDynamics");
if (-1 != tree->setGravityInWorldFrame(id_grav) &&
-1 != tree->calculateInverseDynamics(q, qdot, nu, &joint_force))
{
tree->calculateJacobians(q);
btInverseDynamics::mat3x jac_t(3, numDofs+ baseDofs);
btInverseDynamics::mat3x jac_r(3,numDofs + baseDofs);
// Note that inverse dynamics uses zero-based indexing of bodies, not starting from -1 for the base link.
tree->getBodyJacobianTrans(endEffectorLinkIndex+1, &jac_t);
tree->getBodyJacobianRot(endEffectorLinkIndex+1, &jac_r);
//calculatePositionKinematics is already done inside calculateInverseDynamics
tree->getBodyOrigin(endEffectorLinkIndex+1,&world_origin);
tree->getBodyTransform(endEffectorLinkIndex+1,&world_rot);
for (int i = 0; i < 3; ++i)
{
for (int j = 0; j < numDofs; ++j)
{
jacobian_linear[i*numDofs+j] = jac_t(i,(baseDofs+j));
jacobian_angular[i*numDofs+j] = jac_r(i,(baseDofs+j));
}
}
}
}
q_new.resize(numDofs);
int ikMethod = 0;
if ((clientCmd.m_updateFlags& IK_HAS_TARGET_ORIENTATION)&&(clientCmd.m_updateFlags&IK_HAS_NULL_SPACE_VELOCITY))
{
//Nullspace task only works with DLS now. TODO: add nullspace task to SDLS.
ikMethod = IK2_VEL_DLS_WITH_ORIENTATION_NULLSPACE;
}
else if (clientCmd.m_updateFlags& IK_HAS_TARGET_ORIENTATION)
{
if (clientCmd.m_updateFlags & IK_SDLS)
{
ikMethod = IK2_VEL_SDLS_WITH_ORIENTATION;
}
else
{
ikMethod = IK2_VEL_DLS_WITH_ORIENTATION;
}
}
else if (clientCmd.m_updateFlags& IK_HAS_NULL_SPACE_VELOCITY)
{
//Nullspace task only works with DLS now. TODO: add nullspace task to SDLS.
ikMethod = IK2_VEL_DLS_WITH_NULLSPACE;
} }
else else
{ {
ikMethod = IK2_VEL_DLS_WITH_ORIENTATION; if (clientCmd.m_updateFlags & IK_SDLS)
{
ikMethod = IK2_VEL_SDLS;
}
else
{
ikMethod = IK2_VEL_DLS;;
}
} }
}
else if (clientCmd.m_updateFlags& IK_HAS_NULL_SPACE_VELOCITY)
{
//Nullspace task only works with DLS now. TODO: add nullspace task to SDLS.
ikMethod = IK2_VEL_DLS_WITH_NULLSPACE;
}
else
{
if (clientCmd.m_updateFlags & IK_SDLS)
{
ikMethod = IK2_VEL_SDLS;
}
else
{
ikMethod = IK2_VEL_DLS;;
}
}
if (clientCmd.m_updateFlags& IK_HAS_NULL_SPACE_VELOCITY) if (clientCmd.m_updateFlags& IK_HAS_NULL_SPACE_VELOCITY)
{
btAlignedObjectArray<double> lower_limit;
btAlignedObjectArray<double> upper_limit;
btAlignedObjectArray<double> joint_range;
btAlignedObjectArray<double> rest_pose;
lower_limit.resize(numDofs);
upper_limit.resize(numDofs);
joint_range.resize(numDofs);
rest_pose.resize(numDofs);
for (int i = 0; i < numDofs; ++i)
{ {
lower_limit[i] = clientCmd.m_calculateInverseKinematicsArguments.m_lowerLimit[i];
upper_limit[i] = clientCmd.m_calculateInverseKinematicsArguments.m_upperLimit[i]; lower_limit.resize(numDofs);
joint_range[i] = clientCmd.m_calculateInverseKinematicsArguments.m_jointRange[i]; upper_limit.resize(numDofs);
rest_pose[i] = clientCmd.m_calculateInverseKinematicsArguments.m_restPose[i]; joint_range.resize(numDofs);
rest_pose.resize(numDofs);
for (int i = 0; i < numDofs; ++i)
{
lower_limit[i] = clientCmd.m_calculateInverseKinematicsArguments.m_lowerLimit[i];
upper_limit[i] = clientCmd.m_calculateInverseKinematicsArguments.m_upperLimit[i];
joint_range[i] = clientCmd.m_calculateInverseKinematicsArguments.m_jointRange[i];
rest_pose[i] = clientCmd.m_calculateInverseKinematicsArguments.m_restPose[i];
}
{
BT_PROFILE("computeNullspaceVel");
ikHelperPtr->computeNullspaceVel(numDofs, &q_current[0], &lower_limit[0], &upper_limit[0], &joint_range[0], &rest_pose[0]);
}
} }
ikHelperPtr->computeNullspaceVel(numDofs, &q_current[0], &lower_limit[0], &upper_limit[0], &joint_range[0], &rest_pose[0]);
}
//btTransform endEffectorTransformWorld = bodyHandle->m_multiBody->getLink(endEffectorLinkIndex).m_cachedWorldTransform * bodyHandle->m_linkLocalInertialFrames[endEffectorLinkIndex].inverse();
btTransform endEffectorTransformWorld = bodyHandle->m_multiBody->getLink(endEffectorLinkIndex).m_cachedWorldTransform * bodyHandle->m_linkLocalInertialFrames[endEffectorLinkIndex].inverse();
btVector3DoubleData endEffectorWorldPosition; btVector3DoubleData endEffectorWorldPosition;
btQuaternionDoubleData endEffectorWorldOrientation; btQuaternionDoubleData endEffectorWorldOrientation;
//get the position from the inverse dynamics (based on q) instead of endEffectorTransformWorld
btVector3 endEffectorPosWorldOrg = world_origin;
btQuaternion endEffectorOriWorldOrg;
world_rot.getRotation(endEffectorOriWorldOrg);
btTransform endEffectorBaseCoord;
endEffectorBaseCoord.setOrigin(endEffectorPosWorldOrg);
endEffectorBaseCoord.setRotation(endEffectorOriWorldOrg);
//don't need the next two lines
//btTransform linkInertiaInv = bodyHandle->m_linkLocalInertialFrames[endEffectorLinkIndex].inverse();
//endEffectorBaseCoord = endEffectorBaseCoord * linkInertiaInv;
//btTransform tr = bodyHandle->m_multiBody->getBaseWorldTransform();
//endEffectorBaseCoord = tr.inverse()*endEffectorTransformWorld;
//endEffectorBaseCoord = tr.inverse()*endEffectorTransformWorld;
btQuaternion endEffectorOriBaseCoord= endEffectorBaseCoord.getRotation();
//btVector4 endEffectorOri(endEffectorOriBaseCoord.x(), endEffectorOriBaseCoord.y(), endEffectorOriBaseCoord.z(), endEffectorOriBaseCoord.w());
btVector3 endEffectorPosWorldOrg = endEffectorTransformWorld.getOrigin(); endEffectorBaseCoord.getOrigin().serializeDouble(endEffectorWorldPosition);
btQuaternion endEffectorOriWorldOrg = endEffectorTransformWorld.getRotation(); endEffectorBaseCoord.getRotation().serializeDouble(endEffectorWorldOrientation);
btTransform endEffectorWorld;
endEffectorWorld.setOrigin(endEffectorPosWorldOrg);
endEffectorWorld.setRotation(endEffectorOriWorldOrg);
btTransform tr = bodyHandle->m_multiBody->getBaseWorldTransform();
btTransform endEffectorBaseCoord = tr.inverse()*endEffectorWorld;
btQuaternion endEffectorOriBaseCoord= endEffectorBaseCoord.getRotation();
btVector4 endEffectorOri(endEffectorOriBaseCoord.x(), endEffectorOriBaseCoord.y(), endEffectorOriBaseCoord.z(), endEffectorOriBaseCoord.w());
endEffectorBaseCoord.getOrigin().serializeDouble(endEffectorWorldPosition);
endEffectorBaseCoord.getRotation().serializeDouble(endEffectorWorldOrientation);
btVector3 targetPosWorld(clientCmd.m_calculateInverseKinematicsArguments.m_targetPosition[0], //diff
clientCmd.m_calculateInverseKinematicsArguments.m_targetPosition[1], currentDiff = (endEffectorBaseCoord.getOrigin()-targetBaseCoord.getOrigin()).length();
clientCmd.m_calculateInverseKinematicsArguments.m_targetPosition[2]);
btQuaternion targetOrnWorld(clientCmd.m_calculateInverseKinematicsArguments.m_targetOrientation[0],
clientCmd.m_calculateInverseKinematicsArguments.m_targetOrientation[1],
clientCmd.m_calculateInverseKinematicsArguments.m_targetOrientation[2],
clientCmd.m_calculateInverseKinematicsArguments.m_targetOrientation[3]);
btTransform targetWorld;
targetWorld.setOrigin(targetPosWorld);
targetWorld.setRotation(targetOrnWorld);
btTransform targetBaseCoord;
targetBaseCoord = tr.inverse()*targetWorld;
btVector3DoubleData targetPosBaseCoord; btVector3DoubleData targetPosBaseCoord;
btQuaternionDoubleData targetOrnBaseCoord; btQuaternionDoubleData targetOrnBaseCoord;
targetBaseCoord.getOrigin().serializeDouble(targetPosBaseCoord); targetBaseCoord.getOrigin().serializeDouble(targetPosBaseCoord);
targetBaseCoord.getRotation().serializeDouble(targetOrnBaseCoord); targetBaseCoord.getRotation().serializeDouble(targetOrnBaseCoord);
// Set joint damping coefficents. A small default // Set joint damping coefficents. A small default
// damping constant is added to prevent singularity // damping constant is added to prevent singularity
// with pseudo inverse. The user can set joint damping // with pseudo inverse. The user can set joint damping
// coefficients differently for each joint. The larger // coefficients differently for each joint. The larger
// the damping coefficient is, the less we rely on // the damping coefficient is, the less we rely on
// this joint to achieve the IK target. // this joint to achieve the IK target.
btAlignedObjectArray<double> joint_damping; btAlignedObjectArray<double> joint_damping;
joint_damping.resize(numDofs,0.5); joint_damping.resize(numDofs,0.5);
if (clientCmd.m_updateFlags& IK_HAS_JOINT_DAMPING) if (clientCmd.m_updateFlags& IK_HAS_JOINT_DAMPING)
{
for (int i = 0; i < numDofs; ++i)
{ {
joint_damping[i] = clientCmd.m_calculateInverseKinematicsArguments.m_jointDamping[i]; for (int i = 0; i < numDofs; ++i)
{
joint_damping[i] = clientCmd.m_calculateInverseKinematicsArguments.m_jointDamping[i];
}
}
ikHelperPtr->setDampingCoeff(numDofs, &joint_damping[0]);
double targetDampCoeff[6]={1.0,1.0,1.0,1.0,1.0,1.0};
{
BT_PROFILE("computeIK");
ikHelperPtr->computeIK(targetPosBaseCoord.m_floats, targetOrnBaseCoord.m_floats,
endEffectorWorldPosition.m_floats, endEffectorWorldOrientation.m_floats,
&q_current[0],
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_calculateInverseDynamicsArguments.m_bodyUniqueId;
for (int i=0;i<numDofs;i++)
{
serverCmd.m_inverseKinematicsResultArgs.m_jointPositions[i] = q_new[i];
}
serverCmd.m_inverseKinematicsResultArgs.m_dofCount = numDofs;
serverCmd.m_type = CMD_CALCULATE_INVERSE_KINEMATICS_COMPLETED;
for (int i=0;i<numDofs;i++)
{
startingPositions[i] = q_new[i];
} }
} }
ikHelperPtr->setDampingCoeff(numDofs, &joint_damping[0]);
double targetDampCoeff[6] = { 1.0, 1.0, 1.0, 1.0, 1.0, 1.0 };
ikHelperPtr->computeIK(targetPosBaseCoord.m_floats, targetOrnBaseCoord.m_floats,
endEffectorWorldPosition.m_floats, endEffectorWorldOrientation.m_floats,
&q_current[0],
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_calculateInverseDynamicsArguments.m_bodyUniqueId;
for (int i=0;i<numDofs;i++)
{
serverCmd.m_inverseKinematicsResultArgs.m_jointPositions[i] = q_new[i];
}
serverCmd.m_inverseKinematicsResultArgs.m_dofCount = numDofs;
serverCmd.m_type = CMD_CALCULATE_INVERSE_KINEMATICS_COMPLETED;
} }
} }
} }
return hasStatus; return hasStatus;
} }
// PyModule_AddIntConstant(m, "GEOM_SPHERE", GEOM_SPHERE); // PyModule_AddIntConstant(m, "GEOM_SPHERE", GEOM_SPHERE);
// PyModule_AddIntConstant(m, "GEOM_BOX", GEOM_BOX); // PyModule_AddIntConstant(m, "GEOM_BOX", GEOM_BOX);
// PyModule_AddIntConstant(m, "GEOM_CYLINDER", GEOM_CYLINDER); // PyModule_AddIntConstant(m, "GEOM_CYLINDER", GEOM_CYLINDER);
@@ -9490,7 +9585,7 @@ bool PhysicsServerCommandProcessor::pickBody(const btVector3& rayFromWorld, cons
//see also http://www.bulletphysics.org/Bullet/phpBB3/viewtopic.php?f=4&t=949 //see also http://www.bulletphysics.org/Bullet/phpBB3/viewtopic.php?f=4&t=949
//so we try to avoid it by clamping the maximum impulse (force) that the mouse pick can apply //so we try to avoid it by clamping the maximum impulse (force) that the mouse pick can apply
//it is not satisfying, hopefully we find a better solution (higher order integrator, using joint friction using a zero-velocity target motor with limited force etc?) //it is not satisfying, hopefully we find a better solution (higher order integrator, using joint friction using a zero-velocity target motor with limited force etc?)
btScalar scaling=1; btScalar scaling=10;
p2p->setMaxAppliedImpulse(2*scaling); p2p->setMaxAppliedImpulse(2*scaling);
btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*) m_data->m_dynamicsWorld; btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*) m_data->m_dynamicsWorld;

3
examples/SharedMemory/SharedMemoryCommands.h
View File

@@ -695,6 +695,9 @@ struct CalculateInverseKinematicsArgs
double m_jointRange[MAX_DEGREE_OF_FREEDOM]; double m_jointRange[MAX_DEGREE_OF_FREEDOM];
double m_restPose[MAX_DEGREE_OF_FREEDOM]; double m_restPose[MAX_DEGREE_OF_FREEDOM];
double m_jointDamping[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 struct CalculateInverseKinematicsResultArgs

4
examples/SharedMemory/SharedMemoryPublic.h
View File

@@ -628,7 +628,9 @@ enum EnumCalculateInverseKinematicsFlags
IK_HAS_TARGET_ORIENTATION=32, IK_HAS_TARGET_ORIENTATION=32,
IK_HAS_NULL_SPACE_VELOCITY=64, IK_HAS_NULL_SPACE_VELOCITY=64,
IK_HAS_JOINT_DAMPING=128, 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 enum b3ConfigureDebugVisualizerEnum

6
examples/pybullet/examples/inverse_kinematics.py
View File

@@ -34,12 +34,12 @@ t=0.
prevPose=[0,0,0] prevPose=[0,0,0]
prevPose1=[0,0,0] prevPose1=[0,0,0]
hasPrevPose = 0 hasPrevPose = 0
useNullSpace = 0 useNullSpace = 1
useOrientation = 1 useOrientation = 1
#If we set useSimulation=0, it sets the arm pose to be the IK result directly without using dynamic control. #If we set useSimulation=0, it sets the arm pose to be the IK result directly without using dynamic control.
#This can be used to test the IK result accuracy. #This can be used to test the IK result accuracy.
useSimulation = 1 useSimulation = 0
useRealTimeSimulation = 1 useRealTimeSimulation = 1
ikSolver = 0 ikSolver = 0
p.setRealTimeSimulation(useRealTimeSimulation) p.setRealTimeSimulation(useRealTimeSimulation)
@@ -69,7 +69,7 @@ while 1:
jointPoses = p.calculateInverseKinematics(kukaId,kukaEndEffectorIndex,pos,lowerLimits=ll, upperLimits=ul, jointRanges=jr, restPoses=rp) jointPoses = p.calculateInverseKinematics(kukaId,kukaEndEffectorIndex,pos,lowerLimits=ll, upperLimits=ul, jointRanges=jr, restPoses=rp)
else: else:
if (useOrientation==1): if (useOrientation==1):
jointPoses = p.calculateInverseKinematics(kukaId,kukaEndEffectorIndex,pos,orn,jointDamping=jd,solver=ikSolver) jointPoses = p.calculateInverseKinematics(kukaId,kukaEndEffectorIndex,pos,orn,jointDamping=jd,solver=ikSolver, maxNumIterations=100, residualThreshold=.01)
else: else:
jointPoses = p.calculateInverseKinematics(kukaId,kukaEndEffectorIndex,pos,solver=ikSolver) jointPoses = p.calculateInverseKinematics(kukaId,kukaEndEffectorIndex,pos,solver=ikSolver)

93
examples/pybullet/pybullet.c
View File

@@ -8055,9 +8055,6 @@ static PyObject* pybullet_executePluginCommand(PyObject* self,
return PyInt_FromLong(statusType); return PyInt_FromLong(statusType);
} }
///Inverse Kinematics binding ///Inverse Kinematics binding
static PyObject* pybullet_calculateInverseKinematics(PyObject* self, static PyObject* pybullet_calculateInverseKinematics(PyObject* self,
PyObject* args, PyObject* keywds) PyObject* args, PyObject* keywds)
@@ -8077,9 +8074,12 @@ static PyObject* pybullet_calculateInverseKinematics(PyObject* self,
PyObject* jointRangesObj = 0; PyObject* jointRangesObj = 0;
PyObject* restPosesObj = 0; PyObject* restPosesObj = 0;
PyObject* jointDampingObj = 0; PyObject* jointDampingObj = 0;
PyObject* currentPositionsObj = 0;
int maxNumIterations = -1;
double residualThreshold=-1;
static char* kwlist[] = {"bodyUniqueId", "endEffectorLinkIndex", "targetPosition", "targetOrientation", "lowerLimits", "upperLimits", "jointRanges", "restPoses", "jointDamping", "solver", "physicsClientId", NULL}; static char* kwlist[] = {"bodyUniqueId", "endEffectorLinkIndex", "targetPosition", "targetOrientation", "lowerLimits", "upperLimits", "jointRanges", "restPoses", "jointDamping", "solver", "currentPositions", "maxNumIterations", "residualThreshold", "physicsClientId", NULL};
if (!PyArg_ParseTupleAndKeywords(args, keywds, "iiO|OOOOOOii", kwlist, &bodyUniqueId, &endEffectorLinkIndex, &targetPosObj, &targetOrnObj, &lowerLimitsObj, &upperLimitsObj, &jointRangesObj, &restPosesObj, &jointDampingObj, &solver, &physicsClientId)) if (!PyArg_ParseTupleAndKeywords(args, keywds, "iiO|OOOOOOiOidi", kwlist, &bodyUniqueId, &endEffectorLinkIndex, &targetPosObj, &targetOrnObj, &lowerLimitsObj, &upperLimitsObj, &jointRangesObj, &restPosesObj, &jointDampingObj, &solver, &currentPositionsObj, &maxNumIterations, &residualThreshold, &physicsClientId))
{ {
//backward compatibility bodyIndex -> bodyUniqueId. don't update keywords, people need to migrate to bodyUniqueId version //backward compatibility bodyIndex -> bodyUniqueId. don't update keywords, people need to migrate to bodyUniqueId version
static char* kwlist2[] = {"bodyIndex", "endEffectorLinkIndex", "targetPosition", "targetOrientation", "lowerLimits", "upperLimits", "jointRanges", "restPoses", "jointDamping", "physicsClientId", NULL}; static char* kwlist2[] = {"bodyIndex", "endEffectorLinkIndex", "targetPosition", "targetOrientation", "lowerLimits", "upperLimits", "jointRanges", "restPoses", "jointDamping", "physicsClientId", NULL};
@@ -8107,16 +8107,22 @@ static PyObject* pybullet_calculateInverseKinematics(PyObject* self,
int szRestPoses = restPosesObj ? PySequence_Size(restPosesObj) : 0; int szRestPoses = restPosesObj ? PySequence_Size(restPosesObj) : 0;
int szJointDamping = jointDampingObj ? PySequence_Size(jointDampingObj) : 0; int szJointDamping = jointDampingObj ? PySequence_Size(jointDampingObj) : 0;
int szCurrentPositions = currentPositionsObj ? PySequence_Size(currentPositionsObj) : 0;
int numJoints = b3GetNumJoints(sm, bodyUniqueId); int numJoints = b3GetNumJoints(sm, bodyUniqueId);
int dofCount = b3ComputeDofCount(sm, bodyUniqueId);
int hasNullSpace = 0; int hasNullSpace = 0;
int hasJointDamping = 0; int hasJointDamping = 0;
int hasCurrentPositions = 0;
double* lowerLimits = 0; double* lowerLimits = 0;
double* upperLimits = 0; double* upperLimits = 0;
double* jointRanges = 0; double* jointRanges = 0;
double* restPoses = 0; double* restPoses = 0;
double* jointDamping = 0; double* jointDamping = 0;
double* currentPositions = 0;
if (numJoints && (szLowerLimits == numJoints) && (szUpperLimits == numJoints) && if (numJoints && (szLowerLimits == numJoints) && (szUpperLimits == numJoints) &&
(szJointRanges == numJoints) && (szRestPoses == numJoints)) (szJointRanges == numJoints) && (szRestPoses == numJoints))
{ {
@@ -8141,6 +8147,27 @@ static PyObject* pybullet_calculateInverseKinematics(PyObject* self,
hasNullSpace = 1; hasNullSpace = 1;
} }
if (szCurrentPositions > 0)
{
if (szCurrentPositions < numJoints)
{
PyErr_SetString(SpamError,
"calculateInverseKinematics the size of input current positions is smaller than the number of joints.");
return NULL;
}
else
{
int szInBytes = sizeof(double) * szCurrentPositions;
int i;
currentPositions = (double*)malloc(szInBytes);
for (i = 0; i < szCurrentPositions; i++)
{
currentPositions[i] = pybullet_internalGetFloatFromSequence(currentPositionsObj, i);
}
hasCurrentPositions = 1;
}
}
if (szJointDamping > 0) if (szJointDamping > 0)
{ {
// We allow the number of joint damping values to be larger than // We allow the number of joint damping values to be larger than
@@ -8179,6 +8206,19 @@ static PyObject* pybullet_calculateInverseKinematics(PyObject* self,
b3SharedMemoryCommandHandle command = b3CalculateInverseKinematicsCommandInit(sm, bodyUniqueId); b3SharedMemoryCommandHandle command = b3CalculateInverseKinematicsCommandInit(sm, bodyUniqueId);
b3CalculateInverseKinematicsSelectSolver(command, solver); b3CalculateInverseKinematicsSelectSolver(command, solver);
if (hasCurrentPositions)
{
b3CalculateInverseKinematicsSetCurrentPositions(command, numJoints, currentPositions);
}
if (maxNumIterations>0)
{
b3CalculateInverseKinematicsSetMaxNumIterations(command,maxNumIterations);
}
if (residualThreshold>=0)
{
b3CalculateInverseKinematicsSetResidualThreshold(command,residualThreshold);
}
if (hasNullSpace) if (hasNullSpace)
{ {
if (hasOrn) if (hasOrn)
@@ -8206,6 +8246,7 @@ static PyObject* pybullet_calculateInverseKinematics(PyObject* self,
{ {
b3CalculateInverseKinematicsSetJointDamping(command, numJoints, jointDamping); b3CalculateInverseKinematicsSetJointDamping(command, numJoints, jointDamping);
} }
free(currentPositions);
free(jointDamping); free(jointDamping);
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
@@ -8294,44 +8335,8 @@ static PyObject* pybullet_calculateInverseDynamics(PyObject* self, PyObject* arg
int szObPos = PySequence_Size(objPositionsQ); int szObPos = PySequence_Size(objPositionsQ);
int szObVel = PySequence_Size(objVelocitiesQdot); int szObVel = PySequence_Size(objVelocitiesQdot);
int szObAcc = PySequence_Size(objAccelerations); int szObAcc = PySequence_Size(objAccelerations);
int nj = b3GetNumJoints(sm, bodyUniqueId);
int j=0; int dofCountOrg = b3ComputeDofCount(sm, bodyUniqueId);
int dofCountOrg = 0;
for (j=0;j<nj;j++)
{
struct b3JointInfo info;
b3GetJointInfo(sm, bodyUniqueId, j, &info);
switch (info.m_jointType)
{
case eRevoluteType:
{
dofCountOrg+=1;
break;
}
case ePrismaticType:
{
dofCountOrg+=1;
break;
}
case eSphericalType:
{
PyErr_SetString(SpamError,
"Spherirical joints are not supported in the pybullet binding");
return NULL;
}
case ePlanarType:
{
PyErr_SetString(SpamError,
"Planar joints are not supported in the pybullet binding");
return NULL;
}
default:
{
//fixed joint has 0-dof and at the moment, we don't deal with planar, spherical etc
}
}
}
if (dofCountOrg && (szObPos == dofCountOrg) && (szObVel == dofCountOrg) && if (dofCountOrg && (szObPos == dofCountOrg) && (szObVel == dofCountOrg) &&
(szObAcc == dofCountOrg)) (szObAcc == dofCountOrg))