From 72729468c5e18ad23c1d5c672015fc8d887f004f Mon Sep 17 00:00:00 2001 From: JulioJerez Date: Wed, 13 Dec 2023 19:46:36 -0800 Subject: [PATCH] more on zmp calculation --- .../ndSandbox/demos/ndQuadrupedTest_1.cpp | 27 ++++++++++++++----- 1 file changed, 20 insertions(+), 7 deletions(-) diff --git a/newton-4.00/applications/ndSandbox/demos/ndQuadrupedTest_1.cpp b/newton-4.00/applications/ndSandbox/demos/ndQuadrupedTest_1.cpp index 108a671a84..f1bb605241 100644 --- a/newton-4.00/applications/ndSandbox/demos/ndQuadrupedTest_1.cpp +++ b/newton-4.00/applications/ndSandbox/demos/ndQuadrupedTest_1.cpp @@ -1649,15 +1649,27 @@ namespace ndQuadruped_1 } - ndVector CalculateZeroMomnetPoint(const ndVector& com, const ndVector& centerOfPresure) const + ndVector CalculateZeroMomentPoint(const ndVector& centerOfPresure) const { + ndFixSizeArray r; ndFixSizeArray bodies; + ndVector com(ndVector::m_zero); + ndFloat32 totalMass = ndFloat32(0.0f); for (ndModelArticulation::ndNode* node = GetRoot()->GetFirstIterator(); node; node = node->GetNextIterator()) { const ndBodyKinematic* const body = node->m_body->GetAsBodyKinematic(); + + const ndMatrix matrix(body->GetMatrix()); + const ndVector bodyCom(matrix.TransformVector(body->GetCentreOfMass())); + ndFloat32 mass = body->GetMassMatrix().m_w; + totalMass += mass; + com += matrix.TransformVector(body->GetCentreOfMass()).Scale(mass); + + r.PushBack(bodyCom); bodies.PushBack(body); } + com = com.Scale(ndFloat32 (1.0f) / totalMass); ndVector force(ndVector::m_zero); ndVector torque(ndVector::m_zero); @@ -1666,18 +1678,19 @@ namespace ndQuadruped_1 { const ndBodyKinematic* const body = bodies[i]; - const ndMatrix matrix(body->GetMatrix()); - const ndVector centerOfMass (matrix.TransformVector(body->GetCentreOfMass())); - const ndVector bodyForce((body->GetAccel() - gravity).Scale (body->GetMassMatrix().m_w)); + //const ndMatrix matrix(body->GetMatrix()); + //const ndVector centerOfMass (matrix.TransformVector(body->GetCentreOfMass())); + + //const ndVector& centerOfMass = r[i]; + const ndVector& centerOfMass = r[i] - com; const ndMatrix bodyInertia(body->CalculateInertiaMatrix()); + const ndVector bodyForce((body->GetAccel() - gravity).Scale (body->GetMassMatrix().m_w)); force += bodyForce; torque += centerOfMass.CrossProduct(bodyForce); torque += bodyInertia.RotateVector(body->GetAlpha()); - } - return centerOfPresure; } @@ -1835,7 +1848,7 @@ namespace ndQuadruped_1 context.DrawPoint(centerOfPresure, ndVector(0.0f, 1.0f, 0.0f, 1.0f), 5); //context.DrawPoint(ndVector(p1Out), ndVector(0.0f, 1.0f, 0.0f, 1.0f), 3); - ndVector zmp(CalculateZeroMomnetPoint(centerOfMass, centerOfPresure)); + ndVector zmp(CalculateZeroMomentPoint(centerOfPresure)); context.DrawPoint(zmp, ndVector(1.0f, 0.0f, 0.0f, 1.0f), 5); } else if (desiredSupportPoint.GetCount() == 2)