From 0ee12475af9083ee7ea42e69fda26308bbb8140a Mon Sep 17 00:00:00 2001 From: yunfeibai Date: Thu, 29 Sep 2016 15:45:57 -0700 Subject: [PATCH 1/7] Add matrix inverse computation in BussIK. --- examples/SharedMemory/IKTrajectoryHelper.cpp | 3 +- examples/ThirdPartyLibs/BussIK/Jacobian.cpp | 45 +++++++++++++++++++- examples/ThirdPartyLibs/BussIK/Jacobian.h | 2 +- examples/ThirdPartyLibs/BussIK/MatrixRmn.cpp | 30 +++++++++++++ examples/ThirdPartyLibs/BussIK/MatrixRmn.h | 2 + 5 files changed, 79 insertions(+), 3 deletions(-) diff --git a/examples/SharedMemory/IKTrajectoryHelper.cpp b/examples/SharedMemory/IKTrajectoryHelper.cpp index c4ee51b0e..ebad64f95 100644 --- a/examples/SharedMemory/IKTrajectoryHelper.cpp +++ b/examples/SharedMemory/IKTrajectoryHelper.cpp @@ -129,7 +129,8 @@ bool IKTrajectoryHelper::computeIK(const double endEffectorTargetPosition[3], case IK2_DLS: case IK2_VEL_DLS: case IK2_VEL_DLS_WITH_ORIENTATION: - m_data->m_ikJacobian->CalcDeltaThetasDLS(); // Damped least squares method + //m_data->m_ikJacobian->CalcDeltaThetasDLS(); // Damped least squares method + m_data->m_ikJacobian->CalcDeltaThetasDLSwithNullspace(); break; case IK2_DLS_SVD: m_data->m_ikJacobian->CalcDeltaThetasDLSwithSVD(); diff --git a/examples/ThirdPartyLibs/BussIK/Jacobian.cpp b/examples/ThirdPartyLibs/BussIK/Jacobian.cpp index 6740733a5..916d39765 100644 --- a/examples/ThirdPartyLibs/BussIK/Jacobian.cpp +++ b/examples/ThirdPartyLibs/BussIK/Jacobian.cpp @@ -320,7 +320,7 @@ void Jacobian::CalcDeltaThetasPseudoinverse() } -void Jacobian::CalcDeltaThetasDLS() +void Jacobian::CalcDeltaThetasDLSwithNullspace() { const MatrixRmn& J = ActiveJacobian(); @@ -336,6 +336,25 @@ void Jacobian::CalcDeltaThetasDLS() // Use these two lines for the traditional DLS method U.Solve( dS, &dT1 ); J.MultiplyTranspose( dT1, dTheta ); + + VectorRn nullV(7); + nullV.SetZero(); + nullV.Set(1, 2.0); + MatrixRmn I(U.GetNumRows(),U.GetNumColumns()); + I.SetIdentity(); + + MatrixRmn UInv(U.GetNumRows(),U.GetNumColumns()); + U.ComputeInverse(UInv); + MatrixRmn Res(U.GetNumRows(),U.GetNumColumns()); + MatrixRmn::Multiply(U, UInv, Res); + for (int i = 0; i < Res.GetNumRows(); ++i) + { + for (int j = 0; j < Res.GetNumColumns(); ++j) + { + printf("i%d j%d: %f\n", i, j, Res.Get(i, j)); + } + } + // Scale back to not exceed maximum angle changes double maxChange = dTheta.MaxAbs(); @@ -344,6 +363,30 @@ void Jacobian::CalcDeltaThetasDLS() } } +void Jacobian::CalcDeltaThetasDLS() +{ + const MatrixRmn& J = ActiveJacobian(); + + MatrixRmn::MultiplyTranspose(J, J, U); // U = J * (J^T) + U.AddToDiagonal( DampingLambdaSq ); + + // Use the next four lines instead of the succeeding two lines for the DLS method with clamped error vector e. + // CalcdTClampedFromdS(); + // VectorRn dTextra(3*m_nEffector); + // U.Solve( dT, &dTextra ); + // J.MultiplyTranspose( dTextra, dTheta ); + + // Use these two lines for the traditional DLS method + U.Solve( dS, &dT1 ); + J.MultiplyTranspose( dT1, dTheta ); + + // Scale back to not exceed maximum angle changes + double maxChange = dTheta.MaxAbs(); + if ( maxChange>MaxAngleDLS ) { + 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 e02a3bee4..549d362c3 100644 --- a/examples/ThirdPartyLibs/BussIK/Jacobian.h +++ b/examples/ThirdPartyLibs/BussIK/Jacobian.h @@ -70,7 +70,7 @@ public: void CalcDeltaThetasDLS(); void CalcDeltaThetasDLSwithSVD(); void CalcDeltaThetasSDLS(); - + void CalcDeltaThetasDLSwithNullspace(); void UpdateThetas(); void UpdateThetaDot(); diff --git a/examples/ThirdPartyLibs/BussIK/MatrixRmn.cpp b/examples/ThirdPartyLibs/BussIK/MatrixRmn.cpp index 365d90f6c..85e398628 100644 --- a/examples/ThirdPartyLibs/BussIK/MatrixRmn.cpp +++ b/examples/ThirdPartyLibs/BussIK/MatrixRmn.cpp @@ -513,6 +513,36 @@ void MatrixRmn::ComputeSVD( MatrixRmn& U, VectorRn& w, MatrixRmn& V ) const } +void MatrixRmn::ComputeInverse( MatrixRmn& R) const +{ + assert ( this->NumRows==this->NumCols ); + MatrixRmn U(this->NumRows, this->NumCols); + VectorRn w(this->NumRows); + MatrixRmn V(this->NumRows, this->NumCols); + + this->ComputeSVD(U, w, V); + + assert(this->DebugCheckSVD(U, w , V)); + + double PseudoInverseThresholdFactor = 0.01; + double pseudoInverseThreshold = PseudoInverseThresholdFactor*w.MaxAbs(); + + MatrixRmn VD(this->NumRows, this->NumCols); + MatrixRmn D(this->NumRows, this->NumCols); + D.SetZero(); + long diagLength = w.GetLength(); + double* wPtr = w.GetPtr(); + for ( long i = 0; i < diagLength; ++i ) { + double alpha = *(wPtr++); + if ( fabs(alpha)>pseudoInverseThreshold ) { + D.Set(i, i, 1.0/alpha); + } + } + + Multiply(V,D,VD); + MultiplyTranspose(VD,U,R); +} + // ************************************************ CalcBidiagonal ************************** // Helper routine for SVD computation // U is a matrix to be bidiagonalized. diff --git a/examples/ThirdPartyLibs/BussIK/MatrixRmn.h b/examples/ThirdPartyLibs/BussIK/MatrixRmn.h index 4ce0c515e..11625878a 100644 --- a/examples/ThirdPartyLibs/BussIK/MatrixRmn.h +++ b/examples/ThirdPartyLibs/BussIK/MatrixRmn.h @@ -129,6 +129,8 @@ public: void ComputeSVD( MatrixRmn& U, VectorRn& w, MatrixRmn& V ) const; // Good for debugging SVD computations (I recommend this be used for any new application to check for bugs/instability). bool DebugCheckSVD( const MatrixRmn& U, const VectorRn& w, const MatrixRmn& V ) const; + // Compute inverse of a matrix, the result is written in R + void ComputeInverse( MatrixRmn& R) const; // Some useful routines for experts who understand the inner workings of these classes. inline static double DotArray( long length, const double* ptrA, long strideA, const double* ptrB, long strideB ); From 94c7bbe8e339ba0715bc69b5fe8800a73fb8aef8 Mon Sep 17 00:00:00 2001 From: yunfeibai Date: Thu, 29 Sep 2016 17:12:51 -0700 Subject: [PATCH 2/7] Add null space task for IK. --- examples/ThirdPartyLibs/BussIK/Jacobian.cpp | 37 ++++++++++++-------- examples/ThirdPartyLibs/BussIK/MatrixRmn.cpp | 15 ++++++++ examples/ThirdPartyLibs/BussIK/MatrixRmn.h | 2 ++ 3 files changed, 39 insertions(+), 15 deletions(-) diff --git a/examples/ThirdPartyLibs/BussIK/Jacobian.cpp b/examples/ThirdPartyLibs/BussIK/Jacobian.cpp index 916d39765..238721a75 100644 --- a/examples/ThirdPartyLibs/BussIK/Jacobian.cpp +++ b/examples/ThirdPartyLibs/BussIK/Jacobian.cpp @@ -337,25 +337,32 @@ void Jacobian::CalcDeltaThetasDLSwithNullspace() U.Solve( dS, &dT1 ); J.MultiplyTranspose( dT1, dTheta ); - VectorRn nullV(7); - nullV.SetZero(); - nullV.Set(1, 2.0); - MatrixRmn I(U.GetNumRows(),U.GetNumColumns()); - I.SetIdentity(); + // Desired velocity + VectorRn desiredV(J.GetNumColumns()); + desiredV.SetZero(); + desiredV.Set(3, -0.2); + // Compute JInv in damped least square form MatrixRmn UInv(U.GetNumRows(),U.GetNumColumns()); U.ComputeInverse(UInv); - MatrixRmn Res(U.GetNumRows(),U.GetNumColumns()); - MatrixRmn::Multiply(U, UInv, Res); - for (int i = 0; i < Res.GetNumRows(); ++i) - { - for (int j = 0; j < Res.GetNumColumns(); ++j) - { - printf("i%d j%d: %f\n", i, j, Res.Get(i, j)); - } - } + assert(U.DebugCheckInverse(UInv)); + MatrixRmn JInv(J.GetNumColumns(), J.GetNumRows()); + MatrixRmn::TransposeMultiply(J, UInv, JInv); + + // Compute null space projection + MatrixRmn JInvJ(J.GetNumColumns(), J.GetNumColumns()); + MatrixRmn::Multiply(JInv, J, JInvJ); + MatrixRmn P(J.GetNumColumns(), J.GetNumColumns()); + P.SetIdentity(); + P -= JInvJ; + + // Compute null space velocity + VectorRn nullV(J.GetNumColumns()); + P.Multiply(desiredV, nullV); + + // Add null space velocity + dTheta += nullV; - // Scale back to not exceed maximum angle changes double maxChange = dTheta.MaxAbs(); if ( maxChange>MaxAngleDLS ) { diff --git a/examples/ThirdPartyLibs/BussIK/MatrixRmn.cpp b/examples/ThirdPartyLibs/BussIK/MatrixRmn.cpp index 85e398628..15b94ac23 100644 --- a/examples/ThirdPartyLibs/BussIK/MatrixRmn.cpp +++ b/examples/ThirdPartyLibs/BussIK/MatrixRmn.cpp @@ -973,6 +973,21 @@ bool MatrixRmn::DebugCheckSVD( const MatrixRmn& U, const VectorRn& w, const Matr return ret; } +bool MatrixRmn::DebugCheckInverse( const MatrixRmn& MInv ) const +{ + assert ( this->NumRows==this->NumCols ); + assert ( MInv.NumRows==MInv.NumCols ); + MatrixRmn I(this->NumRows, this->NumCols); + I.SetIdentity(); + MatrixRmn MMInv(this->NumRows, this->NumCols); + Multiply(*this, MInv, MMInv); + I -= MMInv; + double error = I.FrobeniusNorm(); + bool ret = ( fabs(error)<=1.0e-13 ); + assert ( ret ); + return ret; +} + bool MatrixRmn::DebugCalcBidiagCheck( const MatrixRmn& U, const VectorRn& w, const VectorRn& superDiag, const MatrixRmn& V ) const { // Special SVD test code diff --git a/examples/ThirdPartyLibs/BussIK/MatrixRmn.h b/examples/ThirdPartyLibs/BussIK/MatrixRmn.h index 11625878a..491d6340d 100644 --- a/examples/ThirdPartyLibs/BussIK/MatrixRmn.h +++ b/examples/ThirdPartyLibs/BussIK/MatrixRmn.h @@ -131,6 +131,8 @@ public: bool DebugCheckSVD( const MatrixRmn& U, const VectorRn& w, const MatrixRmn& V ) const; // Compute inverse of a matrix, the result is written in R void ComputeInverse( MatrixRmn& R) const; + // Debug matrix inverse computation + bool DebugCheckInverse( const MatrixRmn& MInv ) const; // Some useful routines for experts who understand the inner workings of these classes. inline static double DotArray( long length, const double* ptrA, long strideA, const double* ptrB, long strideB ); From 29f890ae1049b6a92e7d9be955f31d67899d0256 Mon Sep 17 00:00:00 2001 From: yunfeibai Date: Thu, 29 Sep 2016 22:45:31 -0700 Subject: [PATCH 3/7] Add desired null space velocity computation. --- examples/RoboticsLearning/b3RobotSimAPI.h | 3 ++ examples/SharedMemory/IKTrajectoryHelper.cpp | 35 ++++++++++++++++++-- examples/SharedMemory/IKTrajectoryHelper.h | 3 ++ examples/ThirdPartyLibs/BussIK/Jacobian.cpp | 7 +--- examples/ThirdPartyLibs/BussIK/Jacobian.h | 2 +- 5 files changed, 40 insertions(+), 10 deletions(-) diff --git a/examples/RoboticsLearning/b3RobotSimAPI.h b/examples/RoboticsLearning/b3RobotSimAPI.h index f46e7ec3e..bd650b33b 100644 --- a/examples/RoboticsLearning/b3RobotSimAPI.h +++ b/examples/RoboticsLearning/b3RobotSimAPI.h @@ -96,6 +96,9 @@ struct b3RobotSimInverseKinematicArgs double m_endEffectorTargetOrientation[4]; int m_endEffectorLinkIndex; int m_flags; + b3AlignedObjectArray m_lowerLimits; + b3AlignedObjectArray m_upperLimits; + b3AlignedObjectArray m_jointRanges; b3RobotSimInverseKinematicArgs() :m_bodyUniqueId(-1), diff --git a/examples/SharedMemory/IKTrajectoryHelper.cpp b/examples/SharedMemory/IKTrajectoryHelper.cpp index ebad64f95..113aa84ae 100644 --- a/examples/SharedMemory/IKTrajectoryHelper.cpp +++ b/examples/SharedMemory/IKTrajectoryHelper.cpp @@ -15,7 +15,7 @@ struct IKTrajectoryHelperInternalData { VectorR3 m_endEffectorTargetPosition; - + VectorRn m_nullSpaceVelocity; b3AlignedObjectArray m_ikNodes; Jacobian* m_ikJacobian; @@ -23,6 +23,7 @@ struct IKTrajectoryHelperInternalData IKTrajectoryHelperInternalData() { m_endEffectorTargetPosition.SetZero(); + m_nullSpaceVelocity.SetZero(); } }; @@ -129,8 +130,11 @@ bool IKTrajectoryHelper::computeIK(const double endEffectorTargetPosition[3], case IK2_DLS: case IK2_VEL_DLS: case IK2_VEL_DLS_WITH_ORIENTATION: - //m_data->m_ikJacobian->CalcDeltaThetasDLS(); // Damped least squares method - m_data->m_ikJacobian->CalcDeltaThetasDLSwithNullspace(); + m_data->m_ikJacobian->CalcDeltaThetasDLS(); // Damped least squares method + break; + case IK2_VEL_DLS_WITH_NULLSPACE: + assert(m_data->m_nullSpaceVelocity.GetLength()==numQ); + m_data->m_ikJacobian->CalcDeltaThetasDLSwithNullspace(m_data->m_nullSpaceVelocity); break; case IK2_DLS_SVD: m_data->m_ikJacobian->CalcDeltaThetasDLSwithSVD(); @@ -165,3 +169,28 @@ bool IKTrajectoryHelper::computeIK(const double endEffectorTargetPosition[3], } return true; } + +bool IKTrajectoryHelper::computeNullspaceVel(int numQ, const double* q_current, const double* lower_limit, const double* upper_limit, const double* joint_range) +{ + m_data->m_nullSpaceVelocity.SetLength(numQ); + m_data->m_nullSpaceVelocity.SetZero(); + double stayCloseToZeroGain = 0.1; + double stayAwayFromLimitsGain = 10.0; + + // Stay close to zero + for (int i = 0; i < numQ; ++i) + { + m_data->m_nullSpaceVelocity[i] = -stayCloseToZeroGain * q_current[i]; + } + + // Stay away from joint limits + for (int i = 0; i < numQ; ++i) { + if (q_current[i] > upper_limit[i]) { + m_data->m_nullSpaceVelocity[i] += stayAwayFromLimitsGain * (upper_limit[i] - q_current[i]) / joint_range[i]; + } + if (q_current[i] < lower_limit[i]) { + m_data->m_nullSpaceVelocity[i] += stayAwayFromLimitsGain * (lower_limit[i] - q_current[i]) / joint_range[i]; + } + } + return true; +} \ No newline at end of file diff --git a/examples/SharedMemory/IKTrajectoryHelper.h b/examples/SharedMemory/IKTrajectoryHelper.h index c8bb761c2..76145e122 100644 --- a/examples/SharedMemory/IKTrajectoryHelper.h +++ b/examples/SharedMemory/IKTrajectoryHelper.h @@ -10,6 +10,7 @@ enum IK2_Method IK2_DLS_SVD, IK2_VEL_DLS, IK2_VEL_DLS_WITH_ORIENTATION, + IK2_VEL_DLS_WITH_NULLSPACE, }; @@ -28,5 +29,7 @@ public: const double* q_old, 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 computeNullspaceVel(int numQ, const double* q_current, const double* lower_limit, const double* upper_limit, const double* joint_range); + }; #endif //IK_TRAJECTORY_HELPER_H diff --git a/examples/ThirdPartyLibs/BussIK/Jacobian.cpp b/examples/ThirdPartyLibs/BussIK/Jacobian.cpp index 238721a75..c9944e3c3 100644 --- a/examples/ThirdPartyLibs/BussIK/Jacobian.cpp +++ b/examples/ThirdPartyLibs/BussIK/Jacobian.cpp @@ -320,7 +320,7 @@ void Jacobian::CalcDeltaThetasPseudoinverse() } -void Jacobian::CalcDeltaThetasDLSwithNullspace() +void Jacobian::CalcDeltaThetasDLSwithNullspace(const VectorRn& desiredV) { const MatrixRmn& J = ActiveJacobian(); @@ -337,11 +337,6 @@ void Jacobian::CalcDeltaThetasDLSwithNullspace() U.Solve( dS, &dT1 ); J.MultiplyTranspose( dT1, dTheta ); - // Desired velocity - VectorRn desiredV(J.GetNumColumns()); - desiredV.SetZero(); - desiredV.Set(3, -0.2); - // Compute JInv in damped least square form MatrixRmn UInv(U.GetNumRows(),U.GetNumColumns()); U.ComputeInverse(UInv); diff --git a/examples/ThirdPartyLibs/BussIK/Jacobian.h b/examples/ThirdPartyLibs/BussIK/Jacobian.h index 549d362c3..441e6b08a 100644 --- a/examples/ThirdPartyLibs/BussIK/Jacobian.h +++ b/examples/ThirdPartyLibs/BussIK/Jacobian.h @@ -70,7 +70,7 @@ public: void CalcDeltaThetasDLS(); void CalcDeltaThetasDLSwithSVD(); void CalcDeltaThetasSDLS(); - void CalcDeltaThetasDLSwithNullspace(); + void CalcDeltaThetasDLSwithNullspace( const VectorRn& desiredV); void UpdateThetas(); void UpdateThetaDot(); From fd3cb061cbc59d5fd669c557bfd59b8438841b94 Mon Sep 17 00:00:00 2001 From: yunfeibai Date: Fri, 30 Sep 2016 00:05:00 -0700 Subject: [PATCH 4/7] Expose IK with null space task to shared memory API and RobotSimAPI. --- .../RoboticsLearning/KukaGraspExample.cpp | 27 ++++++++++- examples/RoboticsLearning/b3RobotSimAPI.cpp | 10 ++++- examples/RoboticsLearning/b3RobotSimAPI.h | 2 + examples/SharedMemory/IKTrajectoryHelper.cpp | 3 +- examples/SharedMemory/IKTrajectoryHelper.h | 1 + examples/SharedMemory/PhysicsClientC_API.cpp | 45 +++++++++++++++++++ examples/SharedMemory/PhysicsClientC_API.h | 2 + .../PhysicsServerCommandProcessor.cpp | 35 ++++++++++++++- examples/SharedMemory/SharedMemoryCommands.h | 6 ++- 9 files changed, 124 insertions(+), 7 deletions(-) diff --git a/examples/RoboticsLearning/KukaGraspExample.cpp b/examples/RoboticsLearning/KukaGraspExample.cpp index fa49f3e2e..e3c71c738 100644 --- a/examples/RoboticsLearning/KukaGraspExample.cpp +++ b/examples/RoboticsLearning/KukaGraspExample.cpp @@ -195,14 +195,37 @@ 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; + 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]; ikargs.m_endEffectorTargetOrientation[2] = targetOriDataOut.m_floats[2]; ikargs.m_endEffectorTargetOrientation[3] = targetOriDataOut.m_floats[3]; ikargs.m_endEffectorLinkIndex = 6; - + + ikargs.m_lowerLimits.push_back(-2.32); + ikargs.m_lowerLimits.push_back(-1.6); + ikargs.m_lowerLimits.push_back(-2.32); + ikargs.m_lowerLimits.push_back(-1.6); + ikargs.m_lowerLimits.push_back(-2.32); + ikargs.m_lowerLimits.push_back(-1.6); + ikargs.m_lowerLimits.push_back(-2.4); + ikargs.m_upperLimits.push_back(2.32); + ikargs.m_upperLimits.push_back(1.6); + ikargs.m_upperLimits.push_back(2.32); + ikargs.m_upperLimits.push_back(1.6); + ikargs.m_upperLimits.push_back(2.32); + ikargs.m_upperLimits.push_back(1.6); + ikargs.m_upperLimits.push_back(2.4); + ikargs.m_jointRanges.push_back(5.8); + ikargs.m_jointRanges.push_back(4); + ikargs.m_jointRanges.push_back(5.8); + ikargs.m_jointRanges.push_back(4); + ikargs.m_jointRanges.push_back(5.8); + ikargs.m_jointRanges.push_back(4); + ikargs.m_jointRanges.push_back(6); + ikargs.m_numDegreeOfFreedom = numJoints; + if (m_robotSim.calculateInverseKinematics(ikargs,ikresults)) { //copy the IK result to the desired state of the motor/actuator diff --git a/examples/RoboticsLearning/b3RobotSimAPI.cpp b/examples/RoboticsLearning/b3RobotSimAPI.cpp index 1a9d9d2c2..e11103dcf 100644 --- a/examples/RoboticsLearning/b3RobotSimAPI.cpp +++ b/examples/RoboticsLearning/b3RobotSimAPI.cpp @@ -485,10 +485,16 @@ bool b3RobotSimAPI::calculateInverseKinematics(const struct b3RobotSimInverseKin b3SharedMemoryCommandHandle command = b3CalculateInverseKinematicsCommandInit(m_data->m_physicsClient,args.m_bodyUniqueId); - if (args.m_flags & B3_HAS_IK_TARGET_ORIENTATION) + if (args.m_flags & (B3_HAS_IK_TARGET_ORIENTATION + B3_HAS_NULL_SPACE_VELOCITY)) + { + b3CalculateInverseKinematicsPosOrnWithNullSpaceVel(command, args.m_numDegreeOfFreedom, args.m_endEffectorLinkIndex, args.m_endEffectorTargetPosition, args.m_endEffectorTargetOrientation, &args.m_lowerLimits[0], &args.m_upperLimits[0], &args.m_jointRanges[0]); + } else if (args.m_flags & B3_HAS_IK_TARGET_ORIENTATION) { b3CalculateInverseKinematicsAddTargetPositionWithOrientation(command,args.m_endEffectorLinkIndex,args.m_endEffectorTargetPosition,args.m_endEffectorTargetOrientation); - } else + } else if (args.m_flags & B3_HAS_NULL_SPACE_VELOCITY) + { + b3CalculateInverseKinematicsPosWithNullSpaceVel(command, args.m_numDegreeOfFreedom, args.m_endEffectorLinkIndex, args.m_endEffectorTargetPosition, &args.m_lowerLimits[0], &args.m_upperLimits[0], &args.m_jointRanges[0]); + } else { b3CalculateInverseKinematicsAddTargetPurePosition(command,args.m_endEffectorLinkIndex,args.m_endEffectorTargetPosition); } diff --git a/examples/RoboticsLearning/b3RobotSimAPI.h b/examples/RoboticsLearning/b3RobotSimAPI.h index bd650b33b..aa9fbd0eb 100644 --- a/examples/RoboticsLearning/b3RobotSimAPI.h +++ b/examples/RoboticsLearning/b3RobotSimAPI.h @@ -85,6 +85,7 @@ struct b3JointMotorArgs enum b3InverseKinematicsFlags { B3_HAS_IK_TARGET_ORIENTATION=1, + B3_HAS_NULL_SPACE_VELOCITY=2, }; struct b3RobotSimInverseKinematicArgs @@ -96,6 +97,7 @@ struct b3RobotSimInverseKinematicArgs double m_endEffectorTargetOrientation[4]; int m_endEffectorLinkIndex; int m_flags; + int m_numDegreeOfFreedom; b3AlignedObjectArray m_lowerLimits; b3AlignedObjectArray m_upperLimits; b3AlignedObjectArray m_jointRanges; diff --git a/examples/SharedMemory/IKTrajectoryHelper.cpp b/examples/SharedMemory/IKTrajectoryHelper.cpp index 113aa84ae..8ae7e64b1 100644 --- a/examples/SharedMemory/IKTrajectoryHelper.cpp +++ b/examples/SharedMemory/IKTrajectoryHelper.cpp @@ -46,7 +46,7 @@ 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) ? true : false; + bool useAngularPart = (ikMethod==IK2_VEL_DLS_WITH_ORIENTATION || ikMethod==IK2_VEL_DLS_WITH_ORIENTATION_NULLSPACE) ? true : false; m_data->m_ikJacobian = new Jacobian(useAngularPart,numQ); @@ -133,6 +133,7 @@ bool IKTrajectoryHelper::computeIK(const double endEffectorTargetPosition[3], m_data->m_ikJacobian->CalcDeltaThetasDLS(); // Damped least squares method break; case IK2_VEL_DLS_WITH_NULLSPACE: + case IK2_VEL_DLS_WITH_ORIENTATION_NULLSPACE: assert(m_data->m_nullSpaceVelocity.GetLength()==numQ); m_data->m_ikJacobian->CalcDeltaThetasDLSwithNullspace(m_data->m_nullSpaceVelocity); break; diff --git a/examples/SharedMemory/IKTrajectoryHelper.h b/examples/SharedMemory/IKTrajectoryHelper.h index 76145e122..762777d84 100644 --- a/examples/SharedMemory/IKTrajectoryHelper.h +++ b/examples/SharedMemory/IKTrajectoryHelper.h @@ -11,6 +11,7 @@ enum IK2_Method IK2_VEL_DLS, IK2_VEL_DLS_WITH_ORIENTATION, IK2_VEL_DLS_WITH_NULLSPACE, + IK2_VEL_DLS_WITH_ORIENTATION_NULLSPACE, }; diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index a9f7cc56f..923c98c6d 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -1391,6 +1391,51 @@ void b3CalculateInverseKinematicsAddTargetPositionWithOrientation(b3SharedMemory } +void b3CalculateInverseKinematicsPosWithNullSpaceVel(b3SharedMemoryCommandHandle commandHandle, int numDof, int endEffectorLinkIndex, const double targetPosition[3], const double* lowerLimit, const double* upperLimit, const double* jointRange) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; + b3Assert(command); + b3Assert(command->m_type == CMD_CALCULATE_INVERSE_KINEMATICS); + command->m_updateFlags |= IK_HAS_TARGET_POSITION+IK_HAS_NULL_SPACE_VELOCITY; + command->m_calculateInverseKinematicsArguments.m_endEffectorLinkIndex = endEffectorLinkIndex; + + command->m_calculateInverseKinematicsArguments.m_targetPosition[0] = targetPosition[0]; + command->m_calculateInverseKinematicsArguments.m_targetPosition[1] = targetPosition[1]; + command->m_calculateInverseKinematicsArguments.m_targetPosition[2] = targetPosition[2]; + + for (int i = 0; i < numDof; ++i) + { + command->m_calculateInverseKinematicsArguments.m_lowerLimit[i] = lowerLimit[i]; + command->m_calculateInverseKinematicsArguments.m_upperLimit[i] = upperLimit[i]; + command->m_calculateInverseKinematicsArguments.m_jointRange[i] = jointRange[i]; + } +} + +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) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; + b3Assert(command); + b3Assert(command->m_type == CMD_CALCULATE_INVERSE_KINEMATICS); + command->m_updateFlags |= IK_HAS_TARGET_POSITION+IK_HAS_TARGET_ORIENTATION+IK_HAS_NULL_SPACE_VELOCITY; + command->m_calculateInverseKinematicsArguments.m_endEffectorLinkIndex = endEffectorLinkIndex; + + command->m_calculateInverseKinematicsArguments.m_targetPosition[0] = targetPosition[0]; + command->m_calculateInverseKinematicsArguments.m_targetPosition[1] = targetPosition[1]; + command->m_calculateInverseKinematicsArguments.m_targetPosition[2] = targetPosition[2]; + + command->m_calculateInverseKinematicsArguments.m_targetOrientation[0] = targetOrientation[0]; + command->m_calculateInverseKinematicsArguments.m_targetOrientation[1] = targetOrientation[1]; + command->m_calculateInverseKinematicsArguments.m_targetOrientation[2] = targetOrientation[2]; + command->m_calculateInverseKinematicsArguments.m_targetOrientation[3] = targetOrientation[3]; + + for (int i = 0; i < numDof; ++i) + { + command->m_calculateInverseKinematicsArguments.m_lowerLimit[i] = lowerLimit[i]; + command->m_calculateInverseKinematicsArguments.m_upperLimit[i] = upperLimit[i]; + command->m_calculateInverseKinematicsArguments.m_jointRange[i] = jointRange[i]; + } + +} int b3GetStatusInverseKinematicsJointPositions(b3SharedMemoryStatusHandle statusHandle, int* bodyUniqueId, diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index a998ddea4..4952afd60 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -133,6 +133,8 @@ int b3GetStatusJacobian(b3SharedMemoryStatusHandle statusHandle, double* linearJ b3SharedMemoryCommandHandle b3CalculateInverseKinematicsCommandInit(b3PhysicsClientHandle physClient, int bodyIndex); void b3CalculateInverseKinematicsAddTargetPurePosition(b3SharedMemoryCommandHandle commandHandle, int endEffectorLinkIndex, const double targetPosition[3]); 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); +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); int b3GetStatusInverseKinematicsJointPositions(b3SharedMemoryStatusHandle statusHandle, int* bodyUniqueId, int* dofCount, diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index de0da7109..9f3160895 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -2665,7 +2665,40 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm btAlignedObjectArray q_new; q_new.resize(numDofs); - int ikMethod= (clientCmd.m_updateFlags& IK_HAS_TARGET_ORIENTATION)? IK2_VEL_DLS_WITH_ORIENTATION : IK2_VEL_DLS; + int ikMethod = 0; + if (clientCmd.m_updateFlags& (IK_HAS_TARGET_ORIENTATION+IK_HAS_NULL_SPACE_VELOCITY)) + { + ikMethod = IK2_VEL_DLS_WITH_ORIENTATION_NULLSPACE; + } + else if (clientCmd.m_updateFlags& IK_HAS_TARGET_ORIENTATION) + { + ikMethod = IK2_VEL_DLS_WITH_ORIENTATION; + } + else if (clientCmd.m_updateFlags& IK_HAS_NULL_SPACE_VELOCITY) + { + ikMethod = IK2_VEL_DLS_WITH_NULLSPACE; + } + else + { + ikMethod = IK2_VEL_DLS; + } + + if (clientCmd.m_updateFlags& IK_HAS_TARGET_ORIENTATION) + { + btAlignedObjectArray lower_limit; + btAlignedObjectArray upper_limit; + btAlignedObjectArray joint_range; + lower_limit.resize(numDofs); + upper_limit.resize(numDofs); + joint_range.resize(numDofs); + for (int i = 0; i < numDofs; ++i) + { + lower_limit[i] = clientCmd.m_calculateInverseKinematicsArguments.m_lowerLimit[i]; + upper_limit[i] = clientCmd.m_calculateInverseKinematicsArguments.m_upperLimit[i]; + joint_range[i] = clientCmd.m_calculateInverseKinematicsArguments.m_jointRange[i]; + ikHelperPtr->computeNullspaceVel(numDofs, &q_current[0], &lower_limit[0], &upper_limit[0], &joint_range[0]); + } + } btVector3DoubleData endEffectorWorldPosition; btVector3DoubleData endEffectorWorldOrientation; diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index faefc25f5..ff66ec13f 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -395,7 +395,8 @@ enum EnumCalculateInverseKinematicsFlags { IK_HAS_TARGET_POSITION=1, IK_HAS_TARGET_ORIENTATION=2, - //IK_HAS_CURRENT_JOINT_POSITIONS=4,//not used yet + IK_HAS_NULL_SPACE_VELOCITY=4, + //IK_HAS_CURRENT_JOINT_POSITIONS=8,//not used yet }; struct CalculateInverseKinematicsArgs @@ -405,6 +406,9 @@ struct CalculateInverseKinematicsArgs double m_targetPosition[3]; double m_targetOrientation[4];//orientation represented as quaternion, x,y,z,w int m_endEffectorLinkIndex; + double m_lowerLimit[MAX_DEGREE_OF_FREEDOM]; + double m_upperLimit[MAX_DEGREE_OF_FREEDOM]; + double m_jointRange[MAX_DEGREE_OF_FREEDOM]; }; struct CalculateInverseKinematicsResultArgs From ee00696ae9bc13a6910cd0bffaeb35ff84acbc81 Mon Sep 17 00:00:00 2001 From: yunfeibai Date: Fri, 30 Sep 2016 00:15:51 -0700 Subject: [PATCH 5/7] Test four IK modes: with and without orientation constraint, with and without null space task. --- examples/RoboticsLearning/b3RobotSimAPI.cpp | 2 +- examples/SharedMemory/IKTrajectoryHelper.cpp | 21 +++++++++++-------- .../PhysicsServerCommandProcessor.cpp | 4 ++-- 3 files changed, 15 insertions(+), 12 deletions(-) diff --git a/examples/RoboticsLearning/b3RobotSimAPI.cpp b/examples/RoboticsLearning/b3RobotSimAPI.cpp index e11103dcf..84edd5198 100644 --- a/examples/RoboticsLearning/b3RobotSimAPI.cpp +++ b/examples/RoboticsLearning/b3RobotSimAPI.cpp @@ -485,7 +485,7 @@ bool b3RobotSimAPI::calculateInverseKinematics(const struct b3RobotSimInverseKin b3SharedMemoryCommandHandle command = b3CalculateInverseKinematicsCommandInit(m_data->m_physicsClient,args.m_bodyUniqueId); - if (args.m_flags & (B3_HAS_IK_TARGET_ORIENTATION + B3_HAS_NULL_SPACE_VELOCITY)) + if ((args.m_flags & B3_HAS_IK_TARGET_ORIENTATION) && (args.m_flags & B3_HAS_NULL_SPACE_VELOCITY)) { b3CalculateInverseKinematicsPosOrnWithNullSpaceVel(command, args.m_numDegreeOfFreedom, args.m_endEffectorLinkIndex, args.m_endEffectorTargetPosition, args.m_endEffectorTargetOrientation, &args.m_lowerLimits[0], &args.m_upperLimits[0], &args.m_jointRanges[0]); } else if (args.m_flags & B3_HAS_IK_TARGET_ORIENTATION) diff --git a/examples/SharedMemory/IKTrajectoryHelper.cpp b/examples/SharedMemory/IKTrajectoryHelper.cpp index 8ae7e64b1..f671c5951 100644 --- a/examples/SharedMemory/IKTrajectoryHelper.cpp +++ b/examples/SharedMemory/IKTrajectoryHelper.cpp @@ -75,16 +75,19 @@ bool IKTrajectoryHelper::computeIK(const double endEffectorTargetPosition[3], // Set one end effector world orientation from Bullet VectorRn deltaR(3); - btQuaternion startQ(endEffectorWorldOrientation[0],endEffectorWorldOrientation[1],endEffectorWorldOrientation[2],endEffectorWorldOrientation[3]); - btQuaternion endQ(endEffectorTargetOrientation[0],endEffectorTargetOrientation[1],endEffectorTargetOrientation[2],endEffectorTargetOrientation[3]); - btQuaternion deltaQ = endQ*startQ.inverse(); - float angle = deltaQ.getAngle(); - btVector3 axis = deltaQ.getAxis(); - float angleDot = angle; - btVector3 angularVel = angleDot*axis.normalize(); - for (int i = 0; i < 3; ++i) + if (useAngularPart) { - deltaR.Set(i,dampIk[i+3]*angularVel[i]); + btQuaternion startQ(endEffectorWorldOrientation[0],endEffectorWorldOrientation[1],endEffectorWorldOrientation[2],endEffectorWorldOrientation[3]); + btQuaternion endQ(endEffectorTargetOrientation[0],endEffectorTargetOrientation[1],endEffectorTargetOrientation[2],endEffectorTargetOrientation[3]); + btQuaternion deltaQ = endQ*startQ.inverse(); + float angle = deltaQ.getAngle(); + btVector3 axis = deltaQ.getAxis(); + float angleDot = angle; + btVector3 angularVel = angleDot*axis.normalize(); + for (int i = 0; i < 3; ++i) + { + deltaR.Set(i,dampIk[i+3]*angularVel[i]); + } } { diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 9f3160895..edda7b3be 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -2666,7 +2666,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm btAlignedObjectArray q_new; q_new.resize(numDofs); int ikMethod = 0; - if (clientCmd.m_updateFlags& (IK_HAS_TARGET_ORIENTATION+IK_HAS_NULL_SPACE_VELOCITY)) + if ((clientCmd.m_updateFlags& IK_HAS_TARGET_ORIENTATION)&&(clientCmd.m_updateFlags&IK_HAS_NULL_SPACE_VELOCITY)) { ikMethod = IK2_VEL_DLS_WITH_ORIENTATION_NULLSPACE; } @@ -2683,7 +2683,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm ikMethod = IK2_VEL_DLS; } - if (clientCmd.m_updateFlags& IK_HAS_TARGET_ORIENTATION) + if (clientCmd.m_updateFlags& IK_HAS_NULL_SPACE_VELOCITY) { btAlignedObjectArray lower_limit; btAlignedObjectArray upper_limit; From 1c1d3db26dadbb92c4885b77195593bcb6f82937 Mon Sep 17 00:00:00 2001 From: yunfeibai Date: Fri, 30 Sep 2016 00:43:15 -0700 Subject: [PATCH 6/7] An example to compare with and without null space task in IK. --- examples/RoboticsLearning/KukaGraspExample.cpp | 8 ++++---- examples/SharedMemory/IKTrajectoryHelper.cpp | 4 +++- 2 files changed, 7 insertions(+), 5 deletions(-) diff --git a/examples/RoboticsLearning/KukaGraspExample.cpp b/examples/RoboticsLearning/KukaGraspExample.cpp index e3c71c738..998e045db 100644 --- a/examples/RoboticsLearning/KukaGraspExample.cpp +++ b/examples/RoboticsLearning/KukaGraspExample.cpp @@ -195,7 +195,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]; @@ -233,10 +233,10 @@ public: { b3JointMotorArgs t(CONTROL_MODE_POSITION_VELOCITY_PD); t.m_targetPosition = ikresults.m_calculatedJointPositions[i]; - t.m_maxTorqueValue = 100; - t.m_kp= 1; + t.m_maxTorqueValue = 100.0; + t.m_kp= 1.0; t.m_targetVelocity = 0; - t.m_kp = 0.5; + t.m_kd = 1.0; m_robotSim.setJointMotorControl(m_kukaIndex,i,t); } diff --git a/examples/SharedMemory/IKTrajectoryHelper.cpp b/examples/SharedMemory/IKTrajectoryHelper.cpp index f671c5951..7c9793c7f 100644 --- a/examples/SharedMemory/IKTrajectoryHelper.cpp +++ b/examples/SharedMemory/IKTrajectoryHelper.cpp @@ -181,10 +181,12 @@ bool IKTrajectoryHelper::computeNullspaceVel(int numQ, const double* q_current, double stayCloseToZeroGain = 0.1; double stayAwayFromLimitsGain = 10.0; + double q_rest[7] = {0, 0, 0, SIMD_HALF_PI, 0, -SIMD_HALF_PI*0.66, 0}; + // Stay close to zero for (int i = 0; i < numQ; ++i) { - m_data->m_nullSpaceVelocity[i] = -stayCloseToZeroGain * q_current[i]; + m_data->m_nullSpaceVelocity[i] = stayCloseToZeroGain * (q_rest[i]-q_current[i]); } // Stay away from joint limits From 7e8d8b04883533ee06e578a68b8e56c422c8c6e1 Mon Sep 17 00:00:00 2001 From: yunfeibai Date: Fri, 30 Sep 2016 01:03:40 -0700 Subject: [PATCH 7/7] Expose rest pose for null space task to API. --- .../RoboticsLearning/KukaGraspExample.cpp | 54 +++++++++++-------- examples/RoboticsLearning/b3RobotSimAPI.cpp | 4 +- examples/RoboticsLearning/b3RobotSimAPI.h | 1 + examples/SharedMemory/IKTrajectoryHelper.cpp | 6 +-- examples/SharedMemory/IKTrajectoryHelper.h | 2 +- examples/SharedMemory/PhysicsClientC_API.cpp | 6 ++- examples/SharedMemory/PhysicsClientC_API.h | 4 +- .../PhysicsServerCommandProcessor.cpp | 5 +- examples/SharedMemory/SharedMemoryCommands.h | 1 + 9 files changed, 50 insertions(+), 33 deletions(-) diff --git a/examples/RoboticsLearning/KukaGraspExample.cpp b/examples/RoboticsLearning/KukaGraspExample.cpp index 998e045db..a07d6f643 100644 --- a/examples/RoboticsLearning/KukaGraspExample.cpp +++ b/examples/RoboticsLearning/KukaGraspExample.cpp @@ -203,27 +203,39 @@ public: ikargs.m_endEffectorTargetOrientation[3] = targetOriDataOut.m_floats[3]; ikargs.m_endEffectorLinkIndex = 6; - ikargs.m_lowerLimits.push_back(-2.32); - ikargs.m_lowerLimits.push_back(-1.6); - ikargs.m_lowerLimits.push_back(-2.32); - ikargs.m_lowerLimits.push_back(-1.6); - ikargs.m_lowerLimits.push_back(-2.32); - ikargs.m_lowerLimits.push_back(-1.6); - ikargs.m_lowerLimits.push_back(-2.4); - ikargs.m_upperLimits.push_back(2.32); - ikargs.m_upperLimits.push_back(1.6); - ikargs.m_upperLimits.push_back(2.32); - ikargs.m_upperLimits.push_back(1.6); - ikargs.m_upperLimits.push_back(2.32); - ikargs.m_upperLimits.push_back(1.6); - ikargs.m_upperLimits.push_back(2.4); - ikargs.m_jointRanges.push_back(5.8); - ikargs.m_jointRanges.push_back(4); - ikargs.m_jointRanges.push_back(5.8); - ikargs.m_jointRanges.push_back(4); - ikargs.m_jointRanges.push_back(5.8); - ikargs.m_jointRanges.push_back(4); - ikargs.m_jointRanges.push_back(6); + // Settings based on default KUKA arm setting + ikargs.m_lowerLimits.resize(numJoints); + ikargs.m_upperLimits.resize(numJoints); + ikargs.m_jointRanges.resize(numJoints); + ikargs.m_restPoses.resize(numJoints); + ikargs.m_lowerLimits[0] = -2.32; + ikargs.m_lowerLimits[1] = -1.6; + ikargs.m_lowerLimits[2] = -2.32; + ikargs.m_lowerLimits[3] = -1.6; + ikargs.m_lowerLimits[4] = -2.32; + ikargs.m_lowerLimits[5] = -1.6; + ikargs.m_lowerLimits[6] = -2.4; + ikargs.m_upperLimits[0] = 2.32; + ikargs.m_upperLimits[1] = 1.6; + ikargs.m_upperLimits[2] = 2.32; + ikargs.m_upperLimits[3] = 1.6; + ikargs.m_upperLimits[4] = 2.32; + ikargs.m_upperLimits[5] = 1.6; + ikargs.m_upperLimits[6] = 2.4; + ikargs.m_jointRanges[0] = 5.8; + ikargs.m_jointRanges[1] = 4; + ikargs.m_jointRanges[2] = 5.8; + ikargs.m_jointRanges[3] = 4; + ikargs.m_jointRanges[4] = 5.8; + ikargs.m_jointRanges[5] = 4; + ikargs.m_jointRanges[6] = 6; + ikargs.m_restPoses[0] = 0; + ikargs.m_restPoses[1] = 0; + ikargs.m_restPoses[2] = 0; + ikargs.m_restPoses[3] = SIMD_HALF_PI; + ikargs.m_restPoses[4] = 0; + ikargs.m_restPoses[5] = -SIMD_HALF_PI*0.66; + ikargs.m_restPoses[6] = 0; ikargs.m_numDegreeOfFreedom = numJoints; if (m_robotSim.calculateInverseKinematics(ikargs,ikresults)) diff --git a/examples/RoboticsLearning/b3RobotSimAPI.cpp b/examples/RoboticsLearning/b3RobotSimAPI.cpp index 84edd5198..068bf1687 100644 --- a/examples/RoboticsLearning/b3RobotSimAPI.cpp +++ b/examples/RoboticsLearning/b3RobotSimAPI.cpp @@ -487,13 +487,13 @@ bool b3RobotSimAPI::calculateInverseKinematics(const struct b3RobotSimInverseKin b3SharedMemoryCommandHandle command = b3CalculateInverseKinematicsCommandInit(m_data->m_physicsClient,args.m_bodyUniqueId); if ((args.m_flags & B3_HAS_IK_TARGET_ORIENTATION) && (args.m_flags & B3_HAS_NULL_SPACE_VELOCITY)) { - b3CalculateInverseKinematicsPosOrnWithNullSpaceVel(command, args.m_numDegreeOfFreedom, args.m_endEffectorLinkIndex, args.m_endEffectorTargetPosition, args.m_endEffectorTargetOrientation, &args.m_lowerLimits[0], &args.m_upperLimits[0], &args.m_jointRanges[0]); + b3CalculateInverseKinematicsPosOrnWithNullSpaceVel(command, args.m_numDegreeOfFreedom, args.m_endEffectorLinkIndex, args.m_endEffectorTargetPosition, args.m_endEffectorTargetOrientation, &args.m_lowerLimits[0], &args.m_upperLimits[0], &args.m_jointRanges[0], &args.m_restPoses[0]); } else if (args.m_flags & B3_HAS_IK_TARGET_ORIENTATION) { b3CalculateInverseKinematicsAddTargetPositionWithOrientation(command,args.m_endEffectorLinkIndex,args.m_endEffectorTargetPosition,args.m_endEffectorTargetOrientation); } else if (args.m_flags & B3_HAS_NULL_SPACE_VELOCITY) { - b3CalculateInverseKinematicsPosWithNullSpaceVel(command, args.m_numDegreeOfFreedom, args.m_endEffectorLinkIndex, args.m_endEffectorTargetPosition, &args.m_lowerLimits[0], &args.m_upperLimits[0], &args.m_jointRanges[0]); + b3CalculateInverseKinematicsPosWithNullSpaceVel(command, args.m_numDegreeOfFreedom, args.m_endEffectorLinkIndex, args.m_endEffectorTargetPosition, &args.m_lowerLimits[0], &args.m_upperLimits[0], &args.m_jointRanges[0], &args.m_restPoses[0]); } else { b3CalculateInverseKinematicsAddTargetPurePosition(command,args.m_endEffectorLinkIndex,args.m_endEffectorTargetPosition); diff --git a/examples/RoboticsLearning/b3RobotSimAPI.h b/examples/RoboticsLearning/b3RobotSimAPI.h index aa9fbd0eb..798b892e0 100644 --- a/examples/RoboticsLearning/b3RobotSimAPI.h +++ b/examples/RoboticsLearning/b3RobotSimAPI.h @@ -101,6 +101,7 @@ struct b3RobotSimInverseKinematicArgs b3AlignedObjectArray m_lowerLimits; b3AlignedObjectArray m_upperLimits; b3AlignedObjectArray m_jointRanges; + b3AlignedObjectArray m_restPoses; b3RobotSimInverseKinematicArgs() :m_bodyUniqueId(-1), diff --git a/examples/SharedMemory/IKTrajectoryHelper.cpp b/examples/SharedMemory/IKTrajectoryHelper.cpp index 7c9793c7f..89260d47d 100644 --- a/examples/SharedMemory/IKTrajectoryHelper.cpp +++ b/examples/SharedMemory/IKTrajectoryHelper.cpp @@ -174,19 +174,17 @@ bool IKTrajectoryHelper::computeIK(const double endEffectorTargetPosition[3], return true; } -bool IKTrajectoryHelper::computeNullspaceVel(int numQ, const double* q_current, const double* lower_limit, const double* upper_limit, const double* joint_range) +bool IKTrajectoryHelper::computeNullspaceVel(int numQ, const double* q_current, const double* lower_limit, const double* upper_limit, const double* joint_range, const double* rest_pose) { m_data->m_nullSpaceVelocity.SetLength(numQ); m_data->m_nullSpaceVelocity.SetZero(); double stayCloseToZeroGain = 0.1; double stayAwayFromLimitsGain = 10.0; - - double q_rest[7] = {0, 0, 0, SIMD_HALF_PI, 0, -SIMD_HALF_PI*0.66, 0}; // Stay close to zero for (int i = 0; i < numQ; ++i) { - m_data->m_nullSpaceVelocity[i] = stayCloseToZeroGain * (q_rest[i]-q_current[i]); + m_data->m_nullSpaceVelocity[i] = stayCloseToZeroGain * (rest_pose[i]-q_current[i]); } // Stay away from joint limits diff --git a/examples/SharedMemory/IKTrajectoryHelper.h b/examples/SharedMemory/IKTrajectoryHelper.h index 762777d84..71f8eeaf4 100644 --- a/examples/SharedMemory/IKTrajectoryHelper.h +++ b/examples/SharedMemory/IKTrajectoryHelper.h @@ -30,7 +30,7 @@ public: const double* q_old, 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 computeNullspaceVel(int numQ, const double* q_current, const double* lower_limit, const double* upper_limit, const double* joint_range); + bool computeNullspaceVel(int numQ, const double* q_current, const double* lower_limit, const double* upper_limit, const double* joint_range, const double* rest_pose); }; #endif //IK_TRAJECTORY_HELPER_H diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index 923c98c6d..95babb3a6 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -1391,7 +1391,7 @@ void b3CalculateInverseKinematicsAddTargetPositionWithOrientation(b3SharedMemory } -void b3CalculateInverseKinematicsPosWithNullSpaceVel(b3SharedMemoryCommandHandle commandHandle, int numDof, int endEffectorLinkIndex, const double targetPosition[3], const double* lowerLimit, const double* upperLimit, const double* jointRange) +void b3CalculateInverseKinematicsPosWithNullSpaceVel(b3SharedMemoryCommandHandle commandHandle, int numDof, int endEffectorLinkIndex, const double targetPosition[3], const double* lowerLimit, const double* upperLimit, const double* jointRange, const double* restPose) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; b3Assert(command); @@ -1408,10 +1408,11 @@ void b3CalculateInverseKinematicsPosWithNullSpaceVel(b3SharedMemoryCommandHandle command->m_calculateInverseKinematicsArguments.m_lowerLimit[i] = lowerLimit[i]; command->m_calculateInverseKinematicsArguments.m_upperLimit[i] = upperLimit[i]; command->m_calculateInverseKinematicsArguments.m_jointRange[i] = jointRange[i]; + command->m_calculateInverseKinematicsArguments.m_restPose[i] = restPose[i]; } } -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) +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) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; b3Assert(command); @@ -1433,6 +1434,7 @@ void b3CalculateInverseKinematicsPosOrnWithNullSpaceVel(b3SharedMemoryCommandHan command->m_calculateInverseKinematicsArguments.m_lowerLimit[i] = lowerLimit[i]; command->m_calculateInverseKinematicsArguments.m_upperLimit[i] = upperLimit[i]; command->m_calculateInverseKinematicsArguments.m_jointRange[i] = jointRange[i]; + command->m_calculateInverseKinematicsArguments.m_restPose[i] = restPose[i]; } } diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index 4952afd60..dcd68c7d6 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -133,8 +133,8 @@ int b3GetStatusJacobian(b3SharedMemoryStatusHandle statusHandle, double* linearJ b3SharedMemoryCommandHandle b3CalculateInverseKinematicsCommandInit(b3PhysicsClientHandle physClient, int bodyIndex); void b3CalculateInverseKinematicsAddTargetPurePosition(b3SharedMemoryCommandHandle commandHandle, int endEffectorLinkIndex, const double targetPosition[3]); 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); -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); +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); int b3GetStatusInverseKinematicsJointPositions(b3SharedMemoryStatusHandle statusHandle, int* bodyUniqueId, int* dofCount, diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index edda7b3be..f07803694 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -2688,15 +2688,18 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm btAlignedObjectArray lower_limit; btAlignedObjectArray upper_limit; btAlignedObjectArray joint_range; + btAlignedObjectArray rest_pose; lower_limit.resize(numDofs); upper_limit.resize(numDofs); joint_range.resize(numDofs); + rest_pose.resize(numDofs); for (int i = 0; i < numDofs; ++i) { lower_limit[i] = clientCmd.m_calculateInverseKinematicsArguments.m_lowerLimit[i]; upper_limit[i] = clientCmd.m_calculateInverseKinematicsArguments.m_upperLimit[i]; joint_range[i] = clientCmd.m_calculateInverseKinematicsArguments.m_jointRange[i]; - ikHelperPtr->computeNullspaceVel(numDofs, &q_current[0], &lower_limit[0], &upper_limit[0], &joint_range[0]); + rest_pose[i] = clientCmd.m_calculateInverseKinematicsArguments.m_restPose[i]; + ikHelperPtr->computeNullspaceVel(numDofs, &q_current[0], &lower_limit[0], &upper_limit[0], &joint_range[0], &rest_pose[0]); } } diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index ff66ec13f..adfd533f1 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -409,6 +409,7 @@ struct CalculateInverseKinematicsArgs double m_lowerLimit[MAX_DEGREE_OF_FREEDOM]; double m_upperLimit[MAX_DEGREE_OF_FREEDOM]; double m_jointRange[MAX_DEGREE_OF_FREEDOM]; + double m_restPose[MAX_DEGREE_OF_FREEDOM]; }; struct CalculateInverseKinematicsResultArgs