Skip to content

Commit

Permalink
finally foudn the bug with the IK effector joint and the swivel angle…
Browse files Browse the repository at this point in the history 
… calculation.

this bug was hunting me for almost a year, nd it turn out was a sign error.
anyway at lest was no a fundament error in the joint definition.

ok resume the RL training
  • Loading branch information
Julio Jerez committed Dec 10, 2023
1 parent 3c12323 commit c9d4907
Show file tree
Hide file tree
Showing 4 changed files with 79 additions and 164 deletions.
15 changes: 6 additions & 9 deletions newton-4.00/applications/ndSandbox/demos/ndQuadrupedTest_1.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1841,9 +1841,10 @@ namespace ndQuadruped_1

ndVector veloc;
m_animBlendTree->Evaluate(m_animPose, veloc);
//const ndVector upVector(rootBody->GetMatrix().m_up);

const ndVector upVector(0.0f, 1.0f, 0.0f, 0.0f);
//const ndVector upVector(0.0f, 1.0f, 0.0f, 0.0f);
const ndVector upVector(rootBody->GetMatrix().m_up);

for (ndInt32 i = 0; i < m_animPose.GetCount(); ++i)
{
ndEffectorInfo* const info = &m_effectorsInfo[i];
Expand All @@ -1854,10 +1855,10 @@ namespace ndQuadruped_1
ndVector posit(m_animPose[i].m_posit);
effector->SetLocalTargetPosition(posit);

//effector->SetSwivelAngle(0.0f);
//effector->SetSwivelAngle(actions[actionIndex + m_leg0_action_posit_swivel]);
ndFloat32 swivelAngle = effector->CalculateAlignSwivelAngle(upVector);
ndFloat32 swivelAngle = effector->CalculateSwivelAngle(upVector);
effector->SetSwivelAngle(swivelAngle);
//effector->SetSwivelAngle(0.0f);

// calculate lookAt angle
ndMatrix lookAtMatrix0;
Expand Down Expand Up @@ -1915,9 +1916,6 @@ namespace ndQuadruped_1
//location.m_posit.m_y += 1.5f;
torso->SetMatrix(location);

ndAssert(0);
//remember normalize inertia

ndDemoEntity* const entity = (ndDemoEntity*)torso->GetNotifyCallback()->GetUserData();
entity->SetMeshMatrix(ndYawMatrix(90.0f * ndDegreeToRad) * ndPitchMatrix(90.0f * ndDegreeToRad));

Expand All @@ -1939,8 +1937,7 @@ namespace ndQuadruped_1

ndRobot::ndPoseGenerator* const poseGenerator = (ndRobot::ndPoseGenerator*)*sequence;
//const ndVector upDir(location.m_up);
//for (ndInt32 i = 0; i < 4; ++i)
for (ndInt32 i = 0; i < 1; ++i)
for (ndInt32 i = 0; i < 4; ++i)
{
ndMatrix limbPivotLocation(matrix * ndYawMatrix(angles[i] * ndDegreeToRad));
limbPivotLocation.m_posit += torso->GetMatrix().m_posit;
Expand Down
5 changes: 1 addition & 4 deletions newton-4.00/sdk/dFileFormat/ndFileFormatJointIkSwivelPositionEffector.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -63,7 +63,6 @@ void ndFileFormatJointIkSwivelPositionEffector::SaveJoint(ndFileFormatSave* cons
exportJoint->GetWorkSpaceConstraints(minRadio, maxRadio);
xmlSaveParam(classNode, "minWorkSpaceRadio", minRadio);
xmlSaveParam(classNode, "maxWorkSpaceRadio", maxRadio);
xmlSaveParam(classNode, "rotationOrder", ndInt32 (exportJoint->GetRotationOrder()));
xmlSaveParam(classNode, "enableSwivelControl", exportJoint->GetSwivelMode() ? 1 : 0);
}

Expand Down Expand Up @@ -91,15 +90,13 @@ void ndFileFormatJointIkSwivelPositionEffector::LoadJoint(const nd::TiXmlElement

ndFloat32 minWorkSpaceRadio = xmlGetFloat(node, "minWorkSpaceRadio");
ndFloat32 maxWorkSpaceRadio = xmlGetFloat(node, "maxWorkSpaceRadio");

ndInt32 rotationOrder = xmlGetInt(node, "rotationOrder");
ndInt32 enableSwivelControlr = xmlGetInt(node, "enableSwivelControl");

inportJoint->SetMaxForce(maxForce);
inportJoint->SetMaxTorque(maxTorque);
inportJoint->SetSwivelMode(enableSwivelControlr ? true : false);
inportJoint->SetWorkSpaceConstraints(minWorkSpaceRadio, maxWorkSpaceRadio);
inportJoint->SetRotationOrder(ndIkSwivelPositionEffector::ndRotationOrder(rotationOrder));

inportJoint->GetLinearSpringDamper(linearSpringRegularizer, linearSpringConstant, linearDamperConstant);
inportJoint->GetLinearSpringDamper(angularSpringRegularizer, angularSpringConstant, angularDamperConstant);
}
207 changes: 69 additions & 138 deletions newton-4.00/sdk/dNewton/dIkSolver/ndIkSwivelPositionEffector.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -27,7 +27,6 @@ ndIkSwivelPositionEffector::ndIkSwivelPositionEffector()
,m_linearRegularizer(ndFloat32(5.0e-3f))
,m_minWorkSpaceRadio(ndFloat32(0.0f))
,m_maxWorkSpaceRadio(ndFloat32(0.0f))
,m_rotationOrder(m_pitchYawRoll)
,m_enableSwivelControl(true)
{
m_maxDof = 6;
Expand All @@ -47,7 +46,6 @@ ndIkSwivelPositionEffector::ndIkSwivelPositionEffector(const ndVector& childPivo
,m_linearRegularizer(ndFloat32(5.0e-3f))
,m_minWorkSpaceRadio(ndFloat32(0.0f))
,m_maxWorkSpaceRadio(ndFloat32(0.0f))
,m_rotationOrder(m_pitchYawRoll)
,m_enableSwivelControl(true)
{
ndVector offset(pinAndPivotParentInGlobalSpace.UntransformVector(childPivotInGlbalSpace) & ndVector::m_triplexMask);
Expand All @@ -58,14 +56,9 @@ ndIkSwivelPositionEffector::ndIkSwivelPositionEffector(const ndVector& childPivo
ndMatrix matrix(pinAndPivotParentInGlobalSpace);
matrix.m_posit = childPivotInGlbalSpace;
CalculateLocalMatrix(matrix, m_localMatrix0, temp);
m_localSwivelPin = matrix.UnrotateVector(ndVector(ndFloat32 (0.0f), ndFloat32(1.0f), ndFloat32(0.0f), ndFloat32(0.0f)));

//ndMatrix matrix0;
//ndMatrix matrix1;
//CalculateGlobalMatrix(matrix0, matrix1);
//matrix1 = CalculateSwivelFrame(matrix1);
//ndVector swivelPin(matrix0.RotateVector(m_localSwivelPin));
//const ndFloat32 angle = CalculateAngle(swivelPin, matrix1[1], matrix1[0]);
ndAssert((temp[0].DotProduct(m_localMatrix0[0]).GetScalar() - ndFloat32(1.0f)) < ndFloat32(1.0e-4f));
ndAssert((temp[1].DotProduct(m_localMatrix0[1]).GetScalar() - ndFloat32(1.0f)) < ndFloat32(1.0e-4f));
ndAssert((temp[2].DotProduct(m_localMatrix0[2]).GetScalar() - ndFloat32(1.0f)) < ndFloat32(1.0e-4f));

SetSolverModel(m_jointkinematicCloseLoop);
}
Expand All @@ -84,22 +77,12 @@ void ndIkSwivelPositionEffector::SetSwivelMode(bool active)
m_enableSwivelControl = active;
}

ndIkSwivelPositionEffector::ndRotationOrder ndIkSwivelPositionEffector::GetRotationOrder() const
{
return m_rotationOrder;
}

void ndIkSwivelPositionEffector::SetRotationOrder(ndRotationOrder order)
{
m_rotationOrder = order;
}

ndFloat32 ndIkSwivelPositionEffector::GetSwivelAngle() const
{
return m_swivelAngle;
}

void ndIkSwivelPositionEffector::SetSwivelAngle(const ndFloat32 angle)
void ndIkSwivelPositionEffector::SetSwivelAngle(ndFloat32 angle)
{
m_swivelAngle = angle;
}
Expand Down Expand Up @@ -200,68 +183,77 @@ ndMatrix ndIkSwivelPositionEffector::CalculateSwivelFrame(const ndMatrix& matrix
{
ndMatrix swivelMatrix;
ndVector pin((m_localTargetPosit & ndVector::m_triplexMask).Normalize());
if (m_rotationOrder == m_pitchYawRoll)
{
ndFloat32 yaw = -ndAsin(pin.m_z);
ndFloat32 roll = ndAtan2(pin.m_y, pin.m_x);
swivelMatrix = ndYawMatrix(yaw) * ndRollMatrix(roll) * matrix1;
}
else
{
ndAssert(0);
//ndFloat32 yaw = -ndAsin(pin.m_z);
//ndFloat32 roll = ndAtan2(pin.m_y, pin.m_x);
//swivelMatrix = ndYawMatrix(yaw) * ndRollMatrix(roll) * matrix1;
}
ndFloat32 yaw = -ndAsin(pin.m_z);
ndFloat32 roll = ndAtan2(pin.m_y, pin.m_x);
swivelMatrix = ndYawMatrix(yaw) * ndRollMatrix(roll) * matrix1;
swivelMatrix.m_posit = matrix1.TransformVector(m_localTargetPosit);
return swivelMatrix;
}

ndMatrix ndIkSwivelPositionEffector::CalculateAlignSwivelMatrix() const
ndMatrix ndIkSwivelPositionEffector::CalculateSwivelMatrix() const
{
ndAssert(0);
return CalculateSwivelFrame(m_localMatrix1 * m_body1->GetMatrix());
}

ndFloat32 ndIkSwivelPositionEffector::CalculateAlignSwivelAngle(const ndVector& upDir) const
ndVector ndIkSwivelPositionEffector::GetEffectorPosit() const
{
ndMatrix matrix0;
ndMatrix matrix1;
CalculateGlobalMatrix(matrix0, matrix1);
ndVector posit(matrix1.UntransformVector(matrix0.m_posit));

ndAssert(0);
const ndMatrix swivelMatrix(ndPitchMatrix(-m_swivelAngle) * CalculateSwivelFrame(matrix1));
posit.m_w = CalculateAngle(matrix0[1], swivelMatrix[1], swivelMatrix[0]);
return posit;
}

void ndIkSwivelPositionEffector::GetDynamicState(ndVector& posit, ndVector& veloc) const
{
ndMatrix matrix0;
ndMatrix matrix1;
CalculateGlobalMatrix(matrix0, matrix1);
matrix1.m_posit = matrix1.TransformVector(m_localTargetPosit);

const ndMatrix& axisDir = matrix1;
const ndVector omega0(m_body0->GetOmega());
const ndVector omega1(m_body1->GetOmega());
const ndVector veloc0(m_body0->GetVelocity());
const ndVector veloc1(m_body1->GetVelocity());

ndPointParam param;
InitPointParam(param, matrix0.m_posit, matrix1.m_posit);
for (ndInt32 i = 0; i < 3; ++i)
{
ndMatrix matrix0;
ndMatrix matrix1;
CalculateGlobalMatrix(matrix0, matrix1);
const ndVector swivelPin(matrix0.RotateVector(m_localSwivelPin));
const ndMatrix swivelMatrix1(ndPitchMatrix(m_swivelAngle) * CalculateSwivelFrame(matrix1));
const ndFloat32 angle0 = CalculateAngle(matrix0[1], swivelMatrix1[1], swivelMatrix1[0]);
const ndFloat32 angle1 = CalculateAngle(swivelPin, swivelMatrix1[1], swivelMatrix1[0]);

ndTrace(("%f %f\n", angle0, angle1));
const ndVector& pin = axisDir[i];
const ndVector r0CrossDir(param.m_r0.CrossProduct(pin));
const ndVector r1CrossDir(param.m_r1.CrossProduct(pin));
posit[i] = (pin * param.m_posit0 - pin * param.m_posit1).AddHorizontal().GetScalar();
veloc[i] = (pin * veloc0 + r0CrossDir * omega0 - pin * veloc1 - r1CrossDir * omega1).AddHorizontal().GetScalar();
}

ndAssert(0);
const ndMatrix swivelMatrix(ndPitchMatrix(-m_swivelAngle) * CalculateSwivelFrame(matrix1));
posit.m_w = CalculateAngle(matrix0[1], swivelMatrix[1], swivelMatrix[0]);
veloc.m_w = (omega0 * swivelMatrix.m_front - omega1 * swivelMatrix.m_front).AddHorizontal().GetScalar();
}

ndMatrix baseSwivelMatrix0;
ndMatrix baseSwivelMatrix1;
ndAssert(ndAbs(upDir.DotProduct(upDir).GetScalar() - ndFloat32(1.0f)) < ndFloat32(1.0e-4f));
CalculateGlobalMatrix(baseSwivelMatrix0, baseSwivelMatrix1);
baseSwivelMatrix1 = CalculateSwivelFrame(baseSwivelMatrix1);

ndFloat32 ndIkSwivelPositionEffector::CalculateSwivelAngle(const ndVector& upDir) const
{
ndFloat32 swivelAngle = GetSwivelAngle();
ndFloat32 colinearTest = upDir.DotProduct(baseSwivelMatrix1.m_front).GetScalar();
if (ndAbs(colinearTest) < ndFloat32 (0.995f))
const ndMatrix swivelMatrix(CalculateSwivelFrame(m_localMatrix1 * m_body1->GetMatrix()));

ndVector side (swivelMatrix.m_front.CrossProduct(upDir));
if (ndAbs(side.DotProduct(side).GetScalar()) > ndFloat32 (0.01f))
{
auto CalculateSwivelAngle = [&upDir](const ndMatrix& matrix)
{
ndMatrix targetSwivelMatrix(matrix);
targetSwivelMatrix.m_right = targetSwivelMatrix.m_front.CrossProduct(upDir).Normalize();
ndAssert(targetSwivelMatrix.m_right.m_w == ndFloat32(0.0f));
targetSwivelMatrix.m_up = targetSwivelMatrix.m_right.CrossProduct(targetSwivelMatrix.m_front);
ndMatrix swivelAngleMatrix(targetSwivelMatrix * matrix.OrthoInverse());
ndFloat32 swivelAngle = ndAtan2(swivelAngleMatrix.m_up.m_z, swivelAngleMatrix.m_up.m_y);
return swivelAngle;
};

ndFloat32 swivelAngle0 = CalculateSwivelAngle(baseSwivelMatrix0);
ndFloat32 swivelAngle1 = CalculateSwivelAngle(baseSwivelMatrix1);
swivelAngle = swivelAngle0 - swivelAngle1;
ndMatrix targetswivelMatrix(swivelMatrix);
targetswivelMatrix.m_right = side.Normalize();
targetswivelMatrix.m_up = targetswivelMatrix.m_right.CrossProduct(targetswivelMatrix.m_front);
const ndMatrix swivelPitch(swivelMatrix * targetswivelMatrix.OrthoInverse());
swivelAngle = ndAtan2(swivelPitch.m_up.m_z, swivelPitch.m_up.m_y);
}

return swivelAngle;
}

Expand All @@ -270,52 +262,22 @@ void ndIkSwivelPositionEffector::DebugJoint(ndConstraintDebugCallback& debugCall
ndMatrix matrix0;
ndMatrix matrix1;
CalculateGlobalMatrix(matrix0, matrix1);
//debugCallback.DrawFrame(matrix1, ndFloat32 (0.5f));
debugCallback.DrawLine(matrix0.m_posit, matrix1.m_posit, ndVector(ndFloat32(0.89f), ndFloat32(0.70f), ndFloat32(0.13f), ndFloat32(1.0f)));

//ndMatrix swivelMatrix0(CalculateSwivelFrame(matrix1));
//ndMatrix swivelMatrix1(ndPitchMatrix(m_swivelAngle) * swivelMatrix0);
//debugCallback.DrawPoint(swivelMatrix0.m_posit, ndVector(1.0f, 1.0f, 0.0f, 0.0f), 8.0f);
//
//ndVector point((matrix0.m_posit + matrix1.m_posit) * ndVector::m_half);
//swivelMatrix0.m_posit = point;
//swivelMatrix1.m_posit = point;
//
//debugCallback.DrawFrame(swivelMatrix0);
//debugCallback.DrawFrame(swivelMatrix1);

const ndVector origin((matrix0.m_posit + matrix1.m_posit) * ndVector::m_half);
ndMatrix swivelMatrix1(CalculateSwivelFrame(matrix1));
swivelMatrix1.m_posit = origin;
debugCallback.DrawFrame(swivelMatrix1);

const ndMatrix swivelMatrix0(ndPitchMatrix(m_swivelAngle) * swivelMatrix1);
const ndMatrix swivelMatrix0(ndPitchMatrix(-m_swivelAngle) * swivelMatrix1);
debugCallback.DrawFrame(swivelMatrix0);

const ndVector swivelPin0(matrix0.RotateVector(m_localSwivelPin));
//const ndMatrix swivelMatrix1(ndPitchMatrix(m_swivelAngle) * CalculateSwivelFrame(matrix1));
const ndVector& pin = swivelMatrix1.m_front;
//debugCallback.DrawLine(origin, origin + pin, ndVector (0.7f, 0.9f, 0.3f, 1.0f));
debugCallback.DrawLine(origin, origin + swivelPin0, ndVector(0.7f, 0.9f, 0.3f, 1.0f));
}

void ndIkSwivelPositionEffector::SubmitAngularAxis(ndConstraintDescritor& desc, const ndMatrix& matrix0, const ndMatrix& matrix1)
{
const ndVector swivelPin(matrix0.RotateVector(m_localSwivelPin));
const ndMatrix swivelMatrix1(ndPitchMatrix(m_swivelAngle) * CalculateSwivelFrame(matrix1));
const ndVector& pin = swivelMatrix1.m_front;
//const ndFloat32 angle0 = CalculateAngle(matrix0[1], swivelMatrix1[1], swivelMatrix1[0]);
const ndFloat32 angle = CalculateAngle(swivelPin, swivelMatrix1[1], swivelMatrix1[0]);

AddAngularRowJacobian(desc, pin, angle);
SetMassSpringDamperAcceleration(desc, m_angularRegularizer, m_angularSpring, m_angularDamper);
}

void ndIkSwivelPositionEffector::SubmitLinearAxis(ndConstraintDescritor& desc, const ndMatrix& matrix0, const ndMatrix& matrix1)
{
//static ndVector xxx0;
//static ndVector xxx1;
//GetDynamicState(xxx0, xxx1);
//static ndVector xxx0;
//static ndVector xxx1;
//GetDynamicState(xxx0, xxx1);

const ndVector posit0(matrix0.m_posit);
const ndVector posit1(matrix1.TransformVector(m_localTargetPosit));
Expand All @@ -330,45 +292,14 @@ void ndIkSwivelPositionEffector::SubmitLinearAxis(ndConstraintDescritor& desc, c
}
}

ndVector ndIkSwivelPositionEffector::GetEffectorPosit() const
{
ndMatrix matrix0;
ndMatrix matrix1;
CalculateGlobalMatrix(matrix0, matrix1);
ndVector posit(matrix1.UntransformVector(matrix0.m_posit));

const ndMatrix swivelMatrix(ndPitchMatrix(m_swivelAngle) * CalculateSwivelFrame(matrix1));
posit.m_w = CalculateAngle(matrix0[1], swivelMatrix[1], swivelMatrix[0]);
return posit;
}

void ndIkSwivelPositionEffector::GetDynamicState(ndVector& posit, ndVector& veloc) const
void ndIkSwivelPositionEffector::SubmitAngularAxis(ndConstraintDescritor& desc, const ndMatrix& matrix0, const ndMatrix& matrix1)
{
ndMatrix matrix0;
ndMatrix matrix1;
CalculateGlobalMatrix(matrix0, matrix1);
matrix1.m_posit = matrix1.TransformVector(m_localTargetPosit);

const ndMatrix& axisDir = matrix1;
const ndVector omega0(m_body0->GetOmega());
const ndVector omega1(m_body1->GetOmega());
const ndVector veloc0(m_body0->GetVelocity());
const ndVector veloc1(m_body1->GetVelocity());

ndPointParam param;
InitPointParam(param, matrix0.m_posit, matrix1.m_posit);
for (ndInt32 i = 0; i < 3; ++i)
{
const ndVector& pin = axisDir[i];
const ndVector r0CrossDir(param.m_r0.CrossProduct(pin));
const ndVector r1CrossDir(param.m_r1.CrossProduct(pin));
posit[i] = (pin * param.m_posit0 - pin * param.m_posit1).AddHorizontal().GetScalar();
veloc[i] = (pin * veloc0 + r0CrossDir * omega0 - pin * veloc1 - r1CrossDir * omega1).AddHorizontal().GetScalar();
}
const ndMatrix swivelMatrix1(ndPitchMatrix(-m_swivelAngle) * CalculateSwivelFrame(matrix1));
const ndVector& pin = swivelMatrix1.m_front;
const ndFloat32 angle = CalculateAngle(matrix0[1], swivelMatrix1[1], swivelMatrix1[0]);

const ndMatrix swivelMatrix(ndPitchMatrix(m_swivelAngle) * CalculateSwivelFrame(matrix1));
posit.m_w = CalculateAngle(matrix0[1], swivelMatrix[1], swivelMatrix[0]);
veloc.m_w = (omega0 * swivelMatrix.m_front - omega1 * swivelMatrix.m_front).AddHorizontal().GetScalar();
AddAngularRowJacobian(desc, pin, angle);
SetMassSpringDamperAcceleration(desc, m_angularRegularizer, m_angularSpring, m_angularDamper);
}

void ndIkSwivelPositionEffector::JacobianDerivative(ndConstraintDescritor& desc)
Expand Down
Loading

0 comments on commit c9d4907

Please sign in to comment.