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

8
examples/RoboticsLearning/IKTrajectoryHelper.cpp
View File

@@ -112,7 +112,13 @@ 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 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 // Set Jacobian from Bullet body Jacobian
int nRow = m_data->m_ikJacobian->GetNumRows(); int nRow = m_data->m_ikJacobian->GetNumRows();

2
examples/RoboticsLearning/KukaGraspExample.cpp
View File

@@ -175,7 +175,7 @@ public:
// 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_DLS;
b3Vector3DoubleData dataOut; b3Vector3DoubleData dataOut;
m_targetPos.serializeDouble(dataOut); m_targetPos.serializeDouble(dataOut);
b3Vector3DoubleData worldPosDataOut; b3Vector3DoubleData worldPosDataOut;

5
examples/ThirdPartyLibs/BussIK/Jacobian.cpp
View File

@@ -144,6 +144,11 @@ void Jacobian::SetJendTrans(MatrixRmn& J)
Jend.LoadAsSubmatrix(J); Jend.LoadAsSubmatrix(J);
} }
void Jacobian::SetDeltaS(VectorRn& S)
{
dS.Set(S);
}
// The delta theta values have been computed in dTheta array // The delta theta values have been computed in dTheta array
// Apply the delta theta values to the joints // Apply the delta theta values to the joints
// Nothing is done about joint limits for now. // Nothing is done about joint limits for now.

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

@@ -60,6 +60,7 @@ public:
void SetJendActive() { Jactive = &Jend; } // The default setting is Jend. void SetJendActive() { Jactive = &Jend; } // The default setting is Jend.
void SetJtargetActive() { Jactive = &Jtarget; } void SetJtargetActive() { Jactive = &Jtarget; }
void SetJendTrans(MatrixRmn& J); void SetJendTrans(MatrixRmn& J);
void SetDeltaS(VectorRn& S);
void CalcDeltaThetas(); // Use this only if the Current Mode has been set. void CalcDeltaThetas(); // Use this only if the Current Mode has been set.
void ZeroDeltaThetas(); void ZeroDeltaThetas();
@@ -86,7 +87,7 @@ public:
int GetNumRows() {return nRow;} int GetNumRows() {return nRow;}
int GetNumCols() {return nCol;} int GetNumCols() {return nCol;}
private: public:
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
int nJoint; // Number of joints int nJoint; // Number of joints