From 0022d0dafba6b7cffc8e1b1b040d19e1083e310c Mon Sep 17 00:00:00 2001 From: yunfeibai Date: Tue, 31 Jan 2017 22:58:37 -0800 Subject: [PATCH] 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.