diff --git a/examples/RoboticsLearning/IKTrajectoryHelper.cpp b/examples/RoboticsLearning/IKTrajectoryHelper.cpp index a1be4d831..94f6600c9 100644 --- a/examples/RoboticsLearning/IKTrajectoryHelper.cpp +++ b/examples/RoboticsLearning/IKTrajectoryHelper.cpp @@ -112,8 +112,14 @@ bool IKTrajectoryHelper::computeIK(const double endEffectorTargetPosition[3], targets.Set(endEffectorTargetPosition[0],endEffectorTargetPosition[1],endEffectorTargetPosition[2]); m_data->m_ikJacobian->ComputeJacobian(&targets); // Set up Jacobian and deltaS vectors - // Set end effector world position from Bullet - + // Set one end effector world position from Bullet + VectorRn deltaS(3); + for (int i = 0; i < 3; ++i) + { + deltaS.Set(i,endEffectorTargetPosition[i]-endEffectorWorldPosition[i]); + } + m_data->m_ikJacobian->SetDeltaS(deltaS); + // Set Jacobian from Bullet body Jacobian int nRow = m_data->m_ikJacobian->GetNumRows(); int nCol = m_data->m_ikJacobian->GetNumCols(); diff --git a/examples/RoboticsLearning/KukaGraspExample.cpp b/examples/RoboticsLearning/KukaGraspExample.cpp index 3dcae9271..cb4b6c521 100644 --- a/examples/RoboticsLearning/KukaGraspExample.cpp +++ b/examples/RoboticsLearning/KukaGraspExample.cpp @@ -175,7 +175,7 @@ public: // m_robotSim.getJointInfo(m_kukaIndex,jointIndex,jointInfo); double q_new[7]; - int ikMethod=IK2_SDLS; + int ikMethod=IK2_DLS; b3Vector3DoubleData dataOut; m_targetPos.serializeDouble(dataOut); b3Vector3DoubleData worldPosDataOut; diff --git a/examples/ThirdPartyLibs/BussIK/Jacobian.cpp b/examples/ThirdPartyLibs/BussIK/Jacobian.cpp index 0a42ad3d4..68f31158b 100644 --- a/examples/ThirdPartyLibs/BussIK/Jacobian.cpp +++ b/examples/ThirdPartyLibs/BussIK/Jacobian.cpp @@ -144,6 +144,11 @@ void Jacobian::SetJendTrans(MatrixRmn& J) Jend.LoadAsSubmatrix(J); } +void Jacobian::SetDeltaS(VectorRn& S) +{ + dS.Set(S); +} + // The delta theta values have been computed in dTheta array // Apply the delta theta values to the joints // Nothing is done about joint limits for now. diff --git a/examples/ThirdPartyLibs/BussIK/Jacobian.h b/examples/ThirdPartyLibs/BussIK/Jacobian.h index 99ecd19a0..3f62b2da2 100644 --- a/examples/ThirdPartyLibs/BussIK/Jacobian.h +++ b/examples/ThirdPartyLibs/BussIK/Jacobian.h @@ -60,6 +60,7 @@ public: void SetJendActive() { Jactive = &Jend; } // The default setting is Jend. void SetJtargetActive() { Jactive = &Jtarget; } void SetJendTrans(MatrixRmn& J); + void SetDeltaS(VectorRn& S); void CalcDeltaThetas(); // Use this only if the Current Mode has been set. void ZeroDeltaThetas(); @@ -86,7 +87,7 @@ public: int GetNumRows() {return nRow;} int GetNumCols() {return nCol;} -private: +public: Tree* tree; // tree associated with this Jacobian matrix int nEffector; // Number of end effectors int nJoint; // Number of joints