diff --git a/examples/RoboticsLearning/KukaGraspExample.cpp b/examples/RoboticsLearning/KukaGraspExample.cpp index 5a96cc382..b116c0faa 100644 --- a/examples/RoboticsLearning/KukaGraspExample.cpp +++ b/examples/RoboticsLearning/KukaGraspExample.cpp @@ -196,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]; @@ -209,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; @@ -237,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/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 f1f7bd22f..cd4260469 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -4012,8 +4012,22 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm endEffectorPosWorld.serializeDouble(endEffectorWorldPosition); endEffectorOri.serializeDouble(endEffectorWorldOrientation); - double jointDampCoeff[7] = {20.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0}; - ikHelperPtr->setDampingCoeff(numDofs, jointDampCoeff); + // 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, diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index 1f599c685..847a0de75 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -512,7 +512,8 @@ enum EnumCalculateInverseKinematicsFlags IK_HAS_TARGET_POSITION=1, IK_HAS_TARGET_ORIENTATION=2, IK_HAS_NULL_SPACE_VELOCITY=4, - //IK_HAS_CURRENT_JOINT_POSITIONS=8,//not used yet + IK_HAS_JOINT_DAMPING=8, + //IK_HAS_CURRENT_JOINT_POSITIONS=16,//not used yet }; struct CalculateInverseKinematicsArgs @@ -526,6 +527,7 @@ struct CalculateInverseKinematicsArgs double m_upperLimit[MAX_DEGREE_OF_FREEDOM]; double m_jointRange[MAX_DEGREE_OF_FREEDOM]; double m_restPose[MAX_DEGREE_OF_FREEDOM]; + double m_jointDamping[MAX_DEGREE_OF_FREEDOM]; }; struct CalculateInverseKinematicsResultArgs