diff --git a/examples/RoboticsLearning/KukaGraspExample.cpp b/examples/RoboticsLearning/KukaGraspExample.cpp index ebdea84d3..b116c0faa 100644 --- a/examples/RoboticsLearning/KukaGraspExample.cpp +++ b/examples/RoboticsLearning/KukaGraspExample.cpp @@ -148,6 +148,7 @@ public: m_time+=dt; m_targetPos.setValue(0.4-0.4*b3Cos( m_time), 0, 0.8+0.4*b3Cos( m_time)); m_targetOri.setValue(0, 1.0, 0, 0); + m_targetPos.setValue(0.2*b3Cos( m_time), 0.2*b3Sin( m_time), 1.1); int numJoints = m_robotSim.getNumJoints(m_kukaIndex); @@ -195,7 +196,8 @@ public: ikargs.m_endEffectorTargetPosition[1] = targetPosDataOut.m_floats[1]; ikargs.m_endEffectorTargetPosition[2] = targetPosDataOut.m_floats[2]; - ikargs.m_flags |= /*B3_HAS_IK_TARGET_ORIENTATION +*/ B3_HAS_NULL_SPACE_VELOCITY; + //ikargs.m_flags |= B3_HAS_IK_TARGET_ORIENTATION; + ikargs.m_flags |= B3_HAS_JOINT_DAMPING; ikargs.m_endEffectorTargetOrientation[0] = targetOriDataOut.m_floats[0]; ikargs.m_endEffectorTargetOrientation[1] = targetOriDataOut.m_floats[1]; @@ -208,6 +210,7 @@ public: ikargs.m_upperLimits.resize(numJoints); ikargs.m_jointRanges.resize(numJoints); ikargs.m_restPoses.resize(numJoints); + ikargs.m_jointDamping.resize(numJoints,0.5); ikargs.m_lowerLimits[0] = -2.32; ikargs.m_lowerLimits[1] = -1.6; ikargs.m_lowerLimits[2] = -2.32; @@ -236,6 +239,7 @@ public: ikargs.m_restPoses[4] = 0; ikargs.m_restPoses[5] = -SIMD_HALF_PI*0.66; ikargs.m_restPoses[6] = 0; + ikargs.m_jointDamping[0] = 10.0; ikargs.m_numDegreeOfFreedom = numJoints; if (m_robotSim.calculateInverseKinematics(ikargs,ikresults)) diff --git a/examples/RoboticsLearning/b3RobotSimAPI.cpp b/examples/RoboticsLearning/b3RobotSimAPI.cpp index 3ac3c09a7..555ff13cc 100644 --- a/examples/RoboticsLearning/b3RobotSimAPI.cpp +++ b/examples/RoboticsLearning/b3RobotSimAPI.cpp @@ -516,7 +516,11 @@ bool b3RobotSimAPI::calculateInverseKinematics(const struct b3RobotSimInverseKin { b3CalculateInverseKinematicsAddTargetPurePosition(command,args.m_endEffectorLinkIndex,args.m_endEffectorTargetPosition); } - + + if (args.m_flags & B3_HAS_JOINT_DAMPING) + { + b3CalculateInverseKinematicsSetJointDamping(command, args.m_numDegreeOfFreedom, &args.m_jointDamping[0]); + } b3SharedMemoryStatusHandle statusHandle; statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClient, command); diff --git a/examples/RoboticsLearning/b3RobotSimAPI.h b/examples/RoboticsLearning/b3RobotSimAPI.h index 886aafcad..227053379 100644 --- a/examples/RoboticsLearning/b3RobotSimAPI.h +++ b/examples/RoboticsLearning/b3RobotSimAPI.h @@ -86,6 +86,7 @@ enum b3InverseKinematicsFlags { B3_HAS_IK_TARGET_ORIENTATION=1, B3_HAS_NULL_SPACE_VELOCITY=2, + B3_HAS_JOINT_DAMPING=4, }; struct b3RobotSimInverseKinematicArgs @@ -102,6 +103,7 @@ struct b3RobotSimInverseKinematicArgs b3AlignedObjectArray m_upperLimits; b3AlignedObjectArray m_jointRanges; b3AlignedObjectArray m_restPoses; + b3AlignedObjectArray m_jointDamping; b3RobotSimInverseKinematicArgs() :m_bodyUniqueId(-1), diff --git a/examples/SharedMemory/IKTrajectoryHelper.cpp b/examples/SharedMemory/IKTrajectoryHelper.cpp index a9af38164..0a6cbbf80 100644 --- a/examples/SharedMemory/IKTrajectoryHelper.cpp +++ b/examples/SharedMemory/IKTrajectoryHelper.cpp @@ -16,6 +16,7 @@ struct IKTrajectoryHelperInternalData { VectorR3 m_endEffectorTargetPosition; VectorRn m_nullSpaceVelocity; + VectorRn m_dampingCoeff; b3AlignedObjectArray m_ikNodes; @@ -23,6 +24,7 @@ struct IKTrajectoryHelperInternalData { m_endEffectorTargetPosition.SetZero(); m_nullSpaceVelocity.SetZero(); + m_dampingCoeff.SetZero(); } }; @@ -136,7 +138,9 @@ bool IKTrajectoryHelper::computeIK(const double endEffectorTargetPosition[3], case IK2_DLS: case IK2_VEL_DLS: case IK2_VEL_DLS_WITH_ORIENTATION: - ikJacobian.CalcDeltaThetasDLS(); // Damped least squares method + //ikJacobian.CalcDeltaThetasDLS(); // Damped least squares method + assert(m_data->m_dampingCoeff.GetLength()==numQ); + ikJacobian.CalcDeltaThetasDLS2(m_data->m_dampingCoeff); break; case IK2_VEL_DLS_WITH_NULLSPACE: case IK2_VEL_DLS_WITH_ORIENTATION_NULLSPACE: @@ -201,3 +205,14 @@ bool IKTrajectoryHelper::computeNullspaceVel(int numQ, const double* q_current, } return true; } + +bool IKTrajectoryHelper::setDampingCoeff(int numQ, const double* coeff) +{ + m_data->m_dampingCoeff.SetLength(numQ); + m_data->m_dampingCoeff.SetZero(); + for (int i = 0; i < numQ; ++i) + { + m_data->m_dampingCoeff[i] = coeff[i]; + } + return true; +} diff --git a/examples/SharedMemory/IKTrajectoryHelper.h b/examples/SharedMemory/IKTrajectoryHelper.h index 71f8eeaf4..90f2dc409 100644 --- a/examples/SharedMemory/IKTrajectoryHelper.h +++ b/examples/SharedMemory/IKTrajectoryHelper.h @@ -31,6 +31,7 @@ public: double* q_new, int ikMethod, const double* linear_jacobian, const double* angular_jacobian, int jacobian_size, const double dampIk[6]); bool computeNullspaceVel(int numQ, const double* q_current, const double* lower_limit, const double* upper_limit, const double* joint_range, const double* rest_pose); + bool setDampingCoeff(int numQ, const double* coeff); }; #endif //IK_TRAJECTORY_HELPER_H diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index 08be193e8..0138697c5 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -2341,6 +2341,19 @@ void b3CalculateInverseKinematicsPosOrnWithNullSpaceVel(b3SharedMemoryCommandHan } +void b3CalculateInverseKinematicsSetJointDamping(b3SharedMemoryCommandHandle commandHandle, int numDof, const double* jointDampingCoeff) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; + b3Assert(command); + b3Assert(command->m_type == CMD_CALCULATE_INVERSE_KINEMATICS); + command->m_updateFlags |= IK_HAS_JOINT_DAMPING; + + for (int i = 0; i < numDof; ++i) + { + command->m_calculateInverseKinematicsArguments.m_jointDamping[i] = jointDampingCoeff[i]; + } +} + int b3GetStatusInverseKinematicsJointPositions(b3SharedMemoryStatusHandle statusHandle, int* bodyUniqueId, int* dofCount, diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index dc972cfa1..febe08396 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -236,6 +236,7 @@ void b3CalculateInverseKinematicsAddTargetPurePosition(b3SharedMemoryCommandHand void b3CalculateInverseKinematicsAddTargetPositionWithOrientation(b3SharedMemoryCommandHandle commandHandle, int endEffectorLinkIndex, const double targetPosition[3], const double targetOrientation[4]); void b3CalculateInverseKinematicsPosWithNullSpaceVel(b3SharedMemoryCommandHandle commandHandle, int numDof, int endEffectorLinkIndex, const double targetPosition[3], const double* lowerLimit, const double* upperLimit, const double* jointRange, const double* restPose); 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); +void b3CalculateInverseKinematicsSetJointDamping(b3SharedMemoryCommandHandle commandHandle, int numDof, const double* jointDampingCoeff); int b3GetStatusInverseKinematicsJointPositions(b3SharedMemoryStatusHandle statusHandle, int* bodyUniqueId, int* dofCount, diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 26619153a..cd4260469 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -4011,12 +4011,30 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm endEffectorPosWorld.serializeDouble(endEffectorWorldPosition); endEffectorOri.serializeDouble(endEffectorWorldOrientation); - double dampIK[6] = { 1.0, 1.0, 1.0, 1.0, 1.0, 1.0 }; + + // Set joint damping coefficents. A small default + // damping constant is added to prevent singularity + // with pseudo inverse. The user can set joint damping + // coefficients differently for each joint. The larger + // the damping coefficient is, the less we rely on + // this joint to achieve the IK target. + btAlignedObjectArray joint_damping; + joint_damping.resize(numDofs,0.5); + if (clientCmd.m_updateFlags& IK_HAS_JOINT_DAMPING) + { + 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 }; ikHelperPtr->computeIK(clientCmd.m_calculateInverseKinematicsArguments.m_targetPosition, clientCmd.m_calculateInverseKinematicsArguments.m_targetOrientation, 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, dampIK); + &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;iMaxAngleDLS ) { + dTheta *= MaxAngleDLS/maxChange; + } +} + + void Jacobian::CalcDeltaThetasDLSwithSVD() { const MatrixRmn& J = ActiveJacobian(); diff --git a/examples/ThirdPartyLibs/BussIK/Jacobian.h b/examples/ThirdPartyLibs/BussIK/Jacobian.h index 441e6b08a..c5af4fe40 100644 --- a/examples/ThirdPartyLibs/BussIK/Jacobian.h +++ b/examples/ThirdPartyLibs/BussIK/Jacobian.h @@ -68,6 +68,7 @@ public: void CalcDeltaThetasTranspose(); void CalcDeltaThetasPseudoinverse(); void CalcDeltaThetasDLS(); + void CalcDeltaThetasDLS2(const VectorRn& dVec); void CalcDeltaThetasDLSwithSVD(); void CalcDeltaThetasSDLS(); void CalcDeltaThetasDLSwithNullspace( const VectorRn& desiredV); @@ -136,4 +137,4 @@ public: }; -#endif \ No newline at end of file +#endif diff --git a/examples/ThirdPartyLibs/BussIK/MatrixRmn.cpp b/examples/ThirdPartyLibs/BussIK/MatrixRmn.cpp index 76b65584f..ad3ceea5a 100644 --- a/examples/ThirdPartyLibs/BussIK/MatrixRmn.cpp +++ b/examples/ThirdPartyLibs/BussIK/MatrixRmn.cpp @@ -251,6 +251,18 @@ MatrixRmn& MatrixRmn::AddToDiagonal( double d ) // Adds d to each diagonal en return *this; } +// Add a vector to the entries on the diagonal +MatrixRmn& MatrixRmn::AddToDiagonal( const VectorRn& dVec ) // Adds dVec to the diagonal entries +{ + long diagLen = Min( NumRows, NumCols ); + double* dPtr = x; + for (int i = 0; i < diagLen && i < dVec.GetLength(); ++i) { + *dPtr += dVec[i]; + dPtr += NumRows+1; + } + return *this; +} + // Multiply two MatrixRmn's MatrixRmn& MatrixRmn::Multiply( const MatrixRmn& A, const MatrixRmn& B, MatrixRmn& dst ) { diff --git a/examples/ThirdPartyLibs/BussIK/MatrixRmn.h b/examples/ThirdPartyLibs/BussIK/MatrixRmn.h index 491d6340d..dd5506d89 100644 --- a/examples/ThirdPartyLibs/BussIK/MatrixRmn.h +++ b/examples/ThirdPartyLibs/BussIK/MatrixRmn.h @@ -110,6 +110,7 @@ public: // Miscellaneous operation MatrixRmn& AddToDiagonal( double d ); // Adds d to each diagonal + MatrixRmn& AddToDiagonal( const VectorRn& dVec); // Solving systems of linear equations void Solve( const VectorRn& b, VectorRn* x ) const; // Solves the equation (*this)*x = b; Uses row operations. Assumes *this is invertible. diff --git a/examples/pybullet/inverse_kinematics.py b/examples/pybullet/inverse_kinematics.py index f71707097..4c85ec48c 100644 --- a/examples/pybullet/inverse_kinematics.py +++ b/examples/pybullet/inverse_kinematics.py @@ -22,6 +22,8 @@ ul=[.967,2 ,2.96,2.29,2.96,2.09,3.05] jr=[5.8,4,5.8,4,5.8,4,6] #restposes for null space rp=[0,0,0,0.5*math.pi,0,-math.pi*0.5*0.66,0] +#joint damping coefficents +jd=[0.1,0.1,0.1,0.1,0.1,0.1,0.1] for i in range (numJoints): p.resetJointState(kukaId,i,rp[i]) @@ -64,7 +66,7 @@ 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) + jointPoses = p.calculateInverseKinematics(kukaId,kukaEndEffectorIndex,pos,orn,jointDamping=jd) else: jointPoses = p.calculateInverseKinematics(kukaId,kukaEndEffectorIndex,pos) diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index b1629971f..65cc7e108 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -4430,9 +4430,10 @@ static PyObject* pybullet_calculateInverseKinematics(PyObject* self, PyObject* upperLimitsObj = 0; PyObject* jointRangesObj = 0; PyObject* restPosesObj = 0; + PyObject* jointDampingObj = 0; - static char *kwlist[] = { "bodyIndex", "endEffectorLinkIndex", "targetPosition", "targetOrientation","lowerLimits", "upperLimits", "jointRanges", "restPoses", "physicsClientId", NULL }; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "iiO|OOOOOi", kwlist, &bodyIndex, &endEffectorLinkIndex, &targetPosObj,&targetOrnObj,&lowerLimitsObj, &upperLimitsObj, &jointRangesObj, &restPosesObj, &physicsClientId)) + static char *kwlist[] = { "bodyIndex", "endEffectorLinkIndex", "targetPosition", "targetOrientation","lowerLimits", "upperLimits", "jointRanges", "restPoses", "physicsClientId", "jointDamping", NULL }; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "iiO|OOOOOiO", kwlist, &bodyIndex, &endEffectorLinkIndex, &targetPosObj,&targetOrnObj,&lowerLimitsObj, &upperLimitsObj, &jointRangesObj, &restPosesObj, &physicsClientId, & jointDampingObj)) { return NULL; } @@ -4452,18 +4453,21 @@ static PyObject* pybullet_calculateInverseKinematics(PyObject* self, int szUpperLimits = upperLimitsObj? PySequence_Size(upperLimitsObj): 0; int szJointRanges = jointRangesObj? PySequence_Size(jointRangesObj):0; int szRestPoses = restPosesObj? PySequence_Size(restPosesObj):0; + int szJointDamping = jointDampingObj? PySequence_Size(jointDampingObj):0; int numJoints = b3GetNumJoints(sm, bodyIndex); int hasNullSpace = 0; + int hasJointDamping = 0; double* lowerLimits = 0; double* upperLimits = 0; double* jointRanges = 0; double* restPoses = 0; + double* jointDamping = 0; if (numJoints && (szLowerLimits == numJoints) && (szUpperLimits == numJoints) && - (szJointRanges == numJoints) && (szRestPoses == numJoints)) + (szJointRanges == numJoints) && (szRestPoses == numJoints)) { int szInBytes = sizeof(double) * numJoints; int i; @@ -4480,11 +4484,23 @@ static PyObject* pybullet_calculateInverseKinematics(PyObject* self, pybullet_internalGetFloatFromSequence(upperLimitsObj, i); jointRanges[i] = pybullet_internalGetFloatFromSequence(jointRangesObj, i); - restPoses[i] = + restPoses[i] = pybullet_internalGetFloatFromSequence(restPosesObj, i); } hasNullSpace = 1; } + + if (numJoints && (szJointDamping == numJoints)) + { + int szInBytes = sizeof(double) * numJoints; + int i; + jointDamping = (double*)malloc(szInBytes); + for (i = 0; i < numJoints; i++) + { + jointDamping[i] = pybullet_internalGetFloatFromSequence(jointDampingObj, i); + } + hasJointDamping = 1; + } if (hasPos) { @@ -4514,6 +4530,11 @@ static PyObject* pybullet_calculateInverseKinematics(PyObject* self, b3CalculateInverseKinematicsAddTargetPurePosition(command,endEffectorLinkIndex,pos); } } + + if (hasJointDamping) + { + b3CalculateInverseKinematicsSetJointDamping(command, numJoints, jointDamping); + } statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);