Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Use body Jacobian from Bullet for IK.

Browse Source
This commit is contained in:
yunfeibai
2016-09-07 23:14:23 -07:00
parent f635c64205
commit c94a8e0d35
4 changed files with 21 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files

17
examples/RoboticsLearning/IKTrajectoryHelper.cpp
View File

@@ -3,6 +3,7 @@
#include "BussIK/Tree.h" #include "BussIK/Tree.h"
#include "BussIK/Jacobian.h" #include "BussIK/Jacobian.h"
#include "BussIK/VectorRn.h" #include "BussIK/VectorRn.h"
#include "BussIK/MatrixRmn.h"
#include "Bullet3Common/b3AlignedObjectArray.h" #include "Bullet3Common/b3AlignedObjectArray.h"
@@ -86,7 +87,7 @@ void IKTrajectoryHelper::createKukaIIWA()
bool IKTrajectoryHelper::computeIK(const double endEffectorTargetPosition[3], bool IKTrajectoryHelper::computeIK(const double endEffectorTargetPosition[3],
const double* q_current, int numQ, 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) if (numQ != 7)
@@ -110,6 +111,20 @@ bool IKTrajectoryHelper::computeIK(const double endEffectorTargetPosition[3],
targets.Set(endEffectorTargetPosition[0],endEffectorTargetPosition[1],endEffectorTargetPosition[2]); targets.Set(endEffectorTargetPosition[0],endEffectorTargetPosition[1],endEffectorTargetPosition[2]);
m_data->m_ikJacobian->ComputeJacobian(&targets); // Set up Jacobian and deltaS vectors 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 // Calculate the change in theta values
switch (ikMethod) { switch (ikMethod) {
case IK2_JACOB_TRANS: case IK2_JACOB_TRANS:

2
examples/RoboticsLearning/IKTrajectoryHelper.h
View File

@@ -24,7 +24,7 @@ public:
bool computeIK(const double endEffectorTargetPosition[3], bool computeIK(const double endEffectorTargetPosition[3],
const double* q_old, int numQ, 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 #endif //IK_TRAJECTORY_HELPER_H

5
examples/RoboticsLearning/KukaGraspExample.cpp
View File

@@ -166,16 +166,13 @@ public:
// compute body Jacobian // compute body Jacobian
m_robotSim.getBodyJacobian(0, 6, local_position, q_current, qdot_current, qddot_current, jacobian_linear, jacobian_angular); 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); // m_robotSim.getJointInfo(m_kukaIndex,jointIndex,jointInfo);
double q_new[7]; double q_new[7];
int ikMethod=IK2_SDLS; int ikMethod=IK2_SDLS;
b3Vector3DoubleData dataOut; b3Vector3DoubleData dataOut;
m_targetPos.serializeDouble(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], 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]); q_new[3],q_new[4],q_new[5],q_new[6]);

3
examples/ThirdPartyLibs/BussIK/Jacobian.h
View File

@@ -83,6 +83,9 @@ public:
static void CompareErrors( const Jacobian& j1, const Jacobian& j2, double* weightedDist1, double* weightedDist2 ); 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 ); static void CountErrors( const Jacobian& j1, const Jacobian& j2, int* numBetter1, int* numBetter2, int* numTies );
int GetNumRows() {return nRow;}
int GetNumCols() {return nCol;}
private: private:
Tree* tree; // tree associated with this Jacobian matrix Tree* tree; // tree associated with this Jacobian matrix
int nEffector; // Number of end effectors int nEffector; // Number of end effectors