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

Expose IK solver options including DLS and SDLS.

Browse Source
This commit is contained in:
yunfeibai
2017-10-19 14:00:53 -07:00
parent 1393666a83
commit dda1b05f4a
10 changed files with 64 additions and 20 deletions
Download Patch File Download Diff File Expand all files Collapse all files

7
examples/SharedMemory/IKTrajectoryHelper.cpp
View File

@@ -47,7 +47,8 @@ bool IKTrajectoryHelper::computeIK(const double endEffectorTargetPosition[3],
const double* q_current, int numQ,int endEffectorIndex,
double* q_new, int ikMethod, const double* linear_jacobian, const double* angular_jacobian, int jacobian_size, const double dampIk[6])
{
bool useAngularPart = (ikMethod==IK2_VEL_DLS_WITH_ORIENTATION || ikMethod==IK2_VEL_DLS_WITH_ORIENTATION_NULLSPACE) ? true : false;
bool useAngularPart = (ikMethod==IK2_VEL_DLS_WITH_ORIENTATION || ikMethod==IK2_VEL_DLS_WITH_ORIENTATION_NULLSPACE
|| ikMethod==IK2_VEL_SDLS_WITH_ORIENTATION) ? true : false;
Jacobian ikJacobian(useAngularPart,numQ);
@@ -136,8 +137,8 @@ bool IKTrajectoryHelper::computeIK(const double endEffectorTargetPosition[3],
ikJacobian.CalcDeltaThetasTranspose(); // Jacobian transpose method
break;
case IK2_DLS:
case IK2_VEL_DLS:
case IK2_VEL_DLS_WITH_ORIENTATION:
case IK2_VEL_DLS:
//ikJacobian.CalcDeltaThetasDLS(); // Damped least squares method
assert(m_data->m_dampingCoeff.GetLength()==numQ);
ikJacobian.CalcDeltaThetasDLS2(m_data->m_dampingCoeff);
@@ -154,6 +155,8 @@ bool IKTrajectoryHelper::computeIK(const double endEffectorTargetPosition[3],
ikJacobian.CalcDeltaThetasPseudoinverse(); // Pure pseudoinverse method
break;
case IK2_SDLS:
case IK2_VEL_SDLS:
case IK2_VEL_SDLS_WITH_ORIENTATION:
ikJacobian.CalcDeltaThetasSDLS(); // Selectively damped least squares method
break;
default:

2
examples/SharedMemory/IKTrajectoryHelper.h
View File

@@ -12,6 +12,8 @@ enum IK2_Method
IK2_VEL_DLS_WITH_ORIENTATION,
IK2_VEL_DLS_WITH_NULLSPACE,
IK2_VEL_DLS_WITH_ORIENTATION_NULLSPACE,
IK2_VEL_SDLS,
IK2_VEL_SDLS_WITH_ORIENTATION,
};

8
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@@ -3683,6 +3683,14 @@ B3_SHARED_API void b3CalculateInverseKinematicsSetJointDamping(b3SharedMemoryCom
}
}
B3_SHARED_API void b3CalculateInverseKinematicsSelectSolver(b3SharedMemoryCommandHandle commandHandle, int solver)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_CALCULATE_INVERSE_KINEMATICS);
command->m_updateFlags |= solver;
}
B3_SHARED_API int b3GetStatusInverseKinematicsJointPositions(b3SharedMemoryStatusHandle statusHandle,
int* bodyUniqueId,
int* dofCount,

1
examples/SharedMemory/PhysicsClientC_API.h
View File

@@ -333,6 +333,7 @@ B3_SHARED_API void b3CalculateInverseKinematicsAddTargetPositionWithOrientation(
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 b3CalculateInverseKinematicsPosOrnWithNullSpaceVel(b3SharedMemoryCommandHandle commandHandle, int numDof, int endEffectorLinkIndex, const double targetPosition[/*3*/], const double targetOrientation[/*4*/], const double* lowerLimit, const double* upperLimit, const double* jointRange, const double* restPose);
B3_SHARED_API void b3CalculateInverseKinematicsSetJointDamping(b3SharedMemoryCommandHandle commandHandle, int numDof, const double* jointDampingCoeff);
B3_SHARED_API void b3CalculateInverseKinematicsSelectSolver(b3SharedMemoryCommandHandle commandHandle, int solver);
B3_SHARED_API int b3GetStatusInverseKinematicsJointPositions(b3SharedMemoryStatusHandle statusHandle,
int* bodyUniqueId,
int* dofCount,

20
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -7705,19 +7705,35 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
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)
{
ikMethod = IK2_VEL_DLS_WITH_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
{
ikMethod = IK2_VEL_DLS;
if (clientCmd.m_updateFlags & IK_SDLS)
{
ikMethod = IK2_VEL_SDLS;
}
else
{
ikMethod = IK2_VEL_DLS;;
}
}
if (clientCmd.m_updateFlags& IK_HAS_NULL_SPACE_VELOCITY)

9
examples/SharedMemory/SharedMemoryCommands.h
View File

@@ -633,15 +633,6 @@ struct CalculateMassMatrixResultArgs
int m_dofCount;
};
enum EnumCalculateInverseKinematicsFlags
{
IK_HAS_TARGET_POSITION=1,
IK_HAS_TARGET_ORIENTATION=2,
IK_HAS_NULL_SPACE_VELOCITY=4,
IK_HAS_JOINT_DAMPING=8,
//IK_HAS_CURRENT_JOINT_POSITIONS=16,//not used yet
};
struct CalculateInverseKinematicsArgs
{
int m_bodyUniqueId;

12
examples/SharedMemory/SharedMemoryPublic.h
View File

@@ -565,6 +565,18 @@ enum EnumRenderer
//ER_FIRE_RAYS=(1<<18),
};
///flags to pick the IK solver and other options
enum EnumCalculateInverseKinematicsFlags
{
IK_DLS=0,
IK_SDLS=1, //TODO: can add other IK solvers
IK_HAS_TARGET_POSITION=16,
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
};
enum b3ConfigureDebugVisualizerEnum
{
COV_ENABLE_GUI=1,