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

Add IK damping coefficient vector.

Browse Source
This commit is contained in:
YunfeiBai
2016-09-27 16:37:49 -07:00
parent 61fccba5be
commit 3910003e0a
6 changed files with 15 additions and 178 deletions
Download Patch File Download Diff File Expand all files Collapse all files

9
examples/SharedMemory/IKTrajectoryHelper.cpp
View File

@@ -43,7 +43,7 @@ bool IKTrajectoryHelper::computeIK(const double endEffectorTargetPosition[3],
const double endEffectorWorldPosition[3],
const double endEffectorWorldOrientation[4],
const double* q_current, int numQ,int endEffectorIndex,
double* q_new, int ikMethod, const double* linear_jacobian, const double* angular_jacobian, int jacobian_size, double dampIk)
double* q_new, int ikMethod, const double* linear_jacobian, const double* angular_jacobian, int jacobian_size, const double dampIk[6])
{
bool useAngularPart = (ikMethod==IK2_VEL_DLS_WITH_ORIENTATION) ? true : false;
@@ -69,7 +69,7 @@ bool IKTrajectoryHelper::computeIK(const double endEffectorTargetPosition[3],
VectorRn deltaS(3);
for (int i = 0; i < 3; ++i)
{
deltaS.Set(i,dampIk*(endEffectorTargetPosition[i]-endEffectorWorldPosition[i]));
deltaS.Set(i,dampIk[i]*(endEffectorTargetPosition[i]-endEffectorWorldPosition[i]));
}
// Set one end effector world orientation from Bullet
@@ -79,13 +79,12 @@ bool IKTrajectoryHelper::computeIK(const double endEffectorTargetPosition[3],
btQuaternion deltaQ = endQ*startQ.inverse();
float angle = deltaQ.getAngle();
btVector3 axis = deltaQ.getAxis();
float angleDot = angle*dampIk;
float angleDot = angle;
btVector3 angularVel = angleDot*axis.normalize();
for (int i = 0; i < 3; ++i)
{
deltaR.Set(i,angularVel[i]);
deltaR.Set(i,dampIk[i+3]*angularVel[i]);
}
deltaR[2] = 0.0;
{