From dda1b05f4a48a34c859383e08ae6ba7d802a9eb9 Mon Sep 17 00:00:00 2001 From: yunfeibai Date: Thu, 19 Oct 2017 14:00:53 -0700 Subject: [PATCH] Expose IK solver options including DLS and SDLS. --- examples/SharedMemory/IKTrajectoryHelper.cpp | 7 +++++-- examples/SharedMemory/IKTrajectoryHelper.h | 2 ++ examples/SharedMemory/PhysicsClientC_API.cpp | 8 ++++++++ examples/SharedMemory/PhysicsClientC_API.h | 1 + .../PhysicsServerCommandProcessor.cpp | 20 +++++++++++++++++-- examples/SharedMemory/SharedMemoryCommands.h | 9 --------- examples/SharedMemory/SharedMemoryPublic.h | 12 +++++++++++ examples/ThirdPartyLibs/BussIK/Jacobian.cpp | 3 ++- .../pybullet/examples/inverse_kinematics.py | 7 ++++--- examples/pybullet/pybullet.c | 15 +++++++++++--- 10 files changed, 64 insertions(+), 20 deletions(-) diff --git a/examples/SharedMemory/IKTrajectoryHelper.cpp b/examples/SharedMemory/IKTrajectoryHelper.cpp index 0145251bb..ac6faad9f 100644 --- a/examples/SharedMemory/IKTrajectoryHelper.cpp +++ b/examples/SharedMemory/IKTrajectoryHelper.cpp @@ -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: diff --git a/examples/SharedMemory/IKTrajectoryHelper.h b/examples/SharedMemory/IKTrajectoryHelper.h index 90f2dc409..10d4e1976 100644 --- a/examples/SharedMemory/IKTrajectoryHelper.h +++ b/examples/SharedMemory/IKTrajectoryHelper.h @@ -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, }; diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index 58a251949..9851aee07 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -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, diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index 539dacfa5..aa8636c64 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -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, diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index d3cfa2d5b..9f5b48645 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -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) diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index c4c82762d..26ef6d1a7 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -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; diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index b3b223ae1..2fde769bb 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -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, diff --git a/examples/ThirdPartyLibs/BussIK/Jacobian.cpp b/examples/ThirdPartyLibs/BussIK/Jacobian.cpp index c082bef31..805f21625 100644 --- a/examples/ThirdPartyLibs/BussIK/Jacobian.cpp +++ b/examples/ThirdPartyLibs/BussIK/Jacobian.cpp @@ -462,7 +462,8 @@ void Jacobian::CalcDeltaThetasSDLS() // Calculate response vector dTheta that is the SDLS solution. // Delta target values are the dS values int nRows = J.GetNumRows(); - int numEndEffectors = m_tree->GetNumEffector(); // Equals the number of rows of J divided by three + // TODO: Modify it to work with multiple end effectors. + int numEndEffectors = 1; int nCols = J.GetNumColumns(); dTheta.SetZero(); diff --git a/examples/pybullet/examples/inverse_kinematics.py b/examples/pybullet/examples/inverse_kinematics.py index 9aa32afed..928389542 100644 --- a/examples/pybullet/examples/inverse_kinematics.py +++ b/examples/pybullet/examples/inverse_kinematics.py @@ -39,8 +39,9 @@ useNullSpace = 0 useOrientation = 1 #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. -useSimulation = 0 +useSimulation = 1 useRealTimeSimulation = 1 +ikSolver = 0 p.setRealTimeSimulation(useRealTimeSimulation) #trailDuration is duration (in seconds) after debug lines will be removed automatically #use 0 for no-removal @@ -68,9 +69,9 @@ while 1: jointPoses = p.calculateInverseKinematics(kukaId,kukaEndEffectorIndex,pos,lowerLimits=ll, upperLimits=ul, jointRanges=jr, restPoses=rp) else: if (useOrientation==1): - jointPoses = p.calculateInverseKinematics(kukaId,kukaEndEffectorIndex,pos,orn,jointDamping=jd) + jointPoses = p.calculateInverseKinematics(kukaId,kukaEndEffectorIndex,pos,orn,jointDamping=jd,solver=ikSolver) else: - jointPoses = p.calculateInverseKinematics(kukaId,kukaEndEffectorIndex,pos) + jointPoses = p.calculateInverseKinematics(kukaId,kukaEndEffectorIndex,pos,solver=ikSolver) if (useSimulation): for i in range (numJoints): diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index 508e6c461..e3171fc22 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -6998,7 +6998,8 @@ static PyObject* pybullet_calculateInverseKinematics(PyObject* self, PyObject* targetPosObj = 0; PyObject* targetOrnObj = 0; - + + int solver = 0; // the default IK solver is DLS int physicsClientId = 0; b3PhysicsClientHandle sm = 0; PyObject* lowerLimitsObj = 0; @@ -7007,8 +7008,8 @@ static PyObject* pybullet_calculateInverseKinematics(PyObject* self, PyObject* restPosesObj = 0; PyObject* jointDampingObj = 0; - static char* kwlist[] = {"bodyUniqueId", "endEffectorLinkIndex", "targetPosition", "targetOrientation", "lowerLimits", "upperLimits", "jointRanges", "restPoses", "jointDamping", "physicsClientId", NULL}; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "iiO|OOOOOOi", kwlist, &bodyUniqueId, &endEffectorLinkIndex, &targetPosObj, &targetOrnObj, &lowerLimitsObj, &upperLimitsObj, &jointRangesObj, &restPosesObj, &jointDampingObj, &physicsClientId)) + static char* kwlist[] = {"bodyUniqueId", "endEffectorLinkIndex", "targetPosition", "targetOrientation", "lowerLimits", "upperLimits", "jointRanges", "restPoses", "jointDamping", "solver", "physicsClientId", NULL}; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "iiO|OOOOOOii", kwlist, &bodyUniqueId, &endEffectorLinkIndex, &targetPosObj, &targetOrnObj, &lowerLimitsObj, &upperLimitsObj, &jointRangesObj, &restPosesObj, &jointDampingObj, &solver, &physicsClientId)) { //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}; @@ -7106,6 +7107,7 @@ static PyObject* pybullet_calculateInverseKinematics(PyObject* self, int result; b3SharedMemoryCommandHandle command = b3CalculateInverseKinematicsCommandInit(sm, bodyUniqueId); + b3CalculateInverseKinematicsSelectSolver(command, solver); if (hasNullSpace) { @@ -8133,6 +8135,13 @@ initpybullet(void) PyModule_AddIntConstant(m, "ER_TINY_RENDERER", ER_TINY_RENDERER); PyModule_AddIntConstant(m, "ER_BULLET_HARDWARE_OPENGL", ER_BULLET_HARDWARE_OPENGL); + + PyModule_AddIntConstant(m, "IK_DLS", IK_DLS); + PyModule_AddIntConstant(m, "IK_SDLS", IK_SDLS); + PyModule_AddIntConstant(m, "IK_HAS_TARGET_POSITION", IK_HAS_TARGET_POSITION); + PyModule_AddIntConstant(m, "IK_HAS_TARGET_ORIENTATION", IK_HAS_TARGET_ORIENTATION); + PyModule_AddIntConstant(m, "IK_HAS_NULL_SPACE_VELOCITY", IK_HAS_NULL_SPACE_VELOCITY); + PyModule_AddIntConstant(m, "IK_HAS_JOINT_DAMPING", IK_HAS_JOINT_DAMPING); PyModule_AddIntConstant(m, "URDF_USE_INERTIA_FROM_FILE", URDF_USE_INERTIA_FROM_FILE); PyModule_AddIntConstant(m, "URDF_USE_SELF_COLLISION", URDF_USE_SELF_COLLISION);