diff --git a/data/pole.urdf b/data/pole.urdf new file mode 100755 index 000000000..5932f221d --- /dev/null +++ b/data/pole.urdf @@ -0,0 +1,140 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 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 b27d7b6f2..0db35aa69 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -3721,6 +3721,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 979fb9e14..942e07440 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -338,6 +338,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 6c4461d4e..9b858bb30 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -7731,19 +7731,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 3afe9eae0..15449d2f0 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 6a069f6de..03f3d79d8 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -572,6 +572,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/examples/inverse_kinematics_pole.py b/examples/pybullet/examples/inverse_kinematics_pole.py new file mode 100755 index 000000000..f910e7033 --- /dev/null +++ b/examples/pybullet/examples/inverse_kinematics_pole.py @@ -0,0 +1,59 @@ +import pybullet as p +import time +import math +from datetime import datetime + +clid = p.connect(p.SHARED_MEMORY) +if (clid<0): + p.connect(p.GUI) +p.loadURDF("plane.urdf",[0,0,-1.3]) +p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,0) +sawyerId = p.loadURDF("pole.urdf",[0,0,0]) +p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,1) +p.resetBasePositionAndOrientation(sawyerId,[0,0,0],[0,0,0,1]) + + +sawyerEndEffectorIndex = 3 +numJoints = p.getNumJoints(sawyerId) +#joint damping coefficents +jd=[0.01,0.01,0.01,0.01] + +p.setGravity(0,0,0) +t=0. +prevPose=[0,0,0] +prevPose1=[0,0,0] +hasPrevPose = 0 + +ikSolver = 0 +useRealTimeSimulation = 0 +p.setRealTimeSimulation(useRealTimeSimulation) +#trailDuration is duration (in seconds) after debug lines will be removed automatically +#use 0 for no-removal +trailDuration = 1 + +while 1: + if (useRealTimeSimulation): + dt = datetime.now() + t = (dt.second/60.)*2.*math.pi + else: + t=t+0.01 + time.sleep(0.01) + + for i in range (1): + pos = [2.*math.cos(t),2.*math.cos(t),0.+2.*math.sin(t)] + jointPoses = p.calculateInverseKinematics(sawyerId,sawyerEndEffectorIndex,pos,jointDamping=jd,solver=ikSolver) + + #reset the joint state (ignoring all dynamics, not recommended to use during simulation) + for i in range (numJoints): + jointInfo = p.getJointInfo(sawyerId, i) + qIndex = jointInfo[3] + if qIndex > -1: + p.resetJointState(sawyerId,i,jointPoses[qIndex-7]) + + ls = p.getLinkState(sawyerId,sawyerEndEffectorIndex) + if (hasPrevPose): + p.addUserDebugLine(prevPose,pos,[0,0,0.3],1,trailDuration) + p.addUserDebugLine(prevPose1,ls[4],[1,0,0],1,trailDuration) + prevPose=pos + prevPose1=ls[4] + hasPrevPose = 1 diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index e96d24218..426115eb0 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -7049,7 +7049,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; @@ -7058,8 +7059,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}; @@ -7157,6 +7158,7 @@ static PyObject* pybullet_calculateInverseKinematics(PyObject* self, int result; b3SharedMemoryCommandHandle command = b3CalculateInverseKinematicsCommandInit(sm, bodyUniqueId); + b3CalculateInverseKinematicsSelectSolver(command, solver); if (hasNullSpace) { @@ -8187,6 +8189,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);