perform IK in local body-fixed frame
For now, Jacobian, mass matrix and inverse dynamics return results in local coordinates of the tree.
This commit is contained in:
@@ -3638,6 +3638,12 @@ B3_SHARED_API void b3CalculateInverseKinematicsAddTargetPurePosition(b3SharedMem
|
||||
command->m_calculateInverseKinematicsArguments.m_targetPosition[0] = targetPosition[0];
|
||||
command->m_calculateInverseKinematicsArguments.m_targetPosition[1] = targetPosition[1];
|
||||
command->m_calculateInverseKinematicsArguments.m_targetPosition[2] = targetPosition[2];
|
||||
|
||||
|
||||
command->m_calculateInverseKinematicsArguments.m_targetOrientation[0] = 0;
|
||||
command->m_calculateInverseKinematicsArguments.m_targetOrientation[1] = 0;
|
||||
command->m_calculateInverseKinematicsArguments.m_targetOrientation[2] = 0;
|
||||
command->m_calculateInverseKinematicsArguments.m_targetOrientation[3] = 1;
|
||||
|
||||
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user