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

Add desired null space velocity computation.

Browse Source
This commit is contained in:
yunfeibai
2016-09-29 22:45:31 -07:00
parent 94c7bbe8e3
commit 29f890ae10
5 changed files with 40 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files

35
examples/SharedMemory/IKTrajectoryHelper.cpp
View File

@@ -15,7 +15,7 @@
struct IKTrajectoryHelperInternalData
{
VectorR3 m_endEffectorTargetPosition;
VectorRn m_nullSpaceVelocity;
b3AlignedObjectArray<Node*> m_ikNodes;
Jacobian* m_ikJacobian;
@@ -23,6 +23,7 @@ struct IKTrajectoryHelperInternalData
IKTrajectoryHelperInternalData()
{
m_endEffectorTargetPosition.SetZero();
m_nullSpaceVelocity.SetZero();
}
};
@@ -129,8 +130,11 @@ bool IKTrajectoryHelper::computeIK(const double endEffectorTargetPosition[3],
case IK2_DLS:
case IK2_VEL_DLS:
case IK2_VEL_DLS_WITH_ORIENTATION:
//m_data->m_ikJacobian->CalcDeltaThetasDLS(); // Damped least squares method
m_data->m_ikJacobian->CalcDeltaThetasDLSwithNullspace();
m_data->m_ikJacobian->CalcDeltaThetasDLS(); // Damped least squares method
break;
case IK2_VEL_DLS_WITH_NULLSPACE:
assert(m_data->m_nullSpaceVelocity.GetLength()==numQ);
m_data->m_ikJacobian->CalcDeltaThetasDLSwithNullspace(m_data->m_nullSpaceVelocity);
break;
case IK2_DLS_SVD:
m_data->m_ikJacobian->CalcDeltaThetasDLSwithSVD();
@@ -165,3 +169,28 @@ 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)
{
m_data->m_nullSpaceVelocity.SetLength(numQ);
m_data->m_nullSpaceVelocity.SetZero();
double stayCloseToZeroGain = 0.1;
double stayAwayFromLimitsGain = 10.0;
// Stay close to zero
for (int i = 0; i < numQ; ++i)
{
m_data->m_nullSpaceVelocity[i] = -stayCloseToZeroGain * q_current[i];
}
// Stay away from joint limits
for (int i = 0; i < numQ; ++i) {
if (q_current[i] > upper_limit[i]) {
m_data->m_nullSpaceVelocity[i] += stayAwayFromLimitsGain * (upper_limit[i] - q_current[i]) / joint_range[i];
}
if (q_current[i] < lower_limit[i]) {
m_data->m_nullSpaceVelocity[i] += stayAwayFromLimitsGain * (lower_limit[i] - q_current[i]) / joint_range[i];
}
}
return true;
}