Skip to content

Commit

Permalink
go ove setting up quad for RL ai agents training (wip)
Browse files Browse the repository at this point in the history
  • Loading branch information
@JulioJerez
JulioJerez committed Dec 4, 2023
1 parent c618bd2 commit 4d58e32
Show file tree
Hide file tree
Showing 6 changed files with 131 additions and 50 deletions.
48 changes: 38 additions & 10 deletions newton-4.00/applications/ndSandbox/demos/ndQuadrupedTest_1.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -333,7 +333,6 @@ namespace ndQuadruped_1
}
}

//#pragma optimize( "", off ) //for debugging purpose
ndBrainFloat GetReward() const
{
if (IsTerminal())
Expand Down Expand Up @@ -1651,7 +1650,7 @@ namespace ndQuadruped_1

void Debug(ndConstraintDebugCallback& context) const
{
//ndBodyKinematic* const rootBody = GetRoot()->m_body->GetAsBodyKinematic();
ndBodyKinematic* const rootBody = GetRoot()->m_body->GetAsBodyKinematic();
//ndSkeletonContainer* const skeleton = rootBody->GetSkeleton();
//if (!skeleton)
//{
Expand Down Expand Up @@ -1756,14 +1755,33 @@ namespace ndQuadruped_1
//context.DrawPoint(zmpLineOfAction, ndVector(1.0f, 0.0f, 0.0f, 1.0f), 6);
//const ndPoseGenerator* const poseGenerator = (ndPoseGenerator*)*m_poseGenerator->GetSequence();

const ndVector upVector(rootBody->GetMatrix().m_up);
ndFixSizeArray<ndVector, 4> desiredSupportPoint;
for (ndInt32 i = 0; i < m_animPose.GetCount(); ++i)
{
const ndAnimKeyframe& keyFrame = m_animPose[i];
ndEffectorInfo* const info = (ndEffectorInfo*)keyFrame.m_userData;
ndIkSwivelPositionEffector* const effector = (ndIkSwivelPositionEffector*)*info->m_effector;

if (i == 0)
{
//ndMatrix m0;
//ndMatrix m1;
//effector->CalculateGlobalMatrix(m0, m1);
//ndMatrix baseSwivelFrame(effector->CalculateAlignSwivelMatrix());
//baseSwivelFrame.m_posit = (m0.m_posit + m1.m_posit)* ndVector::m_half;
//
//ndFloat32 swivelAngle = effector->CalculateAlignSwivelAngle(upVector);
//ndMatrix upVectorSwivelFrame(ndPitchMatrix(swivelAngle) * baseSwivelFrame);
//
//context.DrawFrame(baseSwivelFrame);
//context.DrawFrame(upVectorSwivelFrame);

effector->DebugJoint(context);
}

if (keyFrame.m_userParamInt)
{
ndEffectorInfo* const info = (ndEffectorInfo*)keyFrame.m_userData;
ndIkSwivelPositionEffector* const effector = (ndIkSwivelPositionEffector*)*info->m_effector;
ndBodyKinematic* const body = effector->GetBody0();
desiredSupportPoint.PushBack(body->GetMatrix().TransformVector(effector->GetLocalMatrix0().m_posit));
}
Expand Down Expand Up @@ -1822,8 +1840,10 @@ namespace ndQuadruped_1
ndFixSizeArray<ndJointBilateralConstraint*, 32> effectors;

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

const ndVector upVector(0.0f, 1.0f, 0.0f, 0.0f);
for (ndInt32 i = 0; i < m_animPose.GetCount(); ++i)
{
ndEffectorInfo* const info = &m_effectorsInfo[i];
Expand All @@ -1833,8 +1853,11 @@ 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]);
effector->SetSwivelAngle(0.0f);
ndFloat32 swivelAngle = effector->CalculateAlignSwivelAngle(upVector);
effector->SetSwivelAngle(swivelAngle);

// calculate lookAt angle
ndMatrix lookAtMatrix0;
Expand All @@ -1846,6 +1869,7 @@ namespace ndQuadruped_1
upMatrix.m_right = (upMatrix.m_front.CrossProduct(upVector) & ndVector::m_triplexMask).Normalize();
upMatrix.m_up = upMatrix.m_right.CrossProduct(upMatrix.m_front);
upMatrix = upMatrix * lookAtMatrix0.OrthoInverse();

const ndFloat32 angle = ndAtan2(upMatrix.m_up.m_z, upMatrix.m_up.m_y);
info->m_footHinge->SetTargetAngle(angle);
}
Expand Down Expand Up @@ -1888,9 +1912,12 @@ namespace ndQuadruped_1

ndMatrix location(matrixLocation);
location.m_posit.m_y = floor.m_y + 0.5f;
//location.m_posit.m_y += 0.5f;
//location.m_posit.m_y += 1.5f;
torso->SetMatrix(location);

ndAssert(0);
//remembr normalize inertia

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

Expand All @@ -1912,7 +1939,8 @@ 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 < 4; ++i)
for (ndInt32 i = 0; i < 1; ++i)
{
ndMatrix limbPivotLocation(matrix * ndYawMatrix(angles[i] * ndDegreeToRad));
limbPivotLocation.m_posit += torso->GetMatrix().m_posit;
Expand Down Expand Up @@ -1943,7 +1971,7 @@ namespace ndQuadruped_1

ndMatrix bodyMatrix(limbPivotLocation);
bodyMatrix.m_posit += limbPivotLocation.m_front.Scale(limbLength * 0.5f);
ndSharedPtr<ndBody> calf0(world->GetBody(AddCapsule(scene, bodyMatrix, limbMass, limbRadios, limbRadios, limbLength)));
ndSharedPtr<ndBody> calf0(world->GetBody(AddCapsule(scene, bodyMatrix, limbMass * 0.25f, limbRadios, limbRadios, limbLength)));
calf0->SetMatrix(bodyMatrix);

ndMatrix caffPinAndPivotFrame(ndGetIdentityMatrix());
Expand Down Expand Up @@ -1973,7 +2001,7 @@ namespace ndQuadruped_1
ndMatrix bodyMatrix(limbPivotLocation);
bodyMatrix.m_posit += limbPivotLocation.m_front.Scale(lenght * 0.5f);

ndSharedPtr<ndBody> foot(world->GetBody(AddCapsule(scene, bodyMatrix, limbMass * 0.5f, limbRadios, limbRadios, lenght)));
ndSharedPtr<ndBody> foot(world->GetBody(AddCapsule(scene, bodyMatrix, limbMass * 0.25f, limbRadios, limbRadios, lenght)));
foot->SetMatrix(bodyMatrix);

// set a Material with zero restitution for the feet
Expand Down
2 changes: 1 addition & 1 deletion newton-4.00/applications/ndSandbox/toolbox/ndCifar10ImageClassification.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -687,6 +687,6 @@ void ndCifar10ImageClassification()
{
ndSetRandSeed(12345);

Cifar10TrainingSet();
//Cifar10TrainingSet();
//Cifar10TestSet();
}
1 change: 0 additions & 1 deletion newton-4.00/sdk/dCollision/ndJointBilateralConstraint.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -251,7 +251,6 @@ void ndJointBilateralConstraint::JointAccelerations(ndJointAccelerationDecriptor
}
}

