void IK_QElbowSegment::Lock(int dof, IK_QJacobian& jacobian, MT_Vector3& delta)
{
if (dof == 0) {
m_locked[0] = true;
jacobian.Lock(m_DoF_id, delta[0]);
}
else {
m_locked[1] = true;
jacobian.Lock(m_DoF_id+1, delta[1]);
}
}
void IK_QSphericalSegment::Lock(int dof, IK_QJacobian& jacobian, MT_Vector3& delta)
{
if (dof == 1) {
m_locked[1] = true;
jacobian.Lock(m_DoF_id+1, delta[1]);
}
else {
m_locked[0] = m_locked[2] = true;
jacobian.Lock(m_DoF_id, delta[0]);
jacobian.Lock(m_DoF_id+2, delta[2]);
}
}
void IK_QJacobian::SubTask(IK_QJacobian& jacobian)
{
if (!ComputeNullProjection())
return;
// restrict lower priority jacobian
jacobian.Restrict(m_d_theta, m_null);
// add angle update from lower priority
jacobian.Invert();
// note: now damps secondary angles with minimum damping value from
// SDLS, to avoid shaking when the primary task is near singularities,
// doesn't work well at all
int i;
for (i = 0; i < m_d_theta.size(); i++)
m_d_theta[i] = m_d_theta[i] + /*m_min_damp**/jacobian.AngleUpdate(i);
}
bool IK_QElbowSegment::UpdateAngle(const IK_QJacobian &jacobian, MT_Vector3& delta, bool *clamp)
{
if (m_locked[0] && m_locked[1])
return false;
clamp[0] = clamp[1] = false;
if (!m_locked[0]) {
m_new_angle = m_angle + jacobian.AngleUpdate(m_DoF_id);
if (m_limit) {
if (m_new_angle > m_max) {
delta[0] = m_max - m_angle;
m_new_angle = m_max;
clamp[0] = true;
}
else if (m_new_angle < m_min) {
delta[0] = m_min - m_angle;
m_new_angle = m_min;
clamp[0] = true;
}
}
}
if (!m_locked[1]) {
m_new_twist = m_twist + jacobian.AngleUpdate(m_DoF_id+1);
if (m_limit_twist) {
if (m_new_twist > m_max_twist) {
delta[1] = m_max_twist - m_twist;
m_new_twist = m_max_twist;
clamp[1] = true;
}
else if (m_new_twist < m_min_twist) {
delta[1] = m_min_twist - m_twist;
m_new_twist = m_min_twist;
clamp[1] = true;
}
}
}
return (clamp[0] || clamp[1]);
}
bool IK_QTranslateSegment::UpdateAngle(const IK_QJacobian &jacobian, MT_Vector3& delta, bool *clamp)
{
int dof_id = m_DoF_id, dof = 0, i, clamped = false;
MT_Vector3 dx(0.0, 0.0, 0.0);
for (i = 0; i < 3; i++) {
if (!m_axis_enabled[i]) {
m_new_translation[i] = m_translation[i];
continue;
}
clamp[dof] = false;
if (!m_locked[dof]) {
m_new_translation[i] = m_translation[i] + jacobian.AngleUpdate(dof_id);
if (m_limit[i]) {
if (m_new_translation[i] > m_max[i]) {
delta[dof] = m_max[i] - m_translation[i];
m_new_translation[i] = m_max[i];
clamped = clamp[dof] = true;
}
else if (m_new_translation[i] < m_min[i]) {
delta[dof] = m_min[i] - m_translation[i];
m_new_translation[i] = m_min[i];
clamped = clamp[dof] = true;
}
}
}
dof_id++;
dof++;
}
return clamped;
}
bool IK_QRevoluteSegment::UpdateAngle(const IK_QJacobian &jacobian, MT_Vector3& delta, bool *clamp)
{
if (m_locked[0])
return false;
m_new_angle = m_angle + jacobian.AngleUpdate(m_DoF_id);
clamp[0] = false;
if (m_limit == false)
return false;
if (m_new_angle > m_max)
delta[0] = m_max - m_angle;
else if (m_new_angle < m_min)
delta[0] = m_min - m_angle;
else
return false;
clamp[0] = true;
m_new_angle = m_angle + delta[0];
return true;
}
void IK_QSwingSegment::Lock(int, IK_QJacobian& jacobian, MT_Vector3& delta)
{
m_locked[0] = m_locked[1] = true;
jacobian.Lock(m_DoF_id, delta[0]);
jacobian.Lock(m_DoF_id+1, delta[1]);
}
bool IK_QSwingSegment::UpdateAngle(const IK_QJacobian &jacobian, MT_Vector3& delta, bool *clamp)
{
if (m_locked[0] && m_locked[1])
return false;
MT_Vector3 dq;
dq.x() = jacobian.AngleUpdate(m_DoF_id);
dq.y() = 0.0;
dq.z() = jacobian.AngleUpdate(m_DoF_id+1);
// Directly update the rotation matrix, with Rodrigues' rotation formula,
// to avoid singularities and allow smooth integration.
MT_Scalar theta = dq.length();
if (!MT_fuzzyZero(theta)) {
MT_Vector3 w = dq*(1.0/theta);
MT_Scalar sine = sin(theta);
MT_Scalar cosine = cos(theta);
MT_Scalar cosineInv = 1-cosine;
MT_Scalar xsine = w.x()*sine;
MT_Scalar zsine = w.z()*sine;
MT_Scalar xxcosine = w.x()*w.x()*cosineInv;
MT_Scalar xzcosine = w.x()*w.z()*cosineInv;
MT_Scalar zzcosine = w.z()*w.z()*cosineInv;
MT_Matrix3x3 M(
cosine + xxcosine, -zsine, xzcosine,
zsine, cosine, -xsine,
xzcosine, xsine, cosine + zzcosine);
m_new_basis = m_basis*M;
RemoveTwist(m_new_basis);
}
else
m_new_basis = m_basis;
if (m_limit_x == false && m_limit_z == false)
return false;
MT_Vector3 a = SphericalRangeParameters(m_new_basis);
MT_Scalar ax = 0, az = 0;
clamp[0] = clamp[1] = false;
if (m_limit_x && m_limit_z) {
ax = a.x();
az = a.z();
if (EllipseClamp(ax, az, m_min, m_max))
clamp[0] = clamp[1] = true;
}
else if (m_limit_x) {
if (ax < m_min[0]) {
ax = m_min[0];
clamp[0] = true;
}
else if (ax > m_max[0]) {
ax = m_max[0];
clamp[0] = true;
}
}
else if (m_limit_z) {
if (az < m_min[1]) {
az = m_min[1];
clamp[1] = true;
}
else if (az > m_max[1]) {
az = m_max[1];
clamp[1] = true;
}
}
if (clamp[0] == false && clamp[1] == false)
return false;
m_new_basis = ComputeSwingMatrix(ax, az);
delta = MatrixToAxisAngle(m_basis.transposed()*m_new_basis);
delta[1] = delta[2]; delta[2] = 0.0;
return true;
}
void IK_QRevoluteSegment::Lock(int, IK_QJacobian& jacobian, MT_Vector3& delta)
{
m_locked[0] = true;
jacobian.Lock(m_DoF_id, delta[0]);
}
void IK_QTranslateSegment::Lock(int dof, IK_QJacobian& jacobian, MT_Vector3& delta)
{
m_locked[dof] = true;
jacobian.Lock(m_DoF_id+dof, delta[dof]);
}
