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

Compute delta S from Bullet link state.

Browse Source
This commit is contained in:
yunfeibai
2016-09-10 19:18:29 -07:00
parent 1b72b91bcf
commit a00841a498
4 changed files with 16 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files

10
examples/RoboticsLearning/IKTrajectoryHelper.cpp
View File

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

2
examples/RoboticsLearning/KukaGraspExample.cpp
View File

@@ -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;