From 0022d0dafba6b7cffc8e1b1b040d19e1083e310c Mon Sep 17 00:00:00 2001 From: yunfeibai Date: Tue, 31 Jan 2017 22:58:37 -0800 Subject: [PATCH 1/6] Modify damped least square IK formulation. Test setting joint damping coefficients for IK. --- .../RoboticsLearning/KukaGraspExample.cpp | 3 ++- examples/SharedMemory/IKTrajectoryHelper.cpp | 16 ++++++++++++++- examples/SharedMemory/IKTrajectoryHelper.h | 1 + .../PhysicsServerCommandProcessor.cpp | 8 ++++++-- examples/ThirdPartyLibs/BussIK/Jacobian.cpp | 20 +++++++++++++++++++ examples/ThirdPartyLibs/BussIK/Jacobian.h | 3 ++- examples/ThirdPartyLibs/BussIK/MatrixRmn.cpp | 12 +++++++++++ examples/ThirdPartyLibs/BussIK/MatrixRmn.h | 1 + 8 files changed, 59 insertions(+), 5 deletions(-) diff --git a/examples/RoboticsLearning/KukaGraspExample.cpp b/examples/RoboticsLearning/KukaGraspExample.cpp index ebdea84d3..5a96cc382 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,7 @@ 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 +*/ B3_HAS_NULL_SPACE_VELOCITY; ikargs.m_endEffectorTargetOrientation[0] = targetOriDataOut.m_floats[0]; ikargs.m_endEffectorTargetOrientation[1] = targetOriDataOut.m_floats[1]; diff --git a/examples/SharedMemory/IKTrajectoryHelper.cpp b/examples/SharedMemory/IKTrajectoryHelper.cpp index a9af38164..f49571257 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,13 @@ 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]; + } +} 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/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 26619153a..f1f7bd22f 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -4011,12 +4011,16 @@ 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 }; + + double jointDampCoeff[7] = {20.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0}; + ikHelperPtr->setDampingCoeff(numDofs, jointDampCoeff); + + 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. From ce9378f8196afc43407816d3fefe17c331230856 Mon Sep 17 00:00:00 2001 From: yunfeibai Date: Fri, 3 Feb 2017 11:08:44 -0800 Subject: [PATCH 2/6] Add shared memory API and RobotSim API for setting joint damping in IK. --- examples/RoboticsLearning/KukaGraspExample.cpp | 5 ++++- examples/RoboticsLearning/b3RobotSimAPI.cpp | 6 +++++- examples/RoboticsLearning/b3RobotSimAPI.h | 2 ++ examples/SharedMemory/PhysicsClientC_API.cpp | 13 +++++++++++++ examples/SharedMemory/PhysicsClientC_API.h | 1 + .../PhysicsServerCommandProcessor.cpp | 18 ++++++++++++++++-- examples/SharedMemory/SharedMemoryCommands.h | 4 +++- 7 files changed, 44 insertions(+), 5 deletions(-) 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 From ac5a8aa2d622a41d2b1fc2ac60a8e13488200f65 Mon Sep 17 00:00:00 2001 From: yunfeibai Date: Fri, 3 Feb 2017 12:03:07 -0800 Subject: [PATCH 3/6] Set joint damping in pybullet. --- examples/pybullet/pybullet.c | 28 ++++++++++++++++++++++++---- 1 file changed, 24 insertions(+), 4 deletions(-) diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index 756894fb7..718f5cb9f 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -4364,9 +4364,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; } @@ -4386,18 +4387,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; @@ -4414,11 +4418,22 @@ 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; + jointDamping = (double*)malloc(szInBytes); + for (int i = 0; i < numJoints; i++) + { + jointDamping[i] = pybullet_internalGetFloatFromSequence(jointDampingObj, i); + } + hasJointDamping = 1; + } if (hasPos) { @@ -4448,6 +4463,11 @@ static PyObject* pybullet_calculateInverseKinematics(PyObject* self, b3CalculateInverseKinematicsAddTargetPurePosition(command,endEffectorLinkIndex,pos); } } + + if (hasJointDamping) + { + b3CalculateInverseKinematicsSetJointDamping(command, numJoints, jointDamping); + } statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); From abbd3ad8840f49a8b2f9ecfa81b571a01d6870f0 Mon Sep 17 00:00:00 2001 From: yunfeibai Date: Fri, 3 Feb 2017 12:46:49 -0800 Subject: [PATCH 4/6] minor fix. --- examples/SharedMemory/IKTrajectoryHelper.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/examples/SharedMemory/IKTrajectoryHelper.cpp b/examples/SharedMemory/IKTrajectoryHelper.cpp index f49571257..0a6cbbf80 100644 --- a/examples/SharedMemory/IKTrajectoryHelper.cpp +++ b/examples/SharedMemory/IKTrajectoryHelper.cpp @@ -214,4 +214,5 @@ bool IKTrajectoryHelper::setDampingCoeff(int numQ, const double* coeff) { m_data->m_dampingCoeff[i] = coeff[i]; } + return true; } From df07f2aaaa80db913e1f46ec46d69d2a7ba99a33 Mon Sep 17 00:00:00 2001 From: yunfeibai Date: Fri, 3 Feb 2017 14:44:25 -0800 Subject: [PATCH 5/6] minor fix --- examples/pybullet/pybullet.c | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index e600d0355..65cc7e108 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -4493,8 +4493,9 @@ static PyObject* pybullet_calculateInverseKinematics(PyObject* self, if (numJoints && (szJointDamping == numJoints)) { int szInBytes = sizeof(double) * numJoints; + int i; jointDamping = (double*)malloc(szInBytes); - for (int i = 0; i < numJoints; i++) + for (i = 0; i < numJoints; i++) { jointDamping[i] = pybullet_internalGetFloatFromSequence(jointDampingObj, i); } From be77fb269f9336c1ec683118faa2b54b24bc4460 Mon Sep 17 00:00:00 2001 From: yunfeibai Date: Fri, 3 Feb 2017 16:36:20 -0800 Subject: [PATCH 6/6] Add joint damping in pybullet IK example. --- examples/pybullet/inverse_kinematics.py | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) 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)