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

Expose rest pose for null space task to API.

Browse Source
This commit is contained in:
yunfeibai
2016-09-30 01:03:40 -07:00
parent 1c1d3db26d
commit 7e8d8b0488
9 changed files with 50 additions and 33 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
examples/SharedMemory/IKTrajectoryHelper.cpp
View File

@@ -174,19 +174,17 @@ bool IKTrajectoryHelper::computeIK(const double endEffectorTargetPosition[3],
return true;
}
bool IKTrajectoryHelper::computeNullspaceVel(int numQ, const double* q_current, const double* lower_limit, const double* upper_limit, const double* joint_range)
bool IKTrajectoryHelper::computeNullspaceVel(int numQ, const double* q_current, const double* lower_limit, const double* upper_limit, const double* joint_range, const double* rest_pose)
{
m_data->m_nullSpaceVelocity.SetLength(numQ);
m_data->m_nullSpaceVelocity.SetZero();
double stayCloseToZeroGain = 0.1;
double stayAwayFromLimitsGain = 10.0;
double q_rest[7] = {0, 0, 0, SIMD_HALF_PI, 0, -SIMD_HALF_PI*0.66, 0};
// Stay close to zero
for (int i = 0; i < numQ; ++i)
{
m_data->m_nullSpaceVelocity[i] = stayCloseToZeroGain * (q_rest[i]-q_current[i]);
m_data->m_nullSpaceVelocity[i] = stayCloseToZeroGain * (rest_pose[i]-q_current[i]);
}
// Stay away from joint limits