//#pragma optimize( "", off ) //for debugging purpose
void ndJointBilateralConstraint::AddLinearRowJacobian(ndConstraintDescritor& desc, const ndVector& pivot0, const ndVector& pivot1, const ndVector& dir)
{
ndPointParam param;
Expand Down
19 changes: 0 additions & 19 deletions newton-4.00/sdk/dCore/ndVectorSimd.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -57,10 +57,6 @@ class ndVector

inline ndVector (const ndFloat32 a)
:m_type(_mm_set1_ps( a))
//:m_x(a)
//,m_y(a)
//,m_z(a)
//,m_w(a)
{
}

Expand Down Expand Up @@ -88,10 +84,6 @@ class ndVector
}

inline ndVector(ndFloat64 x, ndFloat64 y, ndFloat64 z, ndFloat64 w)
//:m_x(ndFloat32(x))
//,m_y(ndFloat32(y))
//,m_z(ndFloat32(z))
//,m_w(ndFloat32(w))
:m_type(_mm_set_ps(ndFloat32(w), ndFloat32(z), ndFloat32(y), ndFloat32(x)))
{
}
Expand All @@ -109,20 +101,12 @@ class ndVector
}

inline ndVector (ndFloat32 x, ndFloat32 y, ndFloat32 z, ndFloat32 w)
//:m_x(x)
//,m_y(y)
//,m_z(z)
//,m_w(w)
:m_type(_mm_set_ps(w, z, y, x))
{
}

inline ndVector (ndInt32 x, ndInt32 y, ndInt32 z, ndInt32 w)
:m_typeInt(_mm_set_epi32(w, z, y, x))
//:m_ix(x)
//,m_iy(y)
//,m_iz(z)
//,m_iw(w)
{
}

Expand Down Expand Up @@ -639,19 +623,16 @@ class ndBigVector
#endif

inline ndBigVector(ndFloat64 x, ndFloat64 y, ndFloat64 z, ndFloat64 w)
//:m_x(x), m_y(y), m_z(z), m_w(w)
:m_type(_mm256_set_pd(w, z, y, x))
{
}

inline ndBigVector(ndInt32 x, ndInt32 y, ndInt32 z, ndInt32 w)
//:m_ix(ndInt64(x)), m_iy(ndInt64(y)), m_iz(ndInt64(z)), m_iw(ndInt64(w))
:m_typeInt(_mm256_set_epi64x(ndInt64(w), ndInt64(z), ndInt64(y), ndInt64(x)))
{
}

inline ndBigVector(ndInt64 x, ndInt64 y, ndInt64 z, ndInt64 w)
//:m_ix(x), m_iy(y), m_iz(z), m_iw(w)
:m_typeInt(_mm256_set_epi64x(w, z, y, x))
{
}
Expand Down
103 changes: 86 additions & 17 deletions newton-4.00/sdk/dNewton/dIkSolver/ndIkSwivelPositionEffector.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -33,8 +33,8 @@ ndIkSwivelPositionEffector::ndIkSwivelPositionEffector()
m_maxDof = 6;
}

ndIkSwivelPositionEffector::ndIkSwivelPositionEffector(const ndVector& childPivot, const ndMatrix& pinAndPivotParent, ndBodyKinematic* const child, ndBodyKinematic* const parent)
:ndJointBilateralConstraint(6, child, parent, pinAndPivotParent)
ndIkSwivelPositionEffector::ndIkSwivelPositionEffector(const ndVector& childPivotInGlbalSpace, const ndMatrix& pinAndPivotParentInGlobalSpace, ndBodyKinematic* const child, ndBodyKinematic* const parent)
:ndJointBilateralConstraint(6, child, parent, pinAndPivotParentInGlobalSpace)
,m_localTargetPosit(ndVector::m_wOne)
,m_swivelAngle(ndFloat32(0.0f))
,m_angularSpring(ndFloat32(1000.0f))
Expand All @@ -50,15 +50,22 @@ ndIkSwivelPositionEffector::ndIkSwivelPositionEffector(const ndVector& childPivo
,m_rotationOrder(m_pitchYawRoll)
,m_enableSwivelControl(true)
{
ndMatrix temp;
ndMatrix matrix1 (pinAndPivotParent);

ndVector offset(pinAndPivotParent.UntransformVector(childPivot) & ndVector::m_triplexMask);
ndVector offset(pinAndPivotParentInGlobalSpace.UntransformVector(childPivotInGlbalSpace) & ndVector::m_triplexMask);
m_maxWorkSpaceRadio = ndFloat32(0.99f) * ndSqrt(offset.DotProduct(offset).GetScalar());
m_localTargetPosit.m_x = m_maxWorkSpaceRadio;

matrix1.m_posit = childPivot;
CalculateLocalMatrix(matrix1, m_localMatrix0, temp);
ndMatrix temp;
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]);

SetSolverModel(m_jointkinematicCloseLoop);
}
Expand Down Expand Up @@ -131,7 +138,7 @@ void ndIkSwivelPositionEffector::SetWorkSpaceConstraints(ndFloat32 minRadio, ndF
{
m_minWorkSpaceRadio = minRadio;
m_maxWorkSpaceRadio = maxRadio;
// make sure the taget is with in the workspace constraint
// make sure the target is with in the workspace constraint
SetLocalTargetPosition(GetLocalTargetPosition());
}

Expand Down Expand Up @@ -210,32 +217,94 @@ ndMatrix ndIkSwivelPositionEffector::CalculateSwivelFrame(const ndMatrix& matrix
return swivelMatrix;
}

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

ndFloat32 ndIkSwivelPositionEffector::CalculateAlignSwivelAngle(const ndVector& upDir) const
{
{
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));
}


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

ndFloat32 swivelAngle = GetSwivelAngle();
ndFloat32 colinearTest = upDir.DotProduct(baseSwivelMatrix1.m_front).GetScalar();
if (ndAbs(colinearTest) < ndFloat32 (0.995f))
{
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;
}
return swivelAngle;
}

void ndIkSwivelPositionEffector::DebugJoint(ndConstraintDebugCallback& debugCallback) const
{
ndMatrix matrix0;
ndMatrix matrix1;
CalculateGlobalMatrix(matrix0, matrix1);
debugCallback.DrawFrame(matrix1, ndFloat32 (0.5f));
debugCallback.DrawLine(matrix0.m_posit, matrix1.m_posit, ndVector(ndFloat32(0.5f), ndFloat32(0.5f), ndFloat32(0.0f), ndFloat32(1.0f)));
//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);

ndMatrix swivelMatrix0(CalculateSwivelFrame(matrix1));
ndMatrix swivelMatrix1(ndPitchMatrix(m_swivelAngle) * swivelMatrix0);
const ndMatrix swivelMatrix0(ndPitchMatrix(m_swivelAngle) * swivelMatrix1);
debugCallback.DrawFrame(swivelMatrix0);
debugCallback.DrawFrame(swivelMatrix1);
debugCallback.DrawPoint(swivelMatrix0.m_posit, ndVector(1.0f, 1.0f, 0.0f, 0.0f), 8.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 angle = CalculateAngle(matrix0[1], swivelMatrix1[1], swivelMatrix1[0]);
//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);
}

//#pragma optimize( "", off ) //for debugging purpose
void ndIkSwivelPositionEffector::SubmitLinearAxis(ndConstraintDescritor& desc, const ndMatrix& matrix0, const ndMatrix& matrix1)
{
//static ndVector xxx0;
Expand Down
8 changes: 6 additions & 2 deletions newton-4.00/sdk/dNewton/dIkSolver/ndIkSwivelPositionEffector.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -27,7 +27,7 @@ class ndIkSwivelPositionEffector: public ndJointBilateralConstraint
D_CLASS_REFLECTION(ndIkSwivelPositionEffector, ndJointBilateralConstraint)

D_NEWTON_API ndIkSwivelPositionEffector();
D_NEWTON_API ndIkSwivelPositionEffector(const ndVector& childPivot, const ndMatrix& pinAndPivotParent, ndBodyKinematic* const child, ndBodyKinematic* const parent);
D_NEWTON_API ndIkSwivelPositionEffector(const ndVector& childPivotInGlobalSpace, const ndMatrix& pinAndPivotParentInGlobalSpace, ndBodyKinematic* const child, ndBodyKinematic* const parent);
D_NEWTON_API virtual ~ndIkSwivelPositionEffector();

D_NEWTON_API ndVector GetLocalTargetPosition() const;
Expand All @@ -38,6 +38,8 @@ class ndIkSwivelPositionEffector: public ndJointBilateralConstraint

D_NEWTON_API ndFloat32 GetSwivelAngle() const;
D_NEWTON_API void SetSwivelAngle(const ndFloat32 angle);
D_NEWTON_API ndMatrix CalculateAlignSwivelMatrix() const;
D_NEWTON_API ndFloat32 CalculateAlignSwivelAngle(const ndVector& upDir) const;

D_NEWTON_API void SetLinearSpringDamper(ndFloat32 regularizer, ndFloat32 springConst, ndFloat32 damperConst);
D_NEWTON_API void GetLinearSpringDamper(ndFloat32& regularizer, ndFloat32& springConst, ndFloat32& damperConst) const;
Expand Down Expand Up @@ -65,11 +67,13 @@ class ndIkSwivelPositionEffector: public ndJointBilateralConstraint
D_NEWTON_API void GetDynamicState(ndVector& posit, ndVector& veloc) const;

protected:
D_NEWTON_API void JacobianDerivative(ndConstraintDescritor& desc);
ndMatrix CalculateSwivelFrame(const ndMatrix& matrix1) const;
D_NEWTON_API void JacobianDerivative(ndConstraintDescritor& desc);

void SubmitLinearAxis(ndConstraintDescritor& desc, const ndMatrix& matrix0, const ndMatrix& matrix1);
void SubmitAngularAxis(ndConstraintDescritor& desc, const ndMatrix& matrix0, const ndMatrix& matrix1);

ndVector m_localSwivelPin;
ndVector m_localTargetPosit;
ndFloat32 m_swivelAngle;

Expand Down

0 comments on commit 4d58e32

Please sign in to comment.