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();