diff --git a/examples/RoboticsLearning/IKTrajectoryHelper.cpp b/examples/RoboticsLearning/IKTrajectoryHelper.cpp index 2e97653f6..9c8ba195d 100644 --- a/examples/RoboticsLearning/IKTrajectoryHelper.cpp +++ b/examples/RoboticsLearning/IKTrajectoryHelper.cpp @@ -3,6 +3,7 @@ #include "BussIK/Tree.h" #include "BussIK/Jacobian.h" #include "BussIK/VectorRn.h" +#include "BussIK/MatrixRmn.h" #include "Bullet3Common/b3AlignedObjectArray.h" @@ -86,7 +87,7 @@ void IKTrajectoryHelper::createKukaIIWA() bool IKTrajectoryHelper::computeIK(const double endEffectorTargetPosition[3], const double* q_current, int numQ, - double* q_new, int ikMethod) + double* q_new, int ikMethod, const double* linear_jacobian, int jacobian_size) { if (numQ != 7) @@ -110,6 +111,20 @@ 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 Jacobian from Bullet body Jacobian + int nRow = m_data->m_ikJacobian->GetNumRows(); + int nCol = m_data->m_ikJacobian->GetNumCols(); + b3Assert(jacobian_size==nRow*nCol); + MatrixRmn linearJacobian(nRow,nCol); + for (int i = 0; i < nRow; ++i) + { + for (int j = 0; j < nCol; ++j) + { + linearJacobian.Set(i,j,linear_jacobian[i*nCol+j]); + } + } + m_data->m_ikJacobian->SetJendTrans(linearJacobian); + // Calculate the change in theta values switch (ikMethod) { case IK2_JACOB_TRANS: diff --git a/examples/RoboticsLearning/IKTrajectoryHelper.h b/examples/RoboticsLearning/IKTrajectoryHelper.h index 5b436057f..06ccfe0d8 100644 --- a/examples/RoboticsLearning/IKTrajectoryHelper.h +++ b/examples/RoboticsLearning/IKTrajectoryHelper.h @@ -24,7 +24,7 @@ public: bool computeIK(const double endEffectorTargetPosition[3], const double* q_old, int numQ, - double* q_new, int ikMethod); + double* q_new, int ikMethod, const double* linear_jacobian, int jacobian_size); }; #endif //IK_TRAJECTORY_HELPER_H diff --git a/examples/RoboticsLearning/KukaGraspExample.cpp b/examples/RoboticsLearning/KukaGraspExample.cpp index 31ae807eb..5c73c7900 100644 --- a/examples/RoboticsLearning/KukaGraspExample.cpp +++ b/examples/RoboticsLearning/KukaGraspExample.cpp @@ -166,16 +166,13 @@ public: // compute body Jacobian m_robotSim.getBodyJacobian(0, 6, local_position, q_current, qdot_current, qddot_current, jacobian_linear, jacobian_angular); - for (int i = 0; i < 21; ++i) { - printf("j%d: %f\n", i, jacobian_linear[i]); - } // m_robotSim.getJointInfo(m_kukaIndex,jointIndex,jointInfo); double q_new[7]; int ikMethod=IK2_SDLS; b3Vector3DoubleData dataOut; m_targetPos.serializeDouble(dataOut); - m_ikHelper.computeIK(dataOut.m_floats,q_current, numJoints, q_new, ikMethod); + m_ikHelper.computeIK(dataOut.m_floats,q_current, numJoints, q_new, ikMethod,jacobian_linear,(sizeof(jacobian_linear)/sizeof(*jacobian_linear))); printf("q_new = %f,%f,%f,%f,%f,%f,%f\n", q_new[0],q_new[1],q_new[2], q_new[3],q_new[4],q_new[5],q_new[6]); diff --git a/examples/ThirdPartyLibs/BussIK/Jacobian.h b/examples/ThirdPartyLibs/BussIK/Jacobian.h index 5f9bd08c5..99ecd19a0 100644 --- a/examples/ThirdPartyLibs/BussIK/Jacobian.h +++ b/examples/ThirdPartyLibs/BussIK/Jacobian.h @@ -83,6 +83,9 @@ public: static void CompareErrors( const Jacobian& j1, const Jacobian& j2, double* weightedDist1, double* weightedDist2 ); static void CountErrors( const Jacobian& j1, const Jacobian& j2, int* numBetter1, int* numBetter2, int* numTies ); + int GetNumRows() {return nRow;} + int GetNumCols() {return nCol;} + private: Tree* tree; // tree associated with this Jacobian matrix int nEffector; // Number of end effectors