diff --git a/examples/RoboticsLearning/KukaGraspExample.cpp b/examples/RoboticsLearning/KukaGraspExample.cpp index fa49f3e2e..a07d6f643 100644 --- a/examples/RoboticsLearning/KukaGraspExample.cpp +++ b/examples/RoboticsLearning/KukaGraspExample.cpp @@ -195,14 +195,49 @@ 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; - + + // 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)) { //copy the IK result to the desired state of the motor/actuator @@ -210,10 +245,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/RoboticsLearning/b3RobotSimAPI.cpp b/examples/RoboticsLearning/b3RobotSimAPI.cpp index 1a9d9d2c2..068bf1687 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) && (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], &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 + } 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], &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 f46e7ec3e..798b892e0 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,11 @@ struct b3RobotSimInverseKinematicArgs double m_endEffectorTargetOrientation[4]; int m_endEffectorLinkIndex; int m_flags; + int m_numDegreeOfFreedom; + 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 c4ee51b0e..89260d47d 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(); } }; @@ -45,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); @@ -74,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]); + } } { @@ -131,6 +135,11 @@ bool IKTrajectoryHelper::computeIK(const double endEffectorTargetPosition[3], case IK2_VEL_DLS_WITH_ORIENTATION: 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; case IK2_DLS_SVD: m_data->m_ikJacobian->CalcDeltaThetasDLSwithSVD(); break; @@ -164,3 +173,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, const double* rest_pose) +{ + 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 * (rest_pose[i]-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..71f8eeaf4 100644 --- a/examples/SharedMemory/IKTrajectoryHelper.h +++ b/examples/SharedMemory/IKTrajectoryHelper.h @@ -10,6 +10,8 @@ enum IK2_Method IK2_DLS_SVD, IK2_VEL_DLS, IK2_VEL_DLS_WITH_ORIENTATION, + IK2_VEL_DLS_WITH_NULLSPACE, + IK2_VEL_DLS_WITH_ORIENTATION_NULLSPACE, }; @@ -28,5 +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, const double* rest_pose); + }; #endif //IK_TRAJECTORY_HELPER_H diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index a9f7cc56f..95babb3a6 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -1391,6 +1391,53 @@ void b3CalculateInverseKinematicsAddTargetPositionWithOrientation(b3SharedMemory } +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); + 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]; + 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, const double* restPose) +{ + 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]; + command->m_calculateInverseKinematicsArguments.m_restPose[i] = restPose[i]; + } + +} int b3GetStatusInverseKinematicsJointPositions(b3SharedMemoryStatusHandle statusHandle, int* bodyUniqueId, diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index a998ddea4..dcd68c7d6 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, 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 de0da7109..f07803694 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -2665,7 +2665,43 @@ 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)&&(clientCmd.m_updateFlags&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_NULL_SPACE_VELOCITY) + { + 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]; + 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]); + } + } btVector3DoubleData endEffectorWorldPosition; btVector3DoubleData endEffectorWorldOrientation; diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index faefc25f5..adfd533f1 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,10 @@ 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]; + double m_restPose[MAX_DEGREE_OF_FREEDOM]; }; struct CalculateInverseKinematicsResultArgs diff --git a/examples/ThirdPartyLibs/BussIK/Jacobian.cpp b/examples/ThirdPartyLibs/BussIK/Jacobian.cpp index 6740733a5..c9944e3c3 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 VectorRn& desiredV) { const MatrixRmn& J = ActiveJacobian(); @@ -336,7 +336,28 @@ void Jacobian::CalcDeltaThetasDLS() // Use these two lines for the traditional DLS method U.Solve( dS, &dT1 ); J.MultiplyTranspose( dT1, dTheta ); - + + // Compute JInv in damped least square form + MatrixRmn UInv(U.GetNumRows(),U.GetNumColumns()); + U.ComputeInverse(UInv); + 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 ) { @@ -344,6 +365,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..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( const VectorRn& desiredV); void UpdateThetas(); void UpdateThetaDot(); diff --git a/examples/ThirdPartyLibs/BussIK/MatrixRmn.cpp b/examples/ThirdPartyLibs/BussIK/MatrixRmn.cpp index 365d90f6c..15b94ac23 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. @@ -943,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 4ce0c515e..491d6340d 100644 --- a/examples/ThirdPartyLibs/BussIK/MatrixRmn.h +++ b/examples/ThirdPartyLibs/BussIK/MatrixRmn.h @@ -129,6 +129,10 @@ 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; + // 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 );