diff --git a/dart/dynamics/Skeleton.cpp b/dart/dynamics/Skeleton.cpp index e08d0e2f4..40de43353 100644 --- a/dart/dynamics/Skeleton.cpp +++ b/dart/dynamics/Skeleton.cpp @@ -2117,12 +2117,13 @@ Eigen::MatrixXs Skeleton::getUnconstrainedVelJacobianWrt( Eigen::MatrixXs Minv = getInvMassMatrix(); Eigen::MatrixXs dC = getJacobianOfC(wrt); + Eigen::VectorXs spring_stiffs = getSpringStiffVector(); if (wrt == neural::WithRespectTo::POSITION) { Eigen::MatrixXs dM = getJacobianOfMinv( dt * (tau - C - getDampingForce() - getSpringForce()), wrt); - return dM - Minv * dt * dC; + return dM - Minv * dt * dC - Minv * dt * spring_stiffs.asDiagonal(); } else { @@ -3028,6 +3029,11 @@ std::size_t Skeleton::getLinkCOMDims() return 3 * getNumBodyNodes(); } +std::size_t Skeleton::getLinkDiagIDims() +{ + return 3 * getNumBodyNodes(); +} + //============================================================================== std::size_t Skeleton::getLinkMOIDims() { @@ -3109,6 +3115,32 @@ Eigen::Vector3s Skeleton::getLinkCOMIndex(size_t index) return mass_center; } +Eigen::VectorXs Skeleton::getLinkDiagIs() +{ + Eigen::VectorXs diag_Is = Eigen::VectorXs::Zero(getLinkDiagIDims()); + std::size_t cursor = 0; + for(std::size_t i = 0; i < getNumBodyNodes(); i++) + { + const Inertia& inertia= getBodyNode(i)->getInertia(); + s_t mass = getBodyNode(i)->getMass(); + diag_Is(cursor++) = inertia.getParameter(dynamics::Inertia::Param::I_XX) / mass; + diag_Is(cursor++) = inertia.getParameter(dynamics::Inertia::Param::I_YY) / mass; + diag_Is(cursor++) = inertia.getParameter(dynamics::Inertia::Param::I_ZZ) / mass; + } + return diag_Is; +} + +Eigen::Vector3s Skeleton::getLinkDiagIIndex(size_t index) +{ + Eigen::Vector3s diag_I = Eigen::Vector3s::Zero(); + const Inertia& node_inertia = getBodyNode(index)->getInertia(); + s_t mass = getBodyNode(index)->getMass(); + diag_I(0) = node_inertia.getParameter(dynamics::Inertia::Param::I_XX) / mass; + diag_I(1) = node_inertia.getParameter(dynamics::Inertia::Param::I_YY) / mass; + diag_I(2) = node_inertia.getParameter(dynamics::Inertia::Param::I_ZZ) / mass; + return diag_I; +} + //============================================================================== Eigen::VectorXs Skeleton::getLinkMOIs() { @@ -3127,6 +3159,20 @@ Eigen::VectorXs Skeleton::getLinkMOIs() return inertias; } +//============================================================================== +Eigen::Matrix3s Skeleton::getMOIMatrix(Eigen::Vector6s moi_vector) +{ + Eigen::Matrix3s MOI = moi_vector.segment(0,3).asDiagonal(); + MOI(1,0) = moi_vector(3); + MOI(0,1) = moi_vector(3); + MOI(2,0) = moi_vector(4); + MOI(0,2) = moi_vector(4); + MOI(2,1) = moi_vector(5); + MOI(1,2) = moi_vector(5); + return MOI; +} + +//============================================================================== Eigen::Vector6s Skeleton::getLinkMOIIndex(size_t index) { Eigen::Vector6s inertia = Eigen::Vector6s::Zero(); @@ -3151,6 +3197,43 @@ Eigen::VectorXs Skeleton::getLinkMasses() return masses; } +//============================================================================== +Eigen::MatrixXs Skeleton::getLinkAkMatrixIndex(size_t index) +{ + Eigen::MatrixXs J = getWorldJacobian(getBodyNode(index)); // TODO: May be problematic + Eigen::MatrixXs Jv = J.block(0, 0, 3, J.cols()); + Eigen::MatrixXs Jw = J.block(3, 0, 3, J.cols()); + Eigen::MatrixXs I = getMOIMatrix(getLinkMOIIndex(index)); + Eigen::MatrixXs A_k = Jv.transpose() * Jv + Jw.transpose() * (I/getBodyNode(index)->getMass()) * Jw; + return A_k; +} + +Eigen::MatrixXs Skeleton::getLinkJvkMatrixIndex(size_t index) +{ + Eigen::MatrixXs J = getWorldJacobian(getBodyNode(index)); // TODO: May be problematic + Eigen::MatrixXs Jv = J.block(0, 0, 3, J.cols()); // 3 * N matrix + return Jv; +} + +Eigen::MatrixXs Skeleton::getLinkJwkMatrixIndex(size_t index) +{ + Eigen::MatrixXs J = getWorldJacobian(getBodyNode(index)); + Eigen::MatrixXs Jw = J.block(3, 0, 3, J.cols()); + return Jw; +} + +Eigen::Matrix3s Skeleton::getLinkRMatrixIndex(size_t index) +{ + return getBodyNode(index)->getWorldTransform().linear(); +} + +Eigen::MatrixXs Skeleton::getLinkLocalJwkMatrixIndex(size_t index) +{ + Eigen::MatrixXs J = getJacobian(getBodyNode(index)); + Eigen::MatrixXs Jw = J.block(3, 0, 3, J.cols()); + return Jw; +} + //============================================================================== void Skeleton::setControlForceUpperLimits(Eigen::VectorXs limits) { @@ -3292,6 +3375,53 @@ void Skeleton::setLinkCOMIndex(Eigen::Vector3s com, size_t index) getBodyNode(index)->setInertia(newInertia); } +//============================================================================== +void Skeleton::setLinkDiagIs(Eigen::VectorXs diag_Is) +{ + std::size_t cursor = 0; + for(std::size_t i = 0; i < getNumBodyNodes(); i++) + { + const Inertia& inertia = getBodyNode(i)->getInertia(); + s_t mass = getBodyNode(i)->getMass(); + s_t I_XX = diag_Is(cursor++) * mass; + s_t I_YY = diag_Is(cursor++) * mass; + s_t I_ZZ = diag_Is(cursor++) * mass; + Inertia newInertia( + inertia.getParameter(dynamics::Inertia::Param::MASS), + inertia.getParameter(dynamics::Inertia::Param::COM_X), + inertia.getParameter(dynamics::Inertia::Param::COM_Y), + inertia.getParameter(dynamics::Inertia::Param::COM_Z), + I_XX, + I_YY, + I_ZZ, + inertia.getParameter(dynamics::Inertia::Param::I_XY), + inertia.getParameter(dynamics::Inertia::Param::I_XZ), + inertia.getParameter(dynamics::Inertia::Param::I_YZ)); + getBodyNode(i)->setInertia(newInertia); + } +} + +void Skeleton::setLinkDiagIIndex(Eigen::Vector3s diag_I, size_t index) +{ + const Inertia& inertia = getBodyNode(index)->getInertia(); + s_t mass = getBodyNode(index)->getMass(); + s_t I_XX = diag_I(0) * mass; + s_t I_YY = diag_I(1) * mass; + s_t I_ZZ = diag_I(2) * mass; + Inertia newInertia( + inertia.getParameter(dynamics::Inertia::Param::MASS), + inertia.getParameter(dynamics::Inertia::Param::COM_X), + inertia.getParameter(dynamics::Inertia::Param::COM_Y), + inertia.getParameter(dynamics::Inertia::Param::COM_Z), + I_XX, + I_YY, + I_ZZ, + inertia.getParameter(dynamics::Inertia::Param::I_XY), + inertia.getParameter(dynamics::Inertia::Param::I_XZ), + inertia.getParameter(dynamics::Inertia::Param::I_YZ)); + getBodyNode(index)->setInertia(newInertia); +} + //============================================================================== void Skeleton::setLinkMOIs(Eigen::VectorXs mois) { @@ -7227,6 +7357,16 @@ Eigen::VectorXs Skeleton::getDampingCoeffVector() return damp_coeffs; } +void Skeleton::setDampingCoeffVector(Eigen::VectorXs damp_coeffs) +{ + std::vector dofs = getDofs(); + size_t nDofs = getNumDofs(); + for(int i = 0; i < nDofs; i++) + { + dofs[i]->setDampingCoefficient(damp_coeffs(i)); + } +} + Eigen::VectorXs Skeleton::getDampingForce() { Eigen::VectorXs velocities = getVelocities(); @@ -7248,6 +7388,16 @@ Eigen::VectorXs Skeleton::getSpringStiffVector() return spring_stiffs; } +void Skeleton::setSpringStiffVector(Eigen::VectorXs spring_stiffs) +{ + std::vector dofs = getDofs(); + size_t nDofs = getNumDofs(); + for(int i = 0; i < nDofs; i++) + { + dofs[i]->setSpringStiffness(spring_stiffs(i)); + } +} + Eigen::VectorXs Skeleton::getRestPositions() { std::vector dofs = getDofs(); diff --git a/dart/dynamics/Skeleton.hpp b/dart/dynamics/Skeleton.hpp index c220a5c13..9d2f22e07 100644 --- a/dart/dynamics/Skeleton.hpp +++ b/dart/dynamics/Skeleton.hpp @@ -826,6 +826,8 @@ class Skeleton : public virtual common::VersionCounter, /// Returns the size of the getLinkCOMs() vector std::size_t getLinkCOMDims(); + std::size_t getLinkDiagIDims(); + /// Returns the size of the getLinkMOIs() vector std::size_t getLinkMOIDims(); @@ -849,6 +851,10 @@ class Skeleton : public virtual common::VersionCounter, // This gets particular center-of-mass vectors of a body node Eigen::Vector3s getLinkCOMIndex(size_t index); + Eigen::VectorXs getLinkDiagIs(); + + Eigen::Vector3s getLinkDiagIIndex(size_t index); + // This gets all the inertia moment-of-inertia paremeters for all the links in // this skeleton concatenated together Eigen::VectorXs getLinkMOIs(); @@ -856,10 +862,31 @@ class Skeleton : public virtual common::VersionCounter, // This get particular moment of inertia of a body node Eigen::Vector6s getLinkMOIIndex(size_t index); + // This get all body nodes' moment of inertia in a matrix + Eigen::Matrix3s getMOIMatrix(Eigen::Vector6s moi_vector); + // This returns a vector of all the link masses for all the links in this // skeleton concatenated into a flat vector. Eigen::VectorXs getLinkMasses(); + // This returns a matrix of A_k as describe in paper + // https://arxiv.org/pdf/1907.03964.pdf + // You can see detailed description of this matrix in the last appendix + // section of this paper. + Eigen::MatrixXs getLinkAkMatrixIndex(size_t index); + + // This returns a jacobian matrix of velocity connect between world and joint coordinate + Eigen::MatrixXs getLinkJvkMatrixIndex(size_t index); + + // This returns a jacobian matrix of angular velocity connect between world and joint coordinate + Eigen::MatrixXs getLinkJwkMatrixIndex(size_t index); + + // This returns a rotation matrix of body node wrt to world coordinate + Eigen::Matrix3s getLinkRMatrixIndex(size_t index); + + // This returns a jacobian matrix of angular velocity connect between world and joint coordiante + Eigen::MatrixXs getLinkLocalJwkMatrixIndex(size_t index); + // Sets the upper limits of all the joints from a single vector void setControlForceUpperLimits(Eigen::VectorXs limits); @@ -886,16 +913,26 @@ class Skeleton : public virtual common::VersionCounter, void setLinkBetas(Eigen::VectorXs betas); - // This sets all the inertia center-of-mass vectors for all the links in this + // This sets all the COM vectors for all the links in this // skeleton concatenated together void setLinkCOMs(Eigen::VectorXs coms); + // This set COM of a particular body node specified by index. void setLinkCOMIndex(Eigen::Vector3s com, size_t index); + // This set all the diagonal term of moment of inertias. + void setLinkDiagIs(Eigen::VectorXs mois); + + // This set diagonal term of moment of inertia of a particular + // body node specified by index + void setLinkDiagIIndex(Eigen::Vector3s com, size_t index); + // This sets all the inertia moment-of-inertia paremeters for all the links in // this skeleton concatenated together void setLinkMOIs(Eigen::VectorXs mois); + // This set moment of inertia of a particular body node specified + // by index. void setLinkMOIIndex(Eigen::Vector6s moi, size_t index); // This returns a vector of all the link masses for all the links in this @@ -1859,6 +1896,9 @@ class Skeleton : public virtual common::VersionCounter, // Get damping coefficients Eigen::VectorXs getDampingCoeffVector(); + + // Set Damping Coeff Vector + void setDampingCoeffVector(Eigen::VectorXs damp_coeffs); // Get damping force of the skeleton. Eigen::VectorXs getDampingForce(); @@ -1866,6 +1906,9 @@ class Skeleton : public virtual common::VersionCounter, //Get spring coefficients Eigen::VectorXs getSpringStiffVector(); + //set spring coefficinets + void setSpringStiffVector(Eigen::VectorXs spring_stiffs); + //Get rest positions Eigen::VectorXs getRestPositions(); diff --git a/dart/neural/BackpropSnapshot.cpp b/dart/neural/BackpropSnapshot.cpp index 713266418..8ca5bbf08 100644 --- a/dart/neural/BackpropSnapshot.cpp +++ b/dart/neural/BackpropSnapshot.cpp @@ -13,6 +13,8 @@ #include "dart/neural/DifferentiableContactConstraint.hpp" #include "dart/neural/RestorableSnapshot.hpp" #include "dart/neural/WithRespectToMass.hpp" +#include "dart/neural/WithRespectToDamping.hpp" +#include "dart/neural/WithRespectToSpring.hpp" #include "dart/simulation/World.hpp" // Make production builds happy with asserts @@ -96,13 +98,21 @@ BackpropSnapshot::BackpropSnapshot( // snapshot.restore(); mCachedPosPosDirty = true; + mCachedContactFreePosPosDirty = true; mCachedVelPosDirty = true; + mCachedContactFreeVelPosDirty = true; mCachedBounceApproximationDirty = true; mCachedPosVelDirty = true; + mCachedContactFreePosVelDirty = true; mCachedVelVelDirty = true; + mCachedContactFreeVelVelDirty = true; mCachedForcePosDirty = true; + mCachedContactFreeForcePosDirty = true; mCachedForceVelDirty = true; + mCachedContactFreeForceVelDirty = true; mCachedMassVelDirty = true; + mCachedDampingVelDirty = true; + mCachedSpringVelDirty = true; mCachedVelCDirty = true; mCachedPosCDirty = true; @@ -151,6 +161,8 @@ void BackpropSnapshot::backprop( thisTimestepLoss.lossWrtVelocity = Eigen::VectorXs::Zero(mNumDOFs); thisTimestepLoss.lossWrtTorque = Eigen::VectorXs::Zero(mNumDOFs); thisTimestepLoss.lossWrtMass = Eigen::VectorXs::Zero(world->getMassDims()); + thisTimestepLoss.lossWrtDamping = Eigen::VectorXs::Zero(world->getDampingDims()); + thisTimestepLoss.lossWrtSpring = Eigen::VectorXs::Zero(world->getSpringDims()); // TODO(opt): remove me, as soon as it's faster to construct the Jacobians // using ConstrainedGroups directly. Currently it's redundant to construct @@ -165,6 +177,8 @@ void BackpropSnapshot::backprop( const Eigen::MatrixXs& forceVel = getControlForceVelJacobian(world, thisLog); const Eigen::MatrixXs& massVel = getMassVelJacobian(world, thisLog); + const Eigen::MatrixXs& dampVel = getDampingVelJacobian(world, thisLog); + const Eigen::MatrixXs& springVel = getSpringVelJacobian(world, thisLog); thisTimestepLoss.lossWrtPosition = posPos.transpose() * nextTimestepLoss.lossWrtPosition @@ -176,6 +190,12 @@ void BackpropSnapshot::backprop( = forceVel.transpose() * nextTimestepLoss.lossWrtVelocity; thisTimestepLoss.lossWrtMass = massVel.transpose() * nextTimestepLoss.lossWrtVelocity; + + thisTimestepLoss.lossWrtDamping + = dampVel.transpose() * nextTimestepLoss.lossWrtVelocity; + + thisTimestepLoss.lossWrtSpring + = springVel.transpose() * nextTimestepLoss.lossWrtVelocity; clipLossGradientsToBounds( world, @@ -196,14 +216,19 @@ void BackpropSnapshot::backprop( ////////////////////////////////////////////////////////// // Exploring alternate strategies requires breaking down into the individual // constrained groups and doing backprop there + // Which is basically complimentarity aware gradient ////////////////////////////////////////////////////////// // Handle mass the old fashioned way, for now + // Since massXXX Jacobian is only useful for SSID, we can ignore this temporarily const Eigen::MatrixXs& massVel = getMassVelJacobian(world, thisLog); thisTimestepLoss.lossWrtMass = massVel.transpose() * nextTimestepLoss.lossWrtVelocity; + const Eigen::MatrixXs& dampVel = getDampingVelJacobian(world, thisLog); + thisTimestepLoss.lossWrtDamping + = dampVel.transpose() * nextTimestepLoss.lossWrtVelocity; // Actually run the backprop std::unordered_map skeletonsVisited; @@ -246,7 +271,7 @@ void BackpropSnapshot::backprop( } // Now actually run the backprop - + // explore Alternative Strategies is a flag group->backprop( world, groupThisTimestepLoss, @@ -293,11 +318,16 @@ void BackpropSnapshot::backprop( ///////////////////////////////////////////////////////////////// Eigen::MatrixXs Minv = skel->getInvMassMatrix(); + // Consider spring and damping + Eigen::VectorXs ddamp = getDampingVector(world).segment(dofCursorWorld, dofs); + Eigen::VectorXs spring_stiffs = getSpringStiffVector(world).segment(dofCursorWorld, dofs); Eigen::MatrixXs forceVel = mTimeStep * Minv; Eigen::MatrixXs velVel = Eigen::MatrixXs::Identity(skel->getNumDofs(), skel->getNumDofs()) - - mTimeStep * Minv * skel->getVelCJacobian(); + - mTimeStep * Minv * skel->getVelCJacobian() + - mTimeStep * Minv * ddamp.asDiagonal() + - mTimeStep * mTimeStep * Minv * spring_stiffs.asDiagonal(); Eigen::MatrixXs posVel = skel->getUnconstrainedVelJacobianWrt( world->getTimeStep(), WithRespectTo::POSITION); Eigen::MatrixXs posPos @@ -416,6 +446,7 @@ LossGradientHighLevelAPI BackpropSnapshot::backpropState( grad.lossWrtAction(i) = thisTimestepLoss.lossWrtTorque(actionMapping[i]); } grad.lossWrtMass = thisTimestepLoss.lossWrtMass; + grad.lossWrtDamping = thisTimestepLoss.lossWrtDamping; return grad; } @@ -573,6 +604,57 @@ const Eigen::MatrixXs& BackpropSnapshot::getControlForceVelJacobian( return mCachedForceVel; } +//============================================================================== +const Eigen::MatrixXs& BackpropSnapshot::getContactFreeControlForceVelJacobian( + WorldPtr world, PerformanceLog* perfLog) +{ + #ifndef NDEBUG + assert( + world->getPositions() == mPreStepPosition + && world->getVelocities() == mPreStepVelocity); +#endif + + PerformanceLog* thisLog = nullptr; +#ifdef LOG_PERFORMANCE_BACKPROP_SNAPSHOT + if (perfLog != nullptr) + { + thisLog = perfLog->startRun("BackpropSnapshot.getContactFreeControlForceVelJacobian"); + } +#endif + + if (mCachedContactFreeForceVelDirty) + { + PerformanceLog* refreshLog = nullptr; +#ifdef LOG_PERFORMANCE_BACKPROP_SNAPSHOT + if (thisLog != nullptr) + { + refreshLog = thisLog->startRun( + "BackpropSnapshot.getContactFreeControlForceVelJacobian#refreshCache"); + } +#endif + + Eigen::MatrixXs Minv = getInvMassMatrix(world); + + mCachedContactFreeForceVel = mTimeStep * Minv; + mCachedContactFreeForceVelDirty = false; + +#ifdef LOG_PERFORMANCE_BACKPROP_SNAPSHOT + if (refreshLog != nullptr) + { + refreshLog->end(); + } +#endif + } + +#ifdef LOG_PERFORMANCE_BACKPROP_SNAPSHOT + if (thisLog != nullptr) + { + thisLog->end(); + } +#endif + return mCachedContactFreeForceVel; +} + //============================================================================== /// This computes and returns the whole mass-vel jacobian. For backprop, you /// don't actually need this matrix, you can compute backprop directly. This @@ -639,6 +721,132 @@ const Eigen::MatrixXs& BackpropSnapshot::getMassVelJacobian( return mCachedMassVel; } + +//============================================================================== +/// This computes and returns the whole damping-vel jacobian. For backprop, you +/// don't actually need this matrix, you can compute backprop directly. This +/// is here if you want access to the full Jacobian for some reason. +const Eigen::MatrixXs& BackpropSnapshot::getDampingVelJacobian( + simulation::WorldPtr world, PerformanceLog* perfLog) +{ + #ifndef NDEBUG + assert( + world->getPositions() == mPreStepPosition + && world->getVelocities() == mPreStepVelocity); +#endif + + PerformanceLog* thisLog = nullptr; +#ifdef LOG_PERFORMANCE_BACKPROP_SNAPSHOT + if (perfLog != nullptr) + { + thisLog = perfLog->startRun("BackpropSnapshot.getDampingVelJacobian"); + } +#endif + + if (mCachedDampingVelDirty) + { + PerformanceLog* refreshLog = nullptr; +#ifdef LOG_PERFORMANCE_BACKPROP_SNAPSHOT + if (thisLog != nullptr) + { + refreshLog = thisLog->startRun( + "BackpropSnapshot.getDampingVelJacobian#refreshCache"); + } +#endif + + // std::cout << "Before Fetch Jacobian" << std::endl; + s_t dt = world->getTimeStep(); + Eigen::MatrixXs full_jac = -dt * getInvMassMatrix(world) * world->getVelocities().asDiagonal(); + Eigen::MatrixXs jac = Eigen::MatrixXs::Zero(world->getNumDofs(), world->getDampingDims()); + Eigen::VectorXi dofs_map = world->getDampingDofsMapping(); + for(size_t i = 0; i < world->getDampingDims(); i++) + { + jac.col(i) = full_jac.col(dofs_map(i)); + } + mCachedDampingVel = jac; + mCachedDampingVelDirty = false; + +#ifdef LOG_PERFORMANCE_BACKPROP_SNAPSHOT + if (refreshLog != nullptr) + { + refreshLog->end(); + } +#endif + } + +#ifdef LOG_PERFORMANCE_BACKPROP_SNAPSHOT + if (thisLog != nullptr) + { + thisLog->end(); + } +#endif + return mCachedDampingVel; +} + +//============================================================================== +/// This computes and returns the whole damping-vel jacobian. For backprop, you +/// don't actually need this matrix, you can compute backprop directly. This +/// is here if you want access to the full Jacobian for some reason. +const Eigen::MatrixXs& BackpropSnapshot::getSpringVelJacobian( + simulation::WorldPtr world, PerformanceLog* perfLog) +{ + #ifndef NDEBUG + assert( + world->getPositions() == mPreStepPosition + && world->getVelocities() == mPreStepVelocity); +#endif + + PerformanceLog* thisLog = nullptr; +#ifdef LOG_PERFORMANCE_BACKPROP_SNAPSHOT + if (perfLog != nullptr) + { + thisLog = perfLog->startRun("BackpropSnapshot.getSpringVelJacobian"); + } +#endif + + if (mCachedSpringVelDirty) + { + PerformanceLog* refreshLog = nullptr; +#ifdef LOG_PERFORMANCE_BACKPROP_SNAPSHOT + if (thisLog != nullptr) + { + refreshLog = thisLog->startRun( + "BackpropSnapshot.getSpringVelJacobian#refreshCache"); + } +#endif + + // std::cout << "Before Fetch Jacobian" << std::endl; + s_t dt = world->getTimeStep(); + + Eigen::MatrixXs full_jac = -dt * getInvMassMatrix(world) * (world->getPositions() + - getRestPositions(world) + dt * world->getVelocities()).asDiagonal(); + Eigen::MatrixXs jac = Eigen::MatrixXs::Zero(world->getNumDofs(),world->getSpringDims()); + Eigen::VectorXi dofs_map = world->getSpringDofsMapping(); + for(size_t i = 0; i < world->getSpringDims(); i++) + { + jac.col(i) = full_jac.col(dofs_map(i)); + } + mCachedSpringVel = jac; + mCachedSpringVelDirty = false; + +#ifdef LOG_PERFORMANCE_BACKPROP_SNAPSHOT + if (refreshLog != nullptr) + { + refreshLog->end(); + } +#endif + } + +#ifdef LOG_PERFORMANCE_BACKPROP_SNAPSHOT + if (thisLog != nullptr) + { + thisLog->end(); + } +#endif + return mCachedSpringVel; +} + + //============================================================================== const Eigen::MatrixXs& BackpropSnapshot::getVelVelJacobian( WorldPtr world, PerformanceLog* perfLog) @@ -758,6 +966,67 @@ const Eigen::MatrixXs& BackpropSnapshot::getVelVelJacobian( return mCachedVelVel; } +//============================================================================== +const Eigen::MatrixXs& BackpropSnapshot::getContactFreeVelVelJacobian( + WorldPtr world, PerformanceLog* perfLog) +{ + #ifndef NDEBUG + assert( + world->getPositions() == mPreStepPosition + && world->getVelocities() == mPreStepVelocity); +#endif + + PerformanceLog* thisLog = nullptr; +#ifdef LOG_PERFORMANCE_BACKPROP_SNAPSHOT + if (perfLog != nullptr) + { + thisLog = perfLog->startRun("BackpropSnapshot.getContactFreeVelVelJacobian"); + } +#endif + + if (mCachedContactFreeVelVelDirty) + { + PerformanceLog* refreshLog = nullptr; +#ifdef LOG_PERFORMANCE_BACKPROP_SNAPSHOT + if (thisLog != nullptr) + { + refreshLog = thisLog->startRun( + "BackpropSnapshot.getContactFreeVelVelJacobian#refreshCache"); + } +#endif + + Eigen::VectorXs ddamp = getDampingVector(world); + Eigen::VectorXs spring_stiffs = getSpringStiffVector(world); + Eigen::MatrixXs Minv = getInvMassMatrix(world); + s_t dt = world->getTimeStep(); + Eigen::MatrixXs A_c = getClampingConstraintMatrix(world); + + // Unconditionally treat the jacobian as non contact version + mCachedContactFreeVelVel = Eigen::MatrixXs::Identity(mNumDOFs, mNumDOFs) + - dt * Minv * ddamp.asDiagonal() + - dt * dt * Minv * spring_stiffs.asDiagonal() + - dt * Minv * getVelCJacobian(world); + + + + mCachedContactFreeVelVelDirty = false; +#ifdef LOG_PERFORMANCE_BACKPROP_SNAPSHOT + if (refreshLog != nullptr) + { + refreshLog->end(); + } +#endif + } + +#ifdef LOG_PERFORMANCE_BACKPROP_SNAPSHOT + if (thisLog != nullptr) + { + thisLog->end(); + } +#endif + return mCachedContactFreeVelVel; +} + //============================================================================== const Eigen::MatrixXs& BackpropSnapshot::getPosVelJacobian( WorldPtr world, PerformanceLog* perfLog) @@ -820,6 +1089,68 @@ const Eigen::MatrixXs& BackpropSnapshot::getPosVelJacobian( return mCachedPosVel; } +//============================================================================== +const Eigen::MatrixXs& BackpropSnapshot::getContactFreePosVelJacobian( + WorldPtr world, PerformanceLog* perfLog) +{ + #ifndef NDEBUG + assert( + world->getPositions() == mPreStepPosition + && world->getVelocities() == mPreStepVelocity); +#endif + + PerformanceLog* thisLog = nullptr; +#ifdef LOG_PERFORMANCE_BACKPROP_SNAPSHOT + if (perfLog != nullptr) + { + thisLog = perfLog->startRun("BackpropSnapshot.getContactFreePosVelJacobian"); + } +#endif + + if (mCachedContactFreePosVelDirty) + { + PerformanceLog* refreshLog = nullptr; +#ifdef LOG_PERFORMANCE_BACKPROP_SNAPSHOT + if (thisLog != nullptr) + { + refreshLog = thisLog->startRun( + "BackpropSnapshot.getContactFreePosVelJacobian#refreshCache"); + } +#endif + s_t dt = world->getTimeStep(); + Eigen::VectorXs C = world->getCoriolisAndGravityAndExternalForces(); + Eigen::VectorXs damping_force = getDampingVector(world).asDiagonal() * world->getVelocities(); + Eigen::VectorXs spring_stiffs = getSpringStiffVector(world); + Eigen::VectorXs spring_force = spring_stiffs.asDiagonal() * (world->getPositions() + - getRestPositions(world) + dt * world->getVelocities()); + Eigen::MatrixXs dM = getJacobianOfMinv( + world, + dt * (world->getControlForces() - C - damping_force - spring_force), + WithRespectTo::POSITION); + + Eigen::MatrixXs dC = getJacobianOfC(world, WithRespectTo::POSITION); + + Eigen::MatrixXs Minv = world->getInvMassMatrix(); + mCachedContactFreePosVel = dM - Minv * dt * dC + - Minv * dt * spring_stiffs.asDiagonal(); + + mCachedContactFreePosVelDirty = false; +#ifdef LOG_PERFORMANCE_BACKPROP_SNAPSHOT + if (refreshLog != nullptr) + { + refreshLog->end(); + } +#endif + } + +#ifdef LOG_PERFORMANCE_BACKPROP_SNAPSHOT + if (thisLog != nullptr) + { + thisLog->end(); + } +#endif + return mCachedContactFreePosVel; +} //============================================================================== Eigen::VectorXs BackpropSnapshot::getAnalyticalNextV( simulation::WorldPtr world, bool morePreciseButSlower) @@ -1036,6 +1367,7 @@ Eigen::MatrixXs BackpropSnapshot::getVelJacobianWrt( << std::endl; */ // snapshot.restore(); + // std::cout << "Reach Force" << std::endl; return Minv * ((A_c_ub_E * dF_c) + (dt @@ -1044,10 +1376,10 @@ Eigen::MatrixXs BackpropSnapshot::getVelJacobianWrt( } Eigen::MatrixXs dC = getJacobianOfC(world, wrt); - if (wrt == WithRespectTo::VELOCITY) { // snapshot.restore(); + // std::cout << "Reach Velocity" << std::endl; return Eigen::MatrixXs::Identity(world->getNumDofs(), world->getNumDofs()) + Minv * (A_c_ub_E * dF_c - dt * dC); } @@ -1056,12 +1388,14 @@ Eigen::MatrixXs BackpropSnapshot::getVelJacobianWrt( Eigen::MatrixXs dA_c = getJacobianOfClampingConstraints(world, f_c); Eigen::MatrixXs dA_ubE = getJacobianOfUpperBoundConstraints(world, E * f_c); // snapshot.restore(); + // std::cout << "Reach Position" << std::endl; return dM + Minv * (A_c_ub_E * dF_c + dA_c + dA_ubE - dt * dC) - Minv * dt * spring_stiffs.asDiagonal(); } else { // snapshot.restore(); + // std::cout << "Reach Mass" << std::endl; return dM + Minv * (A_c_ub_E * dF_c - dt * dC); } @@ -1240,6 +1574,18 @@ Eigen::MatrixXs BackpropSnapshot::getStateJacobian(simulation::WorldPtr world) return stateJac; } +//============================================================================== +Eigen::MatrixXs BackpropSnapshot::getContactFreeStateJacobian(simulation::WorldPtr world) +{ + int dofs = world->getNumDofs(); + Eigen::MatrixXs stateJac = Eigen::MatrixXs::Zero(2 * dofs, 2 * dofs); + stateJac.block(0, 0, dofs, dofs) = getContactFreePosPosJacobian(world); + stateJac.block(dofs, 0, dofs, dofs) = getContactFreePosVelJacobian(world); + stateJac.block(0, dofs, dofs, dofs) = getContactFreeVelPosJacobian(world); + stateJac.block(dofs, dofs, dofs, dofs) = getContactFreeVelVelJacobian(world); + return stateJac; +} + //============================================================================== /// This returns the Jacobian for action_t -> state_{t+1}. Eigen::MatrixXs BackpropSnapshot::getActionJacobian(simulation::WorldPtr world) @@ -1259,6 +1605,20 @@ Eigen::MatrixXs BackpropSnapshot::getActionJacobian(simulation::WorldPtr world) return actionJac; } +//============================================================================== +Eigen::MatrixXs BackpropSnapshot::getContactFreeActionJacobian(simulation::WorldPtr world) +{ + int dofs = world->getNumDofs(); + const Eigen::MatrixXs& forceVelJac = getContactFreeControlForceVelJacobian(world); + std::vector actionSpace = world->getActionSpace(); + int actionDim = world->getActionSize(); + Eigen::MatrixXs actionJac = Eigen::MatrixXs::Zero(2 * dofs, actionDim); + for (int i = 0; i < actionDim; i++) + { + actionJac.block(dofs, i, dofs, 1) = forceVelJac.col(actionSpace[i]); + } + return actionJac; +} //============================================================================== const Eigen::MatrixXs& BackpropSnapshot::getPosPosJacobian( WorldPtr world, PerformanceLog* perfLog) @@ -1334,6 +1694,56 @@ const Eigen::MatrixXs& BackpropSnapshot::getPosPosJacobian( return mCachedPosPos; } +//============================================================================== +const Eigen::MatrixXs& BackpropSnapshot::getContactFreePosPosJacobian( + WorldPtr world, PerformanceLog* perfLog) +{ + #ifndef NDEBUG + assert( + world->getPositions() == mPreStepPosition + && world->getVelocities() == mPreStepVelocity); +#endif + + PerformanceLog* thisLog = nullptr; +#ifdef LOG_PERFORMANCE_BACKPROP_SNAPSHOT + if (perfLog != nullptr) + { + thisLog = perfLog->startRun("BackpropSnapshot.getContactFreePosPosJacobian"); + } +#endif + + if (mCachedContactFreePosPosDirty) + { + PerformanceLog* refreshLog = nullptr; +#ifdef LOG_PERFORMANCE_BACKPROP_SNAPSHOT + if (thisLog != nullptr) + { + refreshLog = thisLog->startRun( + "BackpropSnapshot.getContactFreePosPosJacobian#refreshCache"); + } +#endif + + mCachedContactFreePosPos = world->getPosPosJacobian() + * getBounceApproximationJacobian(world, thisLog); + + mCachedContactFreePosPosDirty = false; +#ifdef LOG_PERFORMANCE_BACKPROP_SNAPSHOT + if (refreshLog != nullptr) + { + refreshLog->end(); + } +#endif + } + +#ifdef LOG_PERFORMANCE_BACKPROP_SNAPSHOT + if (thisLog != nullptr) + { + thisLog->end(); + } +#endif + return mCachedContactFreePosPos; +} + //============================================================================== const Eigen::MatrixXs& BackpropSnapshot::getVelPosJacobian( WorldPtr world, PerformanceLog* perfLog) @@ -1399,6 +1809,56 @@ const Eigen::MatrixXs& BackpropSnapshot::getVelPosJacobian( return mCachedVelPos; } +//============================================================================== +const Eigen::MatrixXs& BackpropSnapshot::getContactFreeVelPosJacobian( + WorldPtr world, PerformanceLog* perfLog) +{ + #ifndef NDEBUG + assert( + world->getPositions() == mPreStepPosition + && world->getVelocities() == mPreStepVelocity); +#endif + + PerformanceLog* thisLog = nullptr; +#ifdef LOG_PERFORMANCE_BACKPROP_SNAPSHOT + if (perfLog != nullptr) + { + thisLog = perfLog->startRun("BackpropSnapshot.getContactFreeVelPosJacobian"); + } +#endif + + if (mCachedContactFreeVelPosDirty) + { + PerformanceLog* refreshLog = nullptr; +#ifdef LOG_PERFORMANCE_BACKPROP_SNAPSHOT + if (thisLog != nullptr) + { + refreshLog = thisLog->startRun( + "BackpropSnapshot.getContactFreeVelPosJacobian#refreshCache"); + } +#endif + + mCachedContactFreeVelPos = world->getVelPosJacobian() + * getBounceApproximationJacobian(world, thisLog); + + mCachedContactFreeVelPosDirty = false; +#ifdef LOG_PERFORMANCE_BACKPROP_SNAPSHOT + if (refreshLog != nullptr) + { + refreshLog->end(); + } +#endif + } + +#ifdef LOG_PERFORMANCE_BACKPROP_SNAPSHOT + if (thisLog != nullptr) + { + thisLog->end(); + } +#endif + return mCachedContactFreeVelPos; +} + //============================================================================== Eigen::VectorXs BackpropSnapshot::getPreStepPosition() { @@ -1517,6 +1977,12 @@ Eigen::MatrixXs BackpropSnapshot::getBouncingConstraintMatrix(WorldPtr world) return assembleMatrix(world, MatrixToAssemble::BOUNCING); } +//============================================================================== +Eigen::MatrixXs BackpropSnapshot::getAllConstraintMatrix(simulation::WorldPtr world) +{ + return assembleMatrix(world, MatrixToAssemble::ALL); +} + //============================================================================== Eigen::MatrixXs BackpropSnapshot::getMassMatrix( WorldPtr world, bool forFiniteDifferencing) @@ -1704,6 +2170,12 @@ std::size_t BackpropSnapshot::getNumUpperBound() return mNumUpperBound; } +//============================================================================== +std::size_t BackpropSnapshot::getNumConstraintDim() +{ + return mNumConstraintDim; +} + //============================================================================== /// This is the clamping constraints from all the constrained /// groups, concatenated together @@ -2756,7 +3228,7 @@ Eigen::MatrixXs BackpropSnapshot::getJacobianOfConstraintForce( = Q.completeOrthogonalDecomposition(); Eigen::MatrixXs dB = getJacobianOfLCPOffsetClampingSubset(world, wrt); - + // TODO: Eric should include damping coeff and spring if (wrt == WithRespectTo::VELOCITY || wrt == WithRespectTo::FORCE) { // dQ_b is 0, so don't compute it @@ -4225,6 +4697,8 @@ Eigen::MatrixXs BackpropSnapshot::assembleMatrix( numCols = mNumUpperBound; else if (whichMatrix == MatrixToAssemble::BOUNCING) numCols = mNumBouncing; + else if (whichMatrix == MatrixToAssemble::ALL) + numCols = mNumConstraintDim; Eigen::MatrixXs matrix = Eigen::MatrixXs::Zero(mNumDOFs, numCols); std::size_t constraintCursor = 0; @@ -4242,6 +4716,8 @@ Eigen::MatrixXs BackpropSnapshot::assembleMatrix( groupMatrix = mGradientMatrices[i]->getMassedUpperBoundConstraintMatrix(); else if (whichMatrix == MatrixToAssemble::BOUNCING) groupMatrix = mGradientMatrices[i]->getBouncingConstraintMatrix(); + else if (whichMatrix == MatrixToAssemble::ALL) + groupMatrix = mGradientMatrices[i]->getAllConstraintMatrix(); // shuffle the clamps into the main matrix std::size_t dofCursorGroup = 0; diff --git a/dart/neural/BackpropSnapshot.hpp b/dart/neural/BackpropSnapshot.hpp index d8b773a02..9da2b5f08 100644 --- a/dart/neural/BackpropSnapshot.hpp +++ b/dart/neural/BackpropSnapshot.hpp @@ -70,36 +70,84 @@ class BackpropSnapshot const Eigen::MatrixXs& getVelVelJacobian( simulation::WorldPtr world, PerformanceLog* perfLog = nullptr); + /// This computes and returns the whole vel-vel jacobian. For backprop, you + /// don't actually need this matrix, you can compute backprop directly. This + /// is here if you want access to the full Jacobian for some reason. + /// This assume all contact constraints are separating. + const Eigen::MatrixXs& getContactFreeVelVelJacobian( + simulation::WorldPtr world, PerformanceLog* perfLog = nullptr); + /// This computes and returns the whole pos-vel jacobian. For backprop, you /// don't actually need this matrix, you can compute backprop directly. This /// is here if you want access to the full Jacobian for some reason. const Eigen::MatrixXs& getPosVelJacobian( simulation::WorldPtr world, PerformanceLog* perfLog = nullptr); + /// This computes and returns the whole pos-vel jacobian. For backprop, you + /// don't actually need this matrix, you can compute backprop directly. This + /// is here if you want access to the full Jacobian for some reason. + /// This assume all contact constraints are separating. + const Eigen::MatrixXs& getContactFreePosVelJacobian( + simulation::WorldPtr world, PerformanceLog* perfLog = nullptr); + /// This computes and returns the whole force-vel jacobian. For backprop, you /// don't actually need this matrix, you can compute backprop directly. This /// is here if you want access to the full Jacobian for some reason. const Eigen::MatrixXs& getControlForceVelJacobian( simulation::WorldPtr world, PerformanceLog* perfLog = nullptr); + /// This computes and returns the whole force-vel jacobian. For backprop, you + /// don't actually need this matrix, you can compute backprop directly. This + /// is here if you want access to the full Jacobian for some reason. + /// This assume all contact constraints are separating. + const Eigen::MatrixXs& getContactFreeControlForceVelJacobian( + simulation::WorldPtr world, PerformanceLog* perfLog = nullptr); + /// This computes and returns the whole mass-vel jacobian. For backprop, you /// don't actually need this matrix, you can compute backprop directly. This /// is here if you want access to the full Jacobian for some reason. const Eigen::MatrixXs& getMassVelJacobian( simulation::WorldPtr world, PerformanceLog* perfLog = nullptr); + /// This computes and returns the whole damping coeffient-vel jacobian. For backprop, you + /// don't actually need this matrix, you can compute backprop directly. This + /// is here if you want access to the full Jacobian for some reason. + const Eigen::MatrixXs& getDampingVelJacobian( + simulation::WorldPtr world, PerformanceLog* perfLog = nullptr); + + /// This computes and returns the whole spring stiffness-vel jacobian. For backprop, you + /// don't actually need this matrix, you can compute backprop directly. This + /// is here if you want access to the full Jacobian for some reason. + const Eigen::MatrixXs& getSpringVelJacobian( + simulation::WorldPtr world, PerformanceLog* perfLog = nullptr); + /// This computes and returns the whole pos-pos jacobian. For backprop, you /// don't actually need this matrix, you can compute backprop directly. This /// is here if you want access to the full Jacobian for some reason. const Eigen::MatrixXs& getPosPosJacobian( simulation::WorldPtr world, PerformanceLog* perfLog = nullptr); + /// This computes and returns the whole pos-pos jacobian. For backprop, you + /// don't actually need this matrix, you can compute backprop directly. This + /// is here if you want access to the full Jacobian for some reason. + /// This assume all contact constraints are separating. + const Eigen::MatrixXs& getContactFreePosPosJacobian( + simulation::WorldPtr world, PerformanceLog* perfLog = nullptr); + /// This computes and returns the whole vel-pos jacobian. For backprop, you /// don't actually need this matrix, you can compute backprop directly. This /// is here if you want access to the full Jacobian for some reason. const Eigen::MatrixXs& getVelPosJacobian( simulation::WorldPtr world, PerformanceLog* perfLog = nullptr); + + /// This computes and returns the whole pos-pos jacobian. For backprop, you + /// don't actually need this matrix, you can compute backprop directly. This + /// is here if you want access to the full Jacobian for some reason. + /// This assume all contact constraints are separating + const Eigen::MatrixXs& getContactFreeVelPosJacobian( + simulation::WorldPtr world, PerformanceLog* perfLog = nullptr); + /// This computes and returns the component of the pos-pos and pos-vel /// jacobians due to bounce approximation. For backprop, you don't actually /// need this matrix, you can compute backprop directly. This is here if you @@ -110,8 +158,16 @@ class BackpropSnapshot /// This returns the Jacobian for state_t -> state_{t+1}. Eigen::MatrixXs getStateJacobian(simulation::WorldPtr world); + /// This returns the Jacobian for state_t -> state_{t+1} + /// This assume all contact constraints are separating. + Eigen::MatrixXs getContactFreeStateJacobian(simulation::WorldPtr world); + /// This returns the Jacobian for action_t -> state_{t+1}. Eigen::MatrixXs getActionJacobian(simulation::WorldPtr world); + + /// This returns the Jacobian for action_t -> state_{t+1}. + /// This assume all contact constraints are separating. + Eigen::MatrixXs getContactFreeActionJacobian(simulation::WorldPtr world); /// Returns a concatenated vector of all the Skeletons' position()'s in the /// World, in order in which the Skeletons appear in the World's @@ -179,6 +235,10 @@ class BackpropSnapshot /// just here to enable testing. Eigen::MatrixXs getBouncingConstraintMatrix(simulation::WorldPtr world); + /// This return the entire constraint matrix which may be used to provide heuristic + /// For iLQR planning through contact + Eigen::MatrixXs getAllConstraintMatrix(simulation::WorldPtr world); + /// This returns the mass matrix for the whole world, a block diagonal /// concatenation of the skeleton mass matrices. Eigen::MatrixXs getMassMatrix( @@ -594,6 +654,9 @@ class BackpropSnapshot /// Returns the number of upper bound contacts in this snapshot. std::size_t getNumUpperBound(); + // Returns number of constraint dimension + std::size_t getNumConstraintDim(); + /// These are the gradient constraint matrices from the LCP solver std::vector> mGradientMatrices; @@ -720,24 +783,40 @@ class BackpropSnapshot /// These are mCached versions of the various Jacobians bool mCachedPosPosDirty; Eigen::MatrixXs mCachedPosPos; + bool mCachedContactFreePosPosDirty; + Eigen::MatrixXs mCachedContactFreePosPos; bool mCachedPosVelDirty; Eigen::MatrixXs mCachedPosVel; + bool mCachedContactFreePosVelDirty; + Eigen::MatrixXs mCachedContactFreePosVel; bool mCachedBounceApproximationDirty; Eigen::MatrixXs mCachedBounceApproximation; bool mCachedVelPosDirty; Eigen::MatrixXs mCachedVelPos; + bool mCachedContactFreeVelPosDirty; + Eigen::MatrixXs mCachedContactFreeVelPos; bool mCachedVelVelDirty; Eigen::MatrixXs mCachedVelVel; + bool mCachedContactFreeVelVelDirty; + Eigen::MatrixXs mCachedContactFreeVelVel; bool mCachedForcePosDirty; Eigen::MatrixXs mCachedForcePos; + bool mCachedContactFreeForcePosDirty; + Eigen::MatrixXs mCachedContactFreeForcePos; bool mCachedForceVelDirty; Eigen::MatrixXs mCachedForceVel; + bool mCachedContactFreeForceVelDirty; + Eigen::MatrixXs mCachedContactFreeForceVel; bool mCachedMassVelDirty; Eigen::MatrixXs mCachedMassVel; bool mCachedPosCDirty; Eigen::MatrixXs mCachedPosC; bool mCachedVelCDirty; Eigen::MatrixXs mCachedVelC; + bool mCachedDampingVelDirty; + Eigen::MatrixXs mCachedDampingVel; + bool mCachedSpringVelDirty; + Eigen::MatrixXs mCachedSpringVel; Eigen::VectorXs scratch(simulation::WorldPtr world); @@ -747,7 +826,8 @@ class BackpropSnapshot MASSED_CLAMPING, UPPER_BOUND, MASSED_UPPER_BOUND, - BOUNCING + BOUNCING, + ALL }; Eigen::MatrixXs assembleMatrix( diff --git a/dart/neural/ConstrainedGroupGradientMatrices.cpp b/dart/neural/ConstrainedGroupGradientMatrices.cpp index f11fa5425..39b12d2e1 100644 --- a/dart/neural/ConstrainedGroupGradientMatrices.cpp +++ b/dart/neural/ConstrainedGroupGradientMatrices.cpp @@ -479,6 +479,11 @@ void ConstrainedGroupGradientMatrices::deduplicateConstraints() /// after the LCP has run, with the result from the LCP solver. This can only /// be called once, and after this is called you cannot call /// measureConstraintImpulse() again! +/// This function is used to construct J aka A_c matrix according to forward +/// Detected collision as well as modified overrideClasses +/// This function assume size of overrideClasses equals to size of constrained velocities +/// Not necessary equals to constraint points number +/// Constraints is not limited to contact. void ConstrainedGroupGradientMatrices::constructMatrices( Eigen::VectorXi overrideClasses) { @@ -1917,7 +1922,7 @@ void ConstrainedGroupGradientMatrices::backprop( bool exploreAlternateStrategies) { // First, we compute the correct gradients through backprop - + // The type of contact is consistent with forward pass Eigen::MatrixXs forceVelJacobian = getControlForceVelJacobian(world); // p_t+1 <-- v_t Eigen::MatrixXs posVelJacobian = getPosVelJacobian(world); @@ -1960,6 +1965,8 @@ void ConstrainedGroupGradientMatrices::backprop( */ Eigen::MatrixXs jac = getAllConstraintMatrix(); + // This is a flag variable of velocities of + // each contact point concatenate together. Eigen::VectorXs lossWrtContactVels = jac.transpose() * nextTimestepLoss.lossWrtVelocity; @@ -1970,6 +1977,10 @@ void ConstrainedGroupGradientMatrices::backprop( Eigen::VectorXi overrideClasses = Eigen::VectorXi::Zero(lossWrtContactVels.size()); + + // This operation is on each degree of freedom + // Size of overrideClasses is number of contact point + // for (int i = 0; i < lossWrtContactVels.size(); i++) { // If this is a frictional contact force diff --git a/dart/neural/MappedBackpropSnapshot.cpp b/dart/neural/MappedBackpropSnapshot.cpp index 4021737ee..f1d454eb8 100644 --- a/dart/neural/MappedBackpropSnapshot.cpp +++ b/dart/neural/MappedBackpropSnapshot.cpp @@ -81,6 +81,19 @@ const Eigen::MatrixXs& MappedBackpropSnapshot::getMassVelJacobian( return mBackpropSnapshot->getMassVelJacobian(world, perfLog); } +//============================================================================== +const Eigen::MatrixXs& MappedBackpropSnapshot::getDampingVelJacobian( + std::shared_ptr world, PerformanceLog* perfLog) +{ + return mBackpropSnapshot->getDampingVelJacobian(world, perfLog); +} + +//============================================================================== +const Eigen::MatrixXs& MappedBackpropSnapshot::getSpringVelJacobian( + std::shared_ptr world, PerformanceLog* perfLog) +{ + return mBackpropSnapshot->getSpringVelJacobian(world, perfLog); +} //============================================================================== Eigen::MatrixXs MappedBackpropSnapshot::getPosMappedPosJacobian( std::shared_ptr world, diff --git a/dart/neural/MappedBackpropSnapshot.hpp b/dart/neural/MappedBackpropSnapshot.hpp index 50131e17d..07d70005a 100644 --- a/dart/neural/MappedBackpropSnapshot.hpp +++ b/dart/neural/MappedBackpropSnapshot.hpp @@ -104,6 +104,12 @@ class MappedBackpropSnapshot const Eigen::MatrixXs& getMassVelJacobian( std::shared_ptr world, PerformanceLog* perfLog = nullptr); + const Eigen::MatrixXs& getDampingVelJacobian( + std::shared_ptr world, + PerformanceLog* perfLog = nullptr); + const Eigen::MatrixXs& getSpringVelJacobian( + std::shared_ptr world, + PerformanceLog* perfLog = nullptr); Eigen::MatrixXs getPosMappedPosJacobian( std::shared_ptr world, diff --git a/dart/neural/NeuralUtils.hpp b/dart/neural/NeuralUtils.hpp index 505c15062..6b91e9a93 100644 --- a/dart/neural/NeuralUtils.hpp +++ b/dart/neural/NeuralUtils.hpp @@ -27,6 +27,8 @@ struct LossGradient Eigen::VectorXs lossWrtVelocity; Eigen::VectorXs lossWrtTorque; Eigen::VectorXs lossWrtMass; + Eigen::VectorXs lossWrtDamping; + Eigen::VectorXs lossWrtSpring; }; struct LossGradientHighLevelAPI @@ -34,6 +36,8 @@ struct LossGradientHighLevelAPI Eigen::VectorXs lossWrtState; Eigen::VectorXs lossWrtAction; Eigen::VectorXs lossWrtMass; + Eigen::VectorXs lossWrtDamping; + Eigen::VectorXs lossWrtSpring; }; // We don't issue a full import here, because we want this file to be safe to diff --git a/dart/neural/WithRespectToDamping.cpp b/dart/neural/WithRespectToDamping.cpp new file mode 100644 index 000000000..f675c7538 --- /dev/null +++ b/dart/neural/WithRespectToDamping.cpp @@ -0,0 +1,265 @@ +#include "dart/neural/WithRespectToDamping.hpp" + +#include "dart/dynamics/BodyNode.hpp" +#include "dart/dynamics/Skeleton.hpp" +#include "dart/simulation/World.hpp" + +namespace dart { +namespace neural { + +//============================================================================== +/// Basic constructor +WithRespectToDamping::WithRespectToDamping() +{ +} + +//============================================================================== +WrtDampingJointEntry::WrtDampingJointEntry( + std::string jointName, WrtDampingJointEntryType type, int dofs, Eigen::VectorXi worldDofs) + : jointName(jointName), type(type), mDofs(dofs), mWorldDofs(worldDofs) +{ +} + +//============================================================================== +int WrtDampingJointEntry::dim() +{ + return mDofs; +} + +//============================================================================== +void WrtDampingJointEntry::set( + dynamics::Skeleton* skel, const Eigen::Ref& value) +{ + dynamics::Joint* joint = skel->getJoint(jointName); + for(int i = 0; i < joint->getNumDofs(); i++) + { + joint->setDampingCoefficient(i, value(i)); + } + return; + + +} + +//============================================================================== +void WrtDampingJointEntry::get( + dynamics::Skeleton* skel, Eigen::Ref out) +{ + dynamics::Joint* joint = skel->getJoint(jointName); + for(int i = 0; i < joint->getNumDofs(); i++) + { + out(i) = joint->getDampingCoefficient(i); + } + return; +} + +Eigen::VectorXi WrtDampingJointEntry::getDofMapping() +{ + return mWorldDofs; +} + +//============================================================================== +/// This registers that we'd like to keep track of this node's mass in this +/// way in this differentiation +WrtDampingJointEntry& WithRespectToDamping::registerJoint( + dynamics::Joint* joint, + WrtDampingJointEntryType type, + Eigen::VectorXi dofs_index, + Eigen::VectorXs upperBound, + Eigen::VectorXs lowerBound) +{ + std::string skelName = joint->getSkeleton()->getName(); + std::vector& skelEntries = mEntries[skelName]; + skelEntries.emplace_back(joint->getName(), type, joint->getNumDofs(), dofs_index); + + WrtDampingJointEntry& entry = skelEntries[skelEntries.size() - 1]; + + int dim = entry.dim(); + assert( + dim == lowerBound.size() + && "Lower bound must be same size as requested type would imply"); + assert( + dim == upperBound.size() + && "Upper bound must be same size as requested type would imply"); + + assert(mUpperBounds.size() == mLowerBounds.size()); + + // Append to the lower bound vector + Eigen::VectorXs newDampingLowerBound + = Eigen::VectorXs::Zero(mLowerBounds.size() + dim); + newDampingLowerBound.segment(0, mLowerBounds.size()) = mLowerBounds; + newDampingLowerBound.segment(mLowerBounds.size(), dim) = lowerBound; + mLowerBounds = newDampingLowerBound; + + // Append to the upper bound vector + Eigen::VectorXs newDampingUpperBound + = Eigen::VectorXs::Zero(mUpperBounds.size() + dim); + newDampingUpperBound.segment(0, mUpperBounds.size()) = mUpperBounds; + newDampingUpperBound.segment(mUpperBounds.size(), dim) = upperBound; + mUpperBounds = newDampingUpperBound; + + assert(mUpperBounds.size() == mLowerBounds.size()); + + return entry; +} + +//============================================================================== +/// This returns the entry object corresponding to this node +WrtDampingJointEntry& WithRespectToDamping::getJoint(dynamics::Joint* joint) +{ + std::string skelName = joint->getSkeleton()->getName(); + std::vector& skelEntries = mEntries[skelName]; + for (WrtDampingJointEntry& entry : skelEntries) + { + if (entry.jointName == joint->getName()) + { + return entry; + } + } + assert(false); + // The code should never reach this point, but this is here to keep the + // compiler happy + throw std::runtime_error{"Execution should never reach this point"}; +} + +//============================================================================== +/// This returns this WRT from the world as a vector +Eigen::VectorXs WithRespectToDamping::get(simulation::World* world) +{ + int worldDim = dim(world); + Eigen::VectorXs result = Eigen::VectorXs::Zero(worldDim); + int cursor = 0; + for (int i = 0; i < world->getNumSkeletons(); i++) + { + dynamics::Skeleton* skel = world->getSkeleton(i).get(); + int skelDim = dim(skel); + result.segment(cursor, skelDim) = get(skel); + cursor += skelDim; + } + assert(cursor == worldDim); + return result; +} + +//============================================================================== +/// This returns this WRT from this skeleton as a vector +Eigen::VectorXs WithRespectToDamping::get(dynamics::Skeleton* skel) +{ + std::vector& skelEntries = mEntries[skel->getName()]; + if (skelEntries.size() == 0) + return Eigen::VectorXs::Zero(0); + int cursor = 0; + int skelDim = dim(skel); + Eigen::VectorXs result = Eigen::VectorXs::Zero(skelDim); + for (WrtDampingJointEntry& entry : skelEntries) + { + entry.get(skel, result.segment(cursor, entry.dim())); + cursor += entry.dim(); + } + assert(cursor == skelDim); + return result; +} + +//============================================================================== +/// This sets the world's state based on our WRT +void WithRespectToDamping::set(simulation::World* world, Eigen::VectorXs value) +{ + int cursor = 0; + for (int i = 0; i < world->getNumSkeletons(); i++) + { + dynamics::Skeleton* skel = world->getSkeleton(i).get(); + int skel_dim = dim(skel); + set(skel, value.segment(cursor, skel_dim)); + cursor += skel_dim; + } + assert(cursor == value.size()); +} + +//============================================================================== +/// This sets the skeleton's state based on our WRT +void WithRespectToDamping::set(dynamics::Skeleton* skel, Eigen::VectorXs value) +{ + std::vector& skelEntries = mEntries[skel->getName()]; + if (skelEntries.size() == 0) + return; + int cursor = 0; + for (WrtDampingJointEntry& entry : skelEntries) + { + entry.set(skel, value.segment(cursor, entry.dim())); + cursor += entry.dim(); + } + assert(cursor == value.size()); +} + +//============================================================================== +/// This gives the dimensions of the WRT +int WithRespectToDamping::dim(simulation::World* world) +{ + int worldDim = 0; + for (int i = 0; i < world->getNumSkeletons(); i++) + { + worldDim += dim(world->getSkeleton(i).get()); + } + return worldDim; +} + +//============================================================================== +/// This gives the dimensions of the WRT +int WithRespectToDamping::dim(dynamics::Skeleton* skel) +{ + std::vector& skelEntries = mEntries[skel->getName()]; + int skelDim = 0; + for (WrtDampingJointEntry& entry : skelEntries) + { + skelDim += entry.dim(); + } + return skelDim; +} + +/// This gives a vector of upper bound values for this WRT, given state in the +/// world +Eigen::VectorXs WithRespectToDamping::upperBound(simulation::World* /* world */) +{ + return mUpperBounds; +} + +/// This gives a vector of lower bound values for this WRT, given state in the +/// world +Eigen::VectorXs WithRespectToDamping::lowerBound(simulation::World* /* world */) +{ + return mLowerBounds; +} + +Eigen::VectorXi WithRespectToDamping::getDofsMapping(simulation::World* world) +{ + int worldDim = dim(world); + Eigen::VectorXi result = Eigen::VectorXi::Zero(worldDim); + int cursor = 0; + for (int i = 0; i < world->getNumSkeletons(); i++) + { + dynamics::Skeleton* skel = world->getSkeleton(i).get(); + int skelDim = dim(skel); + result.segment(cursor, skelDim) = getDofsMapping(skel); + cursor += skelDim; + } + assert(cursor == worldDim); + return result; +} + +Eigen::VectorXi WithRespectToDamping::getDofsMapping(dynamics::Skeleton* skel) +{ + std::vector& skelEntries = mEntries[skel->getName()]; + if (skelEntries.size() == 0) + return Eigen::VectorXi::Zero(0); + int cursor = 0; + int skelDim = dim(skel); + Eigen::VectorXi result = Eigen::VectorXi::Zero(skelDim); + for (WrtDampingJointEntry& entry : skelEntries) + { + result.segment(cursor, entry.dim()) = entry.getDofMapping(); + cursor += entry.dim(); + } + assert(cursor == skelDim); + return result; +} + +} // namespace neural +} // namespace dart \ No newline at end of file diff --git a/dart/neural/WithRespectToDamping.hpp b/dart/neural/WithRespectToDamping.hpp new file mode 100644 index 000000000..0b4510dff --- /dev/null +++ b/dart/neural/WithRespectToDamping.hpp @@ -0,0 +1,111 @@ +#ifndef DART_NEURAL_WRT_DAMPING_HPP_ +#define DART_NEURAL_WRT_DAMPING_HPP_ + +#include +#include +#include +#include + +#include + +#include "dart/neural/WithRespectTo.hpp" + +namespace dart { +namespace simulation { +class World; +} + +namespace dynamics { +class Skeleton; +class BodyNode; +class Joint; +} // namespace dynamics + +namespace neural { + +enum WrtDampingJointEntryType +{ + DAMPING +}; + +struct WrtDampingJointEntry +{ + std::string jointName; + WrtDampingJointEntryType type; + int mDofs; + Eigen::VectorXi mWorldDofs; + + WrtDampingJointEntry(std::string jointName, WrtDampingJointEntryType type, int dofs, Eigen::VectorXi worldDofs); + + int dim(); + + void get(dynamics::Skeleton* skel, Eigen::Ref out); + + Eigen::VectorXi getDofMapping(); + + void set(dynamics::Skeleton* skel, const Eigen::Ref& val); +}; + +class WithRespectToDamping : public WithRespectTo +{ +public: + WithRespectToDamping(); + + /// This registers that we'd like to keep track of this node's mass in this + /// way in this differentiation + /// Need to set the dofs + WrtDampingJointEntry& registerJoint( + dynamics::Joint* joint, + WrtDampingJointEntryType type, + Eigen::VectorXi dofs_index, + Eigen::VectorXs upperBound, + Eigen::VectorXs lowerBound); + + /// This returns the entry object corresponding to this node. Throws an + /// assertion if this node doesn't exist + WrtDampingJointEntry& getJoint(dynamics::Joint* joint); + + ////////////////////////////////////////////////////////////// + // Implement all the methods we need + ////////////////////////////////////////////////////////////// + + /// This returns this WRT from the world as a vector + Eigen::VectorXs get(simulation::World* world) override; + + /// This returns this WRT from a skeleton as a vector + Eigen::VectorXs get(dynamics::Skeleton* skel) override; + + /// This sets the world's state based on our WRT + void set(simulation::World* world, Eigen::VectorXs value) override; + + /// This sets the skeleton's state based on our WRT + void set(dynamics::Skeleton* skel, Eigen::VectorXs value) override; + + /// This gives the dimensions of the WRT + int dim(simulation::World* world) override; + + /// This gives the dimensions of the WRT in a single skeleton + int dim(dynamics::Skeleton* skel) override; + + /// This gives a vector of upper bound values for this WRT, given state in the + /// world + Eigen::VectorXs upperBound(simulation::World* world) override; + + /// This gives a vector of lower bound values for this WRT, given state in the + /// world + Eigen::VectorXs lowerBound(simulation::World* world) override; + + Eigen::VectorXi getDofsMapping(simulation::World* world); + + Eigen::VectorXi getDofsMapping(dynamics::Skeleton* skel); + +protected: + std::unordered_map> mEntries; + Eigen::VectorXs mUpperBounds; + Eigen::VectorXs mLowerBounds; +}; + +} // namespace neural +} // namespace dart + +#endif \ No newline at end of file diff --git a/dart/neural/WithRespectToMass.cpp b/dart/neural/WithRespectToMass.cpp index 3744ea835..7d4453168 100644 --- a/dart/neural/WithRespectToMass.cpp +++ b/dart/neural/WithRespectToMass.cpp @@ -31,6 +31,8 @@ int WrtMassBodyNodyEntry::dim() return 1; if (type == INERTIA_DIAGONAL) return 3; + if (type == INERTIA_DIAGONAL_NOMASS) + return 3; if (type == INERTIA_OFF_DIAGONAL) return 3; if (type == INERTIA_FULL) @@ -97,6 +99,21 @@ void WrtMassBodyNodyEntry::set( inertia.getParameter(dynamics::Inertia::Param::I_YZ)); node->setInertia(newInertia); } + else if (type == INERTIA_DIAGONAL_NOMASS) + { + dynamics::Inertia newInertia( + inertia.getParameter(dynamics::Inertia::Param::MASS), + inertia.getParameter(dynamics::Inertia::Param::COM_X), + inertia.getParameter(dynamics::Inertia::Param::COM_Y), + inertia.getParameter(dynamics::Inertia::Param::COM_Z), + value(0)*inertia.getParameter(dynamics::Inertia::Param::MASS), // I_XX + value(1)*inertia.getParameter(dynamics::Inertia::Param::MASS), // I_YY + value(2)*inertia.getParameter(dynamics::Inertia::Param::MASS), // I_ZZ + inertia.getParameter(dynamics::Inertia::Param::I_XY), + inertia.getParameter(dynamics::Inertia::Param::I_XZ), + inertia.getParameter(dynamics::Inertia::Param::I_YZ)); + node->setInertia(newInertia); + } else if (type == INERTIA_OFF_DIAGONAL) { dynamics::Inertia newInertia( @@ -165,6 +182,12 @@ void WrtMassBodyNodyEntry::get( out(1) = moment(1, 1); // I_YY out(2) = moment(2, 2); // I_ZZ } + if(type == INERTIA_DIAGONAL_NOMASS) + { + out(0) = moment(0, 0) / node->getMass(); + out(1) = moment(1, 1) / node->getMass(); + out(2) = moment(2, 2) / node->getMass(); + } else if (type == INERTIA_OFF_DIAGONAL) { out(0) = moment(0, 1); // I_XY diff --git a/dart/neural/WithRespectToMass.hpp b/dart/neural/WithRespectToMass.hpp index ddd59577e..38d1daf0f 100644 --- a/dart/neural/WithRespectToMass.hpp +++ b/dart/neural/WithRespectToMass.hpp @@ -28,6 +28,7 @@ enum WrtMassBodyNodeEntryType INERTIA_COM, INERTIA_COM_MU, INERTIA_DIAGONAL, + INERTIA_DIAGONAL_NOMASS, INERTIA_OFF_DIAGONAL, INERTIA_FULL }; diff --git a/dart/neural/WithRespectToSpring.cpp b/dart/neural/WithRespectToSpring.cpp new file mode 100644 index 000000000..fb155f6e4 --- /dev/null +++ b/dart/neural/WithRespectToSpring.cpp @@ -0,0 +1,265 @@ +#include "dart/neural/WithRespectToSpring.hpp" + +#include "dart/dynamics/BodyNode.hpp" +#include "dart/dynamics/Skeleton.hpp" +#include "dart/simulation/World.hpp" + +namespace dart { +namespace neural { + +//============================================================================== +/// Basic constructor +WithRespectToSpring::WithRespectToSpring() +{ +} + +//============================================================================== +WrtSpringJointEntry::WrtSpringJointEntry( + std::string jointName, WrtSpringJointEntryType type, int dofs, Eigen::VectorXi worldDofs) + : jointName(jointName), type(type), mDofs(dofs), mWorldDofs(worldDofs) +{ +} + +//============================================================================== +int WrtSpringJointEntry::dim() +{ + return mDofs; +} + +//============================================================================== +void WrtSpringJointEntry::set( + dynamics::Skeleton* skel, const Eigen::Ref& value) +{ + dynamics::Joint* joint = skel->getJoint(jointName); + for(int i = 0; i < joint->getNumDofs(); i++) + { + joint->setSpringStiffness(i, value(i)); + } + return; + + +} + +//============================================================================== +void WrtSpringJointEntry::get( + dynamics::Skeleton* skel, Eigen::Ref out) +{ + dynamics::Joint* joint = skel->getJoint(jointName); + for(int i = 0; i < joint->getNumDofs(); i++) + { + out(i) = joint->getSpringStiffness(i); + } + return; +} + +Eigen::VectorXi WrtSpringJointEntry::getDofMapping() +{ + return mWorldDofs; +} + +//============================================================================== +/// This registers that we'd like to keep track of this node's mass in this +/// way in this differentiation +WrtSpringJointEntry& WithRespectToSpring::registerJoint( + dynamics::Joint* joint, + WrtSpringJointEntryType type, + Eigen::VectorXi dofs_index, + Eigen::VectorXs upperBound, + Eigen::VectorXs lowerBound) +{ + std::string skelName = joint->getSkeleton()->getName(); + std::vector& skelEntries = mEntries[skelName]; + skelEntries.emplace_back(joint->getName(), type, joint->getNumDofs(), dofs_index); + + WrtSpringJointEntry& entry = skelEntries[skelEntries.size() - 1]; + + int dim = entry.dim(); + assert( + dim == lowerBound.size() + && "Lower bound must be same size as requested type would imply"); + assert( + dim == upperBound.size() + && "Upper bound must be same size as requested type would imply"); + + assert(mUpperBounds.size() == mLowerBounds.size()); + + // Append to the lower bound vector + Eigen::VectorXs newDampingLowerBound + = Eigen::VectorXs::Zero(mLowerBounds.size() + dim); + newDampingLowerBound.segment(0, mLowerBounds.size()) = mLowerBounds; + newDampingLowerBound.segment(mLowerBounds.size(), dim) = lowerBound; + mLowerBounds = newDampingLowerBound; + + // Append to the upper bound vector + Eigen::VectorXs newDampingUpperBound + = Eigen::VectorXs::Zero(mUpperBounds.size() + dim); + newDampingUpperBound.segment(0, mUpperBounds.size()) = mUpperBounds; + newDampingUpperBound.segment(mUpperBounds.size(), dim) = upperBound; + mUpperBounds = newDampingUpperBound; + + assert(mUpperBounds.size() == mLowerBounds.size()); + + return entry; +} + +//============================================================================== +/// This returns the entry object corresponding to this node +WrtSpringJointEntry& WithRespectToSpring::getJoint(dynamics::Joint* joint) +{ + std::string skelName = joint->getSkeleton()->getName(); + std::vector& skelEntries = mEntries[skelName]; + for (WrtSpringJointEntry& entry : skelEntries) + { + if (entry.jointName == joint->getName()) + { + return entry; + } + } + assert(false); + // The code should never reach this point, but this is here to keep the + // compiler happy + throw std::runtime_error{"Execution should never reach this point"}; +} + +//============================================================================== +/// This returns this WRT from the world as a vector +Eigen::VectorXs WithRespectToSpring::get(simulation::World* world) +{ + int worldDim = dim(world); + Eigen::VectorXs result = Eigen::VectorXs::Zero(worldDim); + int cursor = 0; + for (int i = 0; i < world->getNumSkeletons(); i++) + { + dynamics::Skeleton* skel = world->getSkeleton(i).get(); + int skelDim = dim(skel); + result.segment(cursor, skelDim) = get(skel); + cursor += skelDim; + } + assert(cursor == worldDim); + return result; +} + +//============================================================================== +/// This returns this WRT from this skeleton as a vector +Eigen::VectorXs WithRespectToSpring::get(dynamics::Skeleton* skel) +{ + std::vector& skelEntries = mEntries[skel->getName()]; + if (skelEntries.size() == 0) + return Eigen::VectorXs::Zero(0); + int cursor = 0; + int skelDim = dim(skel); + Eigen::VectorXs result = Eigen::VectorXs::Zero(skelDim); + for (WrtSpringJointEntry& entry : skelEntries) + { + entry.get(skel, result.segment(cursor, entry.dim())); + cursor += entry.dim(); + } + assert(cursor == skelDim); + return result; +} + +//============================================================================== +/// This sets the world's state based on our WRT +void WithRespectToSpring::set(simulation::World* world, Eigen::VectorXs value) +{ + int cursor = 0; + for (int i = 0; i < world->getNumSkeletons(); i++) + { + dynamics::Skeleton* skel = world->getSkeleton(i).get(); + int skel_dim = dim(skel); + set(skel, value.segment(cursor, skel_dim)); + cursor += skel_dim; + } + assert(cursor == value.size()); +} + +//============================================================================== +/// This sets the skeleton's state based on our WRT +void WithRespectToSpring::set(dynamics::Skeleton* skel, Eigen::VectorXs value) +{ + std::vector& skelEntries = mEntries[skel->getName()]; + if (skelEntries.size() == 0) + return; + int cursor = 0; + for (WrtSpringJointEntry& entry : skelEntries) + { + entry.set(skel, value.segment(cursor, entry.dim())); + cursor += entry.dim(); + } + assert(cursor == value.size()); +} + +//============================================================================== +/// This gives the dimensions of the WRT +int WithRespectToSpring::dim(simulation::World* world) +{ + int worldDim = 0; + for (int i = 0; i < world->getNumSkeletons(); i++) + { + worldDim += dim(world->getSkeleton(i).get()); + } + return worldDim; +} + +//============================================================================== +/// This gives the dimensions of the WRT +int WithRespectToSpring::dim(dynamics::Skeleton* skel) +{ + std::vector& skelEntries = mEntries[skel->getName()]; + int skelDim = 0; + for (WrtSpringJointEntry& entry : skelEntries) + { + skelDim += entry.dim(); + } + return skelDim; +} + +/// This gives a vector of upper bound values for this WRT, given state in the +/// world +Eigen::VectorXs WithRespectToSpring::upperBound(simulation::World* /* world */) +{ + return mUpperBounds; +} + +/// This gives a vector of lower bound values for this WRT, given state in the +/// world +Eigen::VectorXs WithRespectToSpring::lowerBound(simulation::World* /* world */) +{ + return mLowerBounds; +} + +Eigen::VectorXi WithRespectToSpring::getDofsMapping(simulation::World* world) +{ + int worldDim = dim(world); + Eigen::VectorXi result = Eigen::VectorXi::Zero(worldDim); + int cursor = 0; + for (int i = 0; i < world->getNumSkeletons(); i++) + { + dynamics::Skeleton* skel = world->getSkeleton(i).get(); + int skelDim = dim(skel); + result.segment(cursor, skelDim) = getDofsMapping(skel); + cursor += skelDim; + } + assert(cursor == worldDim); + return result; +} + +Eigen::VectorXi WithRespectToSpring::getDofsMapping(dynamics::Skeleton* skel) +{ + std::vector& skelEntries = mEntries[skel->getName()]; + if (skelEntries.size() == 0) + return Eigen::VectorXi::Zero(0); + int cursor = 0; + int skelDim = dim(skel); + Eigen::VectorXi result = Eigen::VectorXi::Zero(skelDim); + for (WrtSpringJointEntry& entry : skelEntries) + { + result.segment(cursor, entry.dim()) = entry.getDofMapping(); + cursor += entry.dim(); + } + assert(cursor == skelDim); + return result; +} + +} // namespace neural +} // namespace dart \ No newline at end of file diff --git a/dart/neural/WithRespectToSpring.hpp b/dart/neural/WithRespectToSpring.hpp new file mode 100644 index 000000000..af4cc5532 --- /dev/null +++ b/dart/neural/WithRespectToSpring.hpp @@ -0,0 +1,111 @@ +#ifndef DART_NEURAL_WRT_SPRING_HPP_ +#define DART_NEURAL_WRT_SPRING_HPP_ + +#include +#include +#include +#include + +#include + +#include "dart/neural/WithRespectTo.hpp" + +namespace dart { +namespace simulation { +class World; +} + +namespace dynamics { +class Skeleton; +class BodyNode; +class Joint; +} // namespace dynamics + +namespace neural { + +enum WrtSpringJointEntryType +{ + SPRING +}; + +struct WrtSpringJointEntry +{ + std::string jointName; + WrtSpringJointEntryType type; + int mDofs; + Eigen::VectorXi mWorldDofs; + + WrtSpringJointEntry(std::string jointName, WrtSpringJointEntryType type, int dofs, Eigen::VectorXi worldDofs); + + int dim(); + + void get(dynamics::Skeleton* skel, Eigen::Ref out); + + Eigen::VectorXi getDofMapping(); + + void set(dynamics::Skeleton* skel, const Eigen::Ref& val); +}; + +class WithRespectToSpring : public WithRespectTo +{ +public: + WithRespectToSpring(); + + /// This registers that we'd like to keep track of this node's mass in this + /// way in this differentiation + /// Need to set the dofs + WrtSpringJointEntry& registerJoint( + dynamics::Joint* joint, + WrtSpringJointEntryType type, + Eigen::VectorXi dofs_index, + Eigen::VectorXs upperBound, + Eigen::VectorXs lowerBound); + + /// This returns the entry object corresponding to this node. Throws an + /// assertion if this node doesn't exist + WrtSpringJointEntry& getJoint(dynamics::Joint* joint); + + ////////////////////////////////////////////////////////////// + // Implement all the methods we need + ////////////////////////////////////////////////////////////// + + /// This returns this WRT from the world as a vector + Eigen::VectorXs get(simulation::World* world) override; + + /// This returns this WRT from a skeleton as a vector + Eigen::VectorXs get(dynamics::Skeleton* skel) override; + + /// This sets the world's state based on our WRT + void set(simulation::World* world, Eigen::VectorXs value) override; + + /// This sets the skeleton's state based on our WRT + void set(dynamics::Skeleton* skel, Eigen::VectorXs value) override; + + /// This gives the dimensions of the WRT + int dim(simulation::World* world) override; + + /// This gives the dimensions of the WRT in a single skeleton + int dim(dynamics::Skeleton* skel) override; + + /// This gives a vector of upper bound values for this WRT, given state in the + /// world + Eigen::VectorXs upperBound(simulation::World* world) override; + + /// This gives a vector of lower bound values for this WRT, given state in the + /// world + Eigen::VectorXs lowerBound(simulation::World* world) override; + + Eigen::VectorXi getDofsMapping(simulation::World* world); + + Eigen::VectorXi getDofsMapping(dynamics::Skeleton* skel); + +protected: + std::unordered_map> mEntries; + Eigen::VectorXs mUpperBounds; + Eigen::VectorXs mLowerBounds; +}; + +} // namespace neural +} // namespace dart + +#endif \ No newline at end of file diff --git a/dart/proto/TrajectoryRollout.proto b/dart/proto/TrajectoryRollout.proto index bb7e307cb..d7cde89cd 100644 --- a/dart/proto/TrajectoryRollout.proto +++ b/dart/proto/TrajectoryRollout.proto @@ -10,5 +10,7 @@ message TrajectoryRollout { map vel = 3; map force = 4; VectorXs mass = 5; - map metadata = 6; + VectorXs damping = 6; + VectorXs spring = 7; + map metadata = 8; } \ No newline at end of file diff --git a/dart/realtime/MPCLocal.cpp b/dart/realtime/MPCLocal.cpp index 2ee08d340..a89f1636f 100644 --- a/dart/realtime/MPCLocal.cpp +++ b/dart/realtime/MPCLocal.cpp @@ -26,7 +26,8 @@ namespace realtime { MPCLocal::MPCLocal( std::shared_ptr world, std::shared_ptr loss, - int planningHorizonMillis) + int planningHorizonMillis, + s_t scale) : mRunning(false), mWorld(world), mLoss(loss), @@ -39,7 +40,7 @@ MPCLocal::MPCLocal( mEnableOptimizationGuards(false), mRecordIterations(false), mPlanningHorizonMillis(planningHorizonMillis), - mMillisPerStep(1000 * world->getTimeStep()), + mMillisPerStep(scale*1000 * world->getTimeStep()), mSteps((int)ceil((s_t)planningHorizonMillis / mMillisPerStep)), mShotLength(50), mMaxIterations(5), @@ -199,7 +200,7 @@ void MPCLocal::optimizePlan(long startTime) mBuffer.estimateWorldStateAt(worldClone, &mObservationLog, startTime); estimateState->end(); - std::cout<<"Optimization Stage"<startRun("Create Default IPOPT"); @@ -246,14 +247,16 @@ void MPCLocal::optimizePlan(long startTime) mProblem->getRolloutCache(worldClone)->getControlForcesConst()); log->end(); - - std::cout << PerformanceLog::finalize()["MPCLocal loop"]->prettyPrint() + if(!mSilent) + { + std::cout << PerformanceLog::finalize()["MPCLocal loop"]->prettyPrint() << std::endl; + } } else { std::shared_ptr worldClone = mWorld->clone(); - std::cout<<"Re-optimization stage "<(floor(static_cast(diff) / mMillisPerStep)); @@ -516,31 +519,28 @@ bool MPCLocal::variableChange() return mVarchange; } -void MPCLocal::setMasschange(s_t mass) +void MPCLocal::setParameterChange(Eigen::VectorXs params) { - if(abs(mass-pre_mass)>0.001) + if(mInitialized == false) { + mPre_parameter = params; + mInitialized = true; mVarchange = true; } - pre_mass = mass; -} - -void MPCLocal::setCOMchange(Eigen::Vector3s com) -{ - if((com-pre_com).norm()>0.001) + else { - mVarchange = true; + assert(params.size() == mPre_parameter.size()); + if((params-mPre_parameter).norm()>0.001) + { + mVarchange = true; + } + mPre_parameter = params; } - pre_com = com; } -void MPCLocal::setMUchange(s_t mu) +void MPCLocal::setReOptThreshold(s_t thresh) { - if(abs(mu-pre_mu) > 0.001) - { - mVarchange = true; - } - pre_mu = mu; + mReOpt_thresh = thresh; } } // namespace realtime diff --git a/dart/realtime/MPCLocal.hpp b/dart/realtime/MPCLocal.hpp index 66df12a54..ce32fe1d2 100644 --- a/dart/realtime/MPCLocal.hpp +++ b/dart/realtime/MPCLocal.hpp @@ -36,7 +36,8 @@ class MPCLocal final : public MPC MPCLocal( std::shared_ptr world, std::shared_ptr loss, - int planningHorizonMillis); + int planningHorizonMillis, + s_t scale); /// Copy constructor MPCLocal(const MPCLocal& mpc); @@ -139,11 +140,9 @@ class MPCLocal final : public MPC bool variableChange(); - void setMasschange(s_t mass); + void setParameterChange(Eigen::VectorXs params); - void setCOMchange(Eigen::Vector3s com); - - void setMUchange(s_t mu); + void setReOptThreshold(s_t thresh); protected: /// This is the function for the optimization thread to run when we're live @@ -169,9 +168,11 @@ class MPCLocal final : public MPC RealTimeControlBuffer mBuffer; std::thread mOptimizationThread; bool mSilent; - s_t pre_mass; - s_t pre_mu; - Eigen::Vector3s pre_com; + // The shape of mass should be equal to number of body nodes that registered + Eigen::VectorXs mPre_parameter; + bool mInitialized = false; + s_t mReOpt_thresh; + bool mVarchange = true; std::shared_ptr mOptimizer; diff --git a/dart/realtime/MappedTargetReachingCost.cpp b/dart/realtime/MappedTargetReachingCost.cpp new file mode 100644 index 000000000..183597b17 --- /dev/null +++ b/dart/realtime/MappedTargetReachingCost.cpp @@ -0,0 +1,548 @@ +#include +#include +#include "dart/math/MathTypes.hpp" +#include "dart/realtime/MappedTargetReachingCost.hpp" + +namespace dart { +using namespace trajectory; + +namespace realtime { + +MappedTargetReachingCost::MappedTargetReachingCost( + Eigen::VectorXs runningStateWeight, + Eigen::VectorXs runningActionWeight, + Eigen::VectorXs finalStateWeight, + std::shared_ptr world): + mRunningStateWeight(runningStateWeight), + mRunningActionWeight(runningActionWeight), + mFinalStateWeight(finalStateWeight), + mStateDim(world->getNumDofs() * 2), + mActionDim(runningActionWeight.size()), + mWorld(world), + mMapping(world) +{ + std::cout << "State and Action Dim in Cost Fn: " << mStateDim << " " << mActionDim << std::endl; + mRunningActionWeight = runningActionWeight; +} + +std::vector MappedTargetReachingCost::ilqrGradientEstimator( + const TrajectoryRollout* rollout, + s_t& total_cost, + WRTFLAG wrt) +{ + total_cost = computeLoss(rollout); + int steps = rollout->getPosesConst().cols(); + Eigen::MatrixXs grad; + if(wrt == WRTFLAG::X) + { + grad = Eigen::MatrixXs::Zero(mStateDim, steps); + computeGradX(rollout, grad); + } + else if(wrt == WRTFLAG::U) + { + grad = Eigen::MatrixXs::Zero(mActionDim, steps-1); + computeGradU(rollout, grad); + } + else + assert(false && "Shouldn't reach here"); + std::vector output; + + for(int i = 0; i < grad.cols(); i++) + { + Eigen::VectorXs col = grad.col(i); + output.push_back(col); + } + return output; +} + +std::vector MappedTargetReachingCost::ilqrHessianEstimator( + const TrajectoryRollout* rollout, + WRTFLAG wrt) +{ + std::vector hess; + switch (wrt) + { + case WRTFLAG::U: + case WRTFLAG::X: + assert(false && "Should not use grad flag"); + break; + case WRTFLAG::XX: + computeHessXX(rollout, hess); + break; + case WRTFLAG::UU: + computeHessUU(rollout, hess); + break; + case WRTFLAG::UX: + computeHessUX(rollout, hess); + break; + case WRTFLAG::XU: + computeHessXU(rollout, hess); + break; + } + return hess; +} + +s_t MappedTargetReachingCost::loss(const TrajectoryRollout* rollout) +{ + return computeLoss(rollout); +} + +s_t MappedTargetReachingCost::lossGrad(const TrajectoryRollout* rollout, TrajectoryRollout* gradWrtRollout) +{ + int steps = rollout->getPosesConst().cols(); + Eigen::MatrixXs grad_x = Eigen::MatrixXs::Zero(mStateDim, steps); + Eigen::MatrixXs grad_u = Eigen::MatrixXs::Zero((int)(mStateDim/2), steps); + computeGradX(rollout, grad_x); + computeGradForce(rollout, grad_u.block(0, 0, (int)(mStateDim/2), steps-1)); + gradWrtRollout->getPoses().setZero(); + gradWrtRollout->getVels().setZero(); + gradWrtRollout->getControlForces().setZero(); + gradWrtRollout->getPoses().block(0, 0, (int)(mStateDim/2), steps) + = grad_x.block(0, 0, (int)(mStateDim/2), steps); + gradWrtRollout->getVels().block(0, 0, (int)(mStateDim/2), steps) + = grad_x.block((int)(mStateDim/2), 0, (int)(mStateDim/2), steps); + gradWrtRollout->getControlForces().block(0, 0, (int)(mStateDim/2), steps) + = grad_u; + return computeLoss(rollout); +} + +std::shared_ptr MappedTargetReachingCost::getLossFn() +{ + boost::function + loss_(boost::bind(&MappedTargetReachingCost::loss, this, _1)); + boost::function + lossGrad_(boost::bind(&MappedTargetReachingCost::lossGrad, this, _1, _2)); + return std::make_shared(loss_, lossGrad_); +} + +void MappedTargetReachingCost::setTarget(Eigen::VectorXs target) +{ + assert(target.size() == mStateDim); + mTarget = target; +} + +void MappedTargetReachingCost::setTimeStep(s_t timestep) +{ + assert(timestep > 0.001); + dt = timestep; +} + +/// Must be set +void MappedTargetReachingCost::setSSIDNodeIndex(std::vector ssid_index) +{ + mSSIDNodeIndex = ssid_index; + for(int i = 0; i < ssid_index.size(); i++) + { + mAks.push_back(std::vector()); + } +} + +void MappedTargetReachingCost::setMapping(neural::IKMapping mapping) +{ + mMapping = mapping; +} + +void MappedTargetReachingCost::setLinkLength(Eigen::VectorXi lengths) +{ + mLinkLength = lengths; +} + +void MappedTargetReachingCost::enableSSIDLoss(s_t weight) +{ + mUseSSIDHeuristic = true; + mSSIDHeuristicWeight = weight; +} + +// For the Velocity we can use loss or not use +// TODO: May need to be modified +s_t MappedTargetReachingCost::computeLoss(const TrajectoryRollout* rollout) +{ + s_t loss = 0; + int steps = rollout->getPosesConst().cols(); + // Compute Running State and Loss from target reaching + Eigen::VectorXs init_state = mWorld->getState(); + for(int i = 0; i < mAks.size(); i++) + { + mAks[i].clear(); + } + for(int i = 0; i < steps - 1; i++) + { + Eigen::VectorXs state = Eigen::VectorXs::Zero(mMappedStateDim); + Eigen::VectorXs action = mWorld->mapToActionSpaceVector(rollout->getControlForcesConst().col(i)); + + state.segment(0, (int)(mMappedStateDim/2)) = getCartesianPos(rollout->getPosesConst().col(i)); + state.segment((int)(mMappedStateDim/2), (int)(mMappedStateDim/2)) + = getCartesianVel(rollout->getPosesConst().col(i) ,rollout->getVelsConst().col(i)); + + loss += (mRunningStateWeight.asDiagonal() * ((state - mTarget).cwiseAbs2())).sum() * dt; + loss += (mRunningActionWeight.asDiagonal() * (action.cwiseAbs2())).sum() * dt; + if(mUseSSIDHeuristic && i >= 1) + { + mWorld->setState(state); + for(int j = 0; j < mSSIDNodeIndex.size(); j++) + { + Eigen::MatrixXs Ak = mWorld->getLinkAkMatrixIndex(mSSIDNodeIndex[j]); + mAks[j].push_back(Ak); + loss += mSSIDHeuristicWeight * (Ak * (rollout->getVelsConst().col(i) - rollout->getVelsConst().col(i-1))).cwiseAbs2().sum() * dt; + } + } + } + mWorld->setState(init_state); + // std::cout<< "None Final Loss: " << loss << std::endl; + // Add Final State Error + Eigen::VectorXs finalState = Eigen::VectorXs::Zero(mStateDim); + finalState.segment(0, (int)(mMappedStateDim/2)) = rollout->getPosesConst().col(steps - 1); + finalState.segment((int)(mMappedStateDim/2), (int)(mStateDim/2)) = rollout->getVelsConst().col(steps - 1); + s_t final_loss = (mFinalStateWeight.asDiagonal() * ((finalState - mTarget).cwiseAbs2())).sum(); + // std::cout << "Final Loss:\n " << final_loss << std::endl; + loss += final_loss; + return loss; +} + +// TODO: Need to be modified need derivation +void MappedTargetReachingCost::computeGradX(const TrajectoryRollout* rollout, Eigen::Ref grads) +{ + int steps = rollout->getPosesConst().cols(); + assert(grads.cols() == steps && grads.rows() == mStateDim); + // Compute Grad for running state + for(int i = 0; i < steps - 1; i++) + { + Eigen::VectorXs state = Eigen::VectorXs::Zero(mMappedStateDim); + Eigen::VectorXs Jac = getStateJacobian(rollout->getPosesConst().col(i),rollout->getVelsConst().col(i)); + //std::cout<< "State Dim: " << mStateDim << "Half Dim: " << (int)(mStateDim/2) << std::endl; + state.segment(0, (int)(mMappedStateDim/2)) = getCartesianPos(rollout->getPosesConst().col(i)); + state.segment((int)(mMappedStateDim/2), (int)(mMappedStateDim/2)) + = getCartesianVel(rollout->getPosesConst().col(i), rollout->getVelsConst().col(i)); + + grads.col(i) = Jac * (2*mRunningStateWeight.asDiagonal() * (state - mTarget) * dt); + if(mUseSSIDHeuristic && i >= 1) + { + for(int j = 0; j < mSSIDNodeIndex.size(); j++) + { + Eigen::VectorXs new_grad = 2 * mSSIDHeuristicWeight * (mAks[j][i-1].transpose() * mAks[j][i-1]) * (rollout->getVelsConst().col(i) - rollout->getVelsConst().col(i-1)) * dt; + // std::cout << new_grad.cols() << " " << grads.block((int)(mStateDim/2), i, (int)(mStateDim/2), 1) << std::endl; + grads.block((int)(mStateDim/2), i, (int)(mStateDim/2), 1) += new_grad; + } + } + } + // Compute Grad for final state + Eigen::VectorXs state = Eigen::VectorXs::Zero(mMappedStateDim); + Eigen::VectorXs Jac = getStateJacobian(rollout->getPosesConst().col(steps-1),rollout->getVelsConst().col(steps-1)); + state.segment(0, (int)(mMappedStateDim/2)) = getCartesianPos(rollout->getPosesConst().col(steps - 1)); + state.segment((int)(mMappedStateDim/2), (int)(mMappedStateDim/2)) + = getCartesianVel(rollout->getPosesConst().col(steps-1) ,rollout->getVelsConst().col(steps - 1)); + grads.col(steps - 1) = 2 * mFinalStateWeight.asDiagonal() * (state - mTarget); +} + +void MappedTargetReachingCost::computeGradU(const TrajectoryRollout* rollout, + Eigen::Ref grads) +{ + int steps = rollout->getPosesConst().cols(); + assert(grads.cols() == steps - 1 && grads.rows() == mActionDim); + + // Compute Grad for Running action + for(int i = 0; i < steps - 1; i++) + { + Eigen::VectorXs action; + action = mWorld->mapToActionSpaceVector(rollout->getControlForcesConst().col(i)); + grads.col(i) = 2 * mRunningActionWeight.asDiagonal() * action * dt; + + } +} + +// Here compute gradients of actions which will not have final steps +void MappedTargetReachingCost::computeGradForce(const TrajectoryRollout* rollout, + Eigen::Ref grads) +{ + int steps = rollout->getPosesConst().cols(); + assert(grads.cols() == steps - 1 && grads.rows() == (int)(mStateDim/2)); + + // Compute Grad for Running action + for(int i = 0; i < steps - 1; i++) + { + Eigen::VectorXs force; + force = rollout->getControlForcesConst().col(i); + grads.col(i) = 2 * mWorld->mapToForceSpaceVector(mRunningActionWeight).asDiagonal() * force * dt; + + } +} + + +// Compute Hessian of XX which should have length of steps +// Assume hess is an empty vector +// TODO: Need to be modified +void MappedTargetReachingCost::computeHessXX( + const TrajectoryRollout* rollout, std::vector &hess) +{ + assert(hess.size() == 0); + int steps = rollout->getPosesConst().cols(); + Eigen::MatrixXs hess_xx; + for(int i = 0; i < steps - 1; i++) + { + Eigen::MatrixXs jac + = getStateJacobian(rollout->getPosesConst().col(i),rollout->getVelsConst().col(i)); + Eigen::VectorXs mapped_state = Eigen::VectorXs::Zero(mMappedStateDim); + Eigen::VectorXs mapped_pos = getCartesianPos(rollout->getPosesConst().col(i)); + Eigen::VectorXs mapped_vel + = getCartesianVel(rollout->getPosesConst().col(i), rollout->getVelsConst().col(i)); + mapped_state.segment(0, (int)(mMappedStateDim/2)) + = mapped_pos; + mapped_state.segment((int)(mMappedStateDim/2), (int)(mMappedStateDim/2)) + = mapped_vel; + // hess: mStateDim , mMappedStateDim + // jac: mMappStateDim, mStateDim + Eigen::VectorXs diff = mapped_state - mTarget; + hess_xx = 2 * dt * (mRunningStateWeight(0) * diff(0) * getXHessian(mapped_pos) + + mRunningStateWeight(1) * diff(1) * getYHessian(mapped_pos) + + mRunningStateWeight(3) * diff(3) * getVxHessian(mapped_pos, mapped_vel) + + mRunningStateWeight(4) * diff(4) * getVyHessian(mapped_pos, mapped_vel) + + jac.transpose() * mRunningStateWeight.asDiagonal() * jac); + if(mUseSSIDHeuristic && i >= 1) + { + for(int j = 0; j < mSSIDNodeIndex.size(); j++) + { + hess_xx.block((int)(mStateDim/2), (int)(mStateDim/2), (int)(mStateDim/2), (int)(mStateDim/2)) + += mSSIDHeuristicWeight * mAks[j][i-1].transpose() * mAks[j][i-1] * dt; + } + } + hess.push_back(hess_xx); + } + + + Eigen::MatrixXs jac + = getStateJacobian(rollout->getPosesConst().col(steps-1),rollout->getVelsConst().col(steps-1)); + Eigen::VectorXs mapped_state = Eigen::VectorXs::Zero(mMappedStateDim); + Eigen::VectorXs mapped_pos = getCartesianPos(rollout->getPosesConst().col(steps-1)); + Eigen::VectorXs mapped_vel + = getCartesianVel(rollout->getPosesConst().col(steps-1) ,rollout->getVelsConst().col(steps-1)); + mapped_state.segment(0, (int)(mMappedStateDim/2)) + = mapped_pos; + mapped_state.segment((int)(mMappedStateDim/2), (int)(mMappedStateDim/2)) + = mapped_vel; + Eigen::VectorXs diff = mapped_state - mTarget; + Eigen::MatrixXs final_hess_xx = 2 * dt * (mFinalStateWeight(0) * diff(0) * getXHessian(mapped_pos) + + mFinalStateWeight(1) * diff(1) * getYHessian(mapped_pos) + + mFinalStateWeight(3) * diff(3) * getVxHessian(mapped_pos, mapped_vel) + + mFinalStateWeight(4) * diff(4) * getVyHessian(mapped_pos, mapped_vel) + + jac.transpose() * mFinalStateWeight.asDiagonal() * jac); // TODO: One jac need transpose + hess.push_back(final_hess_xx); +} + +// There is no cross terms between x and u hence the hessian should be all zero +void MappedTargetReachingCost::computeHessXU( + const TrajectoryRollout* rollout, std::vector &hess) +{ + assert(hess.size() == 0); + int steps = rollout->getPosesConst().cols(); + + for(int i = 0; i < steps - 1; i++) + { + hess.push_back(Eigen::MatrixXs::Zero(mActionDim, mStateDim) * dt); + } +} + +// There is no cross terms between u and x hence the hessian should be all zero +void MappedTargetReachingCost::computeHessUX( + const TrajectoryRollout* rollout, std::vector &hess) +{ + assert(hess.size() == 0); + int steps = rollout->getPosesConst().cols(); + for(int i = 0; i < steps - 1; i++) + { + hess.push_back(Eigen::MatrixXs::Zero(mStateDim,mActionDim) * dt); + } +} + +void MappedTargetReachingCost::computeHessUU( + const TrajectoryRollout* rollout, std::vector &hess) +{ + assert(hess.size() == 0); + int steps = rollout->getPosesConst().cols(); + Eigen::MatrixXs hess_uu; + + for(int i = 0; i < steps - 1; i++) + { + hess_uu = 2*mRunningActionWeight.asDiagonal() * dt; + hess.push_back(hess_uu); + } +} + +Eigen::VectorXs MappedTargetReachingCost::getCartesianPos(Eigen::VectorXs q_pos) +{ + Eigen::VectorXs start_pos = mWorld->getPositions(); + mWorld->setPositions(q_pos); + Eigen::VectorXs cart_pos = mMapping.getPositions(mWorld); + mWorld->setPositions(start_pos); + return cart_pos; +} + +Eigen::VectorXs MappedTargetReachingCost::getCartesianVel(Eigen::VectorXs q_pos, Eigen::VectorXs q_vel) +{ + Eigen::VectorXs start_state = mWorld->getState(); + mWorld->setPositions(q_pos); + mWorld->setVelocities(q_vel); + Eigen::VectorXs cart_vel = mMapping.getVelocities(mWorld); + mWorld->setState(start_state); + return cart_vel; +} + +// This function can directly use Nimble's api +Eigen::MatrixXs MappedTargetReachingCost::getStateJacobian(Eigen::VectorXs q_pos, Eigen::VectorXs q_vel) +{ + Eigen::MatrixXs jac = Eigen::MatrixXs::Zero(mMappedStateDim, mStateDim); + int mapped_dofs = (int)(mMappedStateDim/2); + int dofs = (int)(mStateDim/2); + Eigen::VectorXs start_state = mWorld->getState(); + mWorld->setPositions(q_pos); + mWorld->setVelocities(q_vel); + jac.block(0, 0, mapped_dofs, dofs) = mMapping.getRealPosToMappedPosJac(mWorld); + jac.block(mapped_dofs, dofs, mapped_dofs, dofs) = mMapping.getRealVelToMappedVelJac(mWorld); + jac.block(mapped_dofs, 0, mapped_dofs, dofs) = mMapping.getRealPosToMappedVelJac(mWorld); + jac.block(0, dofs, mapped_dofs, dofs) = mMapping.getRealVelToMappedPosJac(mWorld); + mWorld->setState(start_state); + return jac; +} + +Eigen::MatrixXs MappedTargetReachingCost::getXHessian(Eigen::VectorXs q_pos) +{ + Eigen::MatrixXs hess = Eigen::MatrixXs::Zero(mStateDim, mStateDim); + int dofs = (int)(mStateDim/2); + for(int i = 0; i < dofs; i++) + { + for(int j = i; j < dofs; j++) + { + if(i==0 || j==0) + { + hess(i,j) = 0; + } + else + { + s_t cum_q = 0; + for(int cur = 1; cur < j+1;cur++) + { + cum_q += q_pos(cur); + } + for(int k = j; k < dofs;k++) + { + hess(i,j) += sin(cum_q) * mLinkLength(k-1); + hess(j,i) += sin(cum_q) * mLinkLength(k-1); + cum_q += q_pos(k); + } + } + } + } + return hess; +} + +Eigen::MatrixXs MappedTargetReachingCost::getYHessian(Eigen::VectorXs q_pos) +{ + Eigen::MatrixXs hess = Eigen::MatrixXs::Zero(mStateDim, mStateDim); + int dofs = (int)(mStateDim/2); + for(int i = 0; i < dofs; i++) + { + for(int j = i; j < dofs; j++) + { + if(i==0 || j==0) + { + hess(i,j) = 0; + } + else + { + s_t cum_q = 0; + for(int cur = 1; cur < j+1;cur++) + { + cum_q += q_pos(cur); + } + for(int k = j; k < dofs;k++) + { + hess(i,j) -= cos(cum_q) * mLinkLength(k-1); + hess(j,i) -= cos(cum_q) * mLinkLength(k-1); + cum_q += q_pos(k); + } + } + } + } + return hess; +} + +Eigen::MatrixXs MappedTargetReachingCost::getVxHessian(Eigen::VectorXs q_pos, Eigen::VectorXs q_vel) +{ + Eigen::MatrixXs hess = Eigen::MatrixXs::Zero(mStateDim, mStateDim); + int dofs = (int)(mStateDim/2); + for(int i = 0; i < dofs; i++) + { + s_t global_cum_q = 0; + s_t global_cum_dq = 0; + for(int j = i; j < dofs; j++) + { + if(i==0 || j==0) + { + hess(i,j) = 0; + } + else + { + s_t cum_q = global_cum_q; + s_t cum_dq = global_cum_dq; + for(int k = j; k < dofs;k++) + { + cum_q += q_pos(k); + cum_dq += q_vel(k); + hess(i,j) += cos(cum_q) * cum_dq * mLinkLength(k-1); + hess(j,i) += cos(cum_q) * cum_dq * mLinkLength(k-1); + hess(i, j+dofs) += sin(cum_q) * mLinkLength(k-1); + hess(j+dofs, i) += sin(cum_q) * mLinkLength(k-1); + hess(i+dofs, j) += sin(cum_q) * mLinkLength(k-1); + hess(j, i+dofs) += sin(cum_q) * mLinkLength(k-1); + } + global_cum_q += q_pos(j); + global_cum_dq += q_vel(j); + } + } + } + return hess; +} + +Eigen::MatrixXs MappedTargetReachingCost::getVyHessian(Eigen::VectorXs q_pos, Eigen::VectorXs q_vel) +{ + Eigen::MatrixXs hess = Eigen::MatrixXs::Zero(mStateDim, mStateDim); + int dofs = (int)(mStateDim/2); + for(int i = 0; i < dofs; i++) + { + s_t global_cum_q = 0; + s_t global_cum_dq = 0; + for(int j = i; j < dofs; j++) + { + if(i==0 || j==0) + { + hess(i,j) = 0; + } + else + { + s_t cum_q = global_cum_q; + s_t cum_dq = global_cum_dq; + for(int cur = 1; cur < j;cur++) + { + cum_q += q_pos(cur); + cum_dq += q_vel(cur); + } + for(int k = j; k < dofs;k++) + { + cum_q += q_pos(k); + cum_dq += q_vel(k); + hess(i,j) += sin(cum_q) * cum_dq * mLinkLength(k-1); + hess(j,i) += sin(cum_q) * cum_dq * mLinkLength(k-1); + hess(i, j+dofs) -= cos(cum_q) * mLinkLength(k-1); + hess(j+dofs, i) -= cos(cum_q) * mLinkLength(k-1); + hess(i+dofs, j) -= cos(cum_q) * mLinkLength(k-1); + hess(j, i+dofs) -= cos(cum_q) * mLinkLength(k-1); + } + global_cum_q += q_pos(j); + global_cum_dq += q_vel(j); + } + } + } + return hess; +} + +} +} \ No newline at end of file diff --git a/dart/realtime/MappedTargetReachingCost.hpp b/dart/realtime/MappedTargetReachingCost.hpp new file mode 100644 index 000000000..3f6039a82 --- /dev/null +++ b/dart/realtime/MappedTargetReachingCost.hpp @@ -0,0 +1,154 @@ +#ifndef DART_REALTIME_MappedTargetReachingCost +#define DART_REALTIME_MappedTargetReachingCost + +#include +#include +#include +#include +#include "dart/trajectory/TrajectoryConstants.hpp" +#include "dart/trajectory/TrajectoryRollout.hpp" +#include "dart/simulation/World.hpp" +#include "dart/simulation/SmartPointer.hpp" +#include "dart/trajectory/LossFn.hpp" +#include "dart/math/MathTypes.hpp" +#include "dart/neural/IKMapping.hpp" +#include "dart/realtime/TargetReachingCost.hpp" + +namespace dart{ + +namespace trajectory { +class LossFn; +class TrajectoryRollout; +class TrajectoryRolloutReal; +} + +namespace neural{ +class IKMapping; +} + +namespace realtime { + +class MappedTargetReachingCost +{ +public: + // Constructor using weight of different states + MappedTargetReachingCost( + Eigen::VectorXs runningStateWeight, + Eigen::VectorXs runningActionWeight, + Eigen::VectorXs finalStateWeight, + std::shared_ptr world); + + // API for iLQR Gradient and Loss compute + // It should call protected function + std::vector ilqrGradientEstimator(const trajectory::TrajectoryRollout* rollout, + s_t& total_cost, + WRTFLAG wrt); + + // API for iLQR Hessian compute + // It should call protected function + std::vector ilqrHessianEstimator(const trajectory::TrajectoryRollout* rollout, + WRTFLAG wrt); + + // API for IP-OPT LossFn + // It should call protected function + s_t loss(const trajectory::TrajectoryRollout* rollout); + + // API for IP-OPT LossAndGradienFn + // It should call protected function + s_t lossGrad(const trajectory::TrajectoryRollout* rollout, + trajectory::TrajectoryRollout* gradWrtRollout); + + // API to get Loss Function for IP-OPT + std::shared_ptr getLossFn(); + + // set target + void setTarget(Eigen::VectorXs target); + + void setTimeStep(s_t timestep); + + void setSSIDNodeIndex(std::vector ssid_index); + + void setMapping(neural::IKMapping mapping); + + void enableSSIDLoss(s_t weight); + + /// This is dedicated for Whip + void setLinkLength(Eigen::VectorXi lengths); + + s_t computeLoss(const trajectory::TrajectoryRollout* rollout); + + // Compute Loss and Gradient + void computeGradX(const trajectory::TrajectoryRollout* rollout, Eigen::Ref grads); + + void computeGradU(const trajectory::TrajectoryRollout* rollout, Eigen::Ref grads); + + void computeGradForce(const trajectory::TrajectoryRollout* rollout, Eigen::Ref grads); + // Compute Hessian from trajectory + void computeHessXX(const trajectory::TrajectoryRollout* rollout, std::vector &hess); + + void computeHessUU(const trajectory::TrajectoryRollout* rollout, std::vector &hess); + + void computeHessUX(const trajectory::TrajectoryRollout* rollout, std::vector &hess); + + void computeHessXU(const trajectory::TrajectoryRollout* rollout, std::vector &hess); + +protected: + + Eigen::VectorXs getCartesianPos(Eigen::VectorXs q_pos); + + Eigen::VectorXs getCartesianVel(Eigen::VectorXs q_pos, Eigen::VectorXs q_vel); + + Eigen::MatrixXs getStateJacobian(Eigen::VectorXs q_pos, Eigen::VectorXs q_vel); + + Eigen::MatrixXs getXHessian(Eigen::VectorXs q_pos); + + Eigen::MatrixXs getYHessian(Eigen::VectorXs q_pos); + + Eigen::MatrixXs getVxHessian(Eigen::VectorXs q_pos, Eigen::VectorXs q_vel); + + Eigen::MatrixXs getVyHessian(Eigen::VectorXs q_pos, Eigen::VectorXs q_vel); + + // Internal information + Eigen::VectorXs mRunningStateWeight; + + Eigen::VectorXs mRunningActionWeight; + + Eigen::VectorXs mRunningActionWeightFull; + + Eigen::VectorXs mFinalStateWeight; + + Eigen::VectorXi mLinkLength; + + neural::IKMapping mMapping; + + std::shared_ptr mWorld; + + // Target + + Eigen::VectorXs mTarget; + + int mStateDim; + + int mMappedStateDim = 6; + + int mActionDim; + + s_t dt = 1.0; + + // SSID Heuristic + bool mUseSSIDHeuristic = false; + + s_t mSSIDHeuristicWeight; + + std::vector mSSIDNodeIndex; + + std::vector> mAks; + +}; + +} +} + + + +#endif \ No newline at end of file diff --git a/dart/realtime/RealTimeControlBuffer.cpp b/dart/realtime/RealTimeControlBuffer.cpp index 9b091d028..de42921eb 100644 --- a/dart/realtime/RealTimeControlBuffer.cpp +++ b/dart/realtime/RealTimeControlBuffer.cpp @@ -8,15 +8,30 @@ namespace dart { namespace realtime { RealTimeControlBuffer::RealTimeControlBuffer( - int forceDim, int steps, int millisPerStep) + int forceDim, int steps, int millisPerStep, int stateDim) : mForceDim(forceDim), + mStateDim(stateDim), mNumSteps(steps), mMillisPerStep(millisPerStep), mActiveBuffer(UNINITIALIZED), + mActiveBufferLaw(UNINITIALIZED), mBufA(Eigen::MatrixXs::Zero(forceDim, steps)), mBufB(Eigen::MatrixXs::Zero(forceDim, steps)), + mkBufA(Eigen::MatrixXs::Zero(forceDim, steps)), + mkBufB(Eigen::MatrixXs::Zero(forceDim, steps)), + mxBufA(Eigen::MatrixXs::Zero(stateDim, steps)), + mxBufB(Eigen::MatrixXs::Zero(stateDim, steps)), + mAlphaBufA(Eigen::VectorXs::Zero(steps)), + mAlphaBufB(Eigen::VectorXs::Zero(steps)), mControlLog(ControlLog(forceDim, millisPerStep)) { + // std::cout<<"Initializing Buffer..." << std::endl; + for(int i=0; i < steps; i++) + { + mKBufA.push_back(Eigen::MatrixXs::Zero(forceDim, stateDim)); + mKBufB.push_back(Eigen::MatrixXs::Zero(forceDim, stateDim)); + } + // std::cout << "Initialization Complete" < forcesOut) { + assert(forcesOut.rows() == mForceDim); + assert(forcesOut.cols() == mNumSteps); if (mActiveBuffer == UNINITIALIZED) { // Unitialized, default to 0 @@ -110,22 +305,255 @@ void RealTimeControlBuffer::getPlannedForcesStartingAt( } } +void RealTimeControlBuffer::getPlannedkStartingAt( + long start, Eigen::Ref kOut) +{ + assert(kOut.rows() == mForceDim); + assert(kOut.cols() == mNumSteps); + if (mActiveBufferLaw == UNINITIALIZED) + { + // Unitialized, default to 0 + kOut.setZero(); + return; + } + int elapsed = start - mLastWroteLawBufferAt; + if (elapsed < 0) + { + // Asking for some time in the past, default to 0 + kOut.setZero(); + return; + } + int startStep = (int)floor((s_t)elapsed / mMillisPerStep); + if (startStep < mNumSteps) + { + // Copy the appropriate block of our active buffer to the forcesOut block + if (mActiveBufferLaw == BUF_A) + { + kOut.block(0, 0, mForceDim, mNumSteps - startStep) + = mkBufA.block(0, startStep, mForceDim, mNumSteps - startStep); + } + else if (mActiveBufferLaw == BUF_B) + { + kOut.block(0, 0, mForceDim, mNumSteps - startStep) + = mkBufB.block(0, startStep, mForceDim, mNumSteps - startStep); + } + else + assert(false && "Should never reach this point"); + // Zero out the remainder of the forcesOut block + kOut.block(0, mNumSteps - startStep, mForceDim, startStep).setZero(); + } + else + { + // std::cout << "WARNING: MPC isn't keeping up!" << std::endl; + kOut.setZero(); + } +} + +void RealTimeControlBuffer::getPlannedStateStartingAt( + long start, Eigen::Ref stateOut) +{ + assert(stateOut.rows() == mStateDim); + assert(stateOut.cols() == mNumSteps); + if (mActiveBufferLaw == UNINITIALIZED) + { + // Unitialized, default to 0 + stateOut.setZero(); + return; + } + int elapsed = start - mLastWroteLawBufferAt; + if (elapsed < 0) + { + // Asking for some time in the past, default to 0 + stateOut.setZero(); + return; + } + int startStep = (int)floor((s_t)elapsed / mMillisPerStep); + if (startStep < mNumSteps) + { + // Copy the appropriate block of our active buffer to the forcesOut block + if (mActiveBufferLaw == BUF_A) + { + stateOut.block(0, 0, mStateDim, mNumSteps - startStep) + = mxBufA.block(0, startStep, mStateDim, mNumSteps - startStep); + } + else if (mActiveBufferLaw == BUF_B) + { + stateOut.block(0, 0, mStateDim, mNumSteps - startStep) + = mxBufB.block(0, startStep, mStateDim, mNumSteps - startStep); + } + else + assert(false && "Should never reach this point"); + // Zero out the remainder of the forcesOut block + stateOut.block(0, mNumSteps - startStep, mStateDim, startStep).setZero(); + } + else + { + // std::cout << "WARNING: MPC isn't keeping up!" << std::endl; + stateOut.setZero(); + } +} + +void RealTimeControlBuffer::getPlannedAlphaStartingAt(long start, + Eigen::Ref alphaOut) +{ + assert(alphaOut.size() == mNumSteps); + if (mActiveBufferLaw == UNINITIALIZED) + { + // Unitialized, default to 0 + alphaOut.setZero(); + return; + } + int elapsed = start - mLastWroteLawBufferAt; + if (elapsed < 0) + { + // Asking for some time in the past, default to 0 + alphaOut.setZero(); + return; + } + int startStep = (int)floor((s_t)elapsed / mMillisPerStep); + if (startStep < mNumSteps) + { + // Copy the appropriate block of our active buffer to the forcesOut block + if (mActiveBufferLaw == BUF_A) + { + alphaOut.segment(0,mNumSteps - startStep) + = mAlphaBufA.segment(startStep, mNumSteps - startStep); + } + else if (mActiveBufferLaw == BUF_B) + { + alphaOut.segment(0, mNumSteps - startStep) + = mAlphaBufB.segment(startStep, mNumSteps - startStep); + } + else + assert(false && "Should never reach this point"); + // Zero out the remainder of the forcesOut block + alphaOut.segment(mNumSteps - startStep, startStep).setZero(); + } + else + { + // std::cout << "WARNING: MPC isn't keeping up!" << std::endl; + alphaOut.setZero(); + } +} + +void RealTimeControlBuffer::getPlannedKStartingAt(long start, + std::vector &KOut) +{ + // Need to make sure that empty and filed KOut will be treated similarly + assert(KOut.size() == mNumSteps); + assert(KOut[0].rows() == mForceDim); + assert(KOut[0].cols() == mStateDim); + + if (mActiveBufferLaw == UNINITIALIZED) + { + // Unitialized, default to 0 + for(int i=0; i < KOut.size(); i++) + { + KOut[i] = Eigen::MatrixXs::Zero(mForceDim, mStateDim); + } + return; + } + int elapsed = start - mLastWroteLawBufferAt; + if (elapsed < 0) + { + // Asking for some time in the past, default to 0 + for(int i = 0; i < KOut.size(); i++) + { + KOut[i] = Eigen::MatrixXs::Zero(mForceDim, mStateDim); + } + return; + } + int startStep = (int)floor((s_t)elapsed / mMillisPerStep); + if (startStep < mNumSteps) + { + // Copy the appropriate block of our active buffer to the forcesOut block + if (mActiveBufferLaw == BUF_A) + { + /* + kOut.block(0, 0, mForceDim, mNumSteps - startStep) + = mkBufA.block(0, startStep, mForceDim, mNumSteps - startStep); + */ + for(int i = 0; i < mNumSteps- startStep; i++) + { + KOut[i] = mKBufA[i]; + } + } + else if (mActiveBufferLaw == BUF_B) + { + /* + kOut.block(0, 0, mForceDim, mNumSteps - startStep) + = mkBufB.block(0, startStep, mForceDim, mNumSteps - startStep); + */ + for(int i = 0; i < mNumSteps- startStep; i++) + { + KOut[i] = mKBufB[i]; + } + } + else + { + assert(false && "Should never reach this point"); + } + // Zero out the remainder of the forcesOut block + + // KOut.block(0, mNumSteps - startStep, mForceDim, startStep).setZero(); + for(int i = 0; i < mNumSteps- startStep; i++) + { + KOut[i] = Eigen::MatrixXs::Zero(mForceDim, mStateDim); + } + } + else + { + // std::cout << "WARNING: MPC isn't keeping up!" << std::endl; + for(int i = 0; i < KOut.size(); i++) + { + KOut[i] = Eigen::MatrixXs::Zero(mForceDim, mStateDim); + } + } +} + +size_t RealTimeControlBuffer::getRemainSteps(long start) +{ + int elapsed = start - mLastWroteBufferAt; + if(elapsed < 0) + { + return 0; + } + size_t elapsed_steps = (size_t)(((s_t)elapsed / mMillisPerStep)); + size_t remain_steps; + if(mNumSteps > elapsed_steps) + remain_steps = mNumSteps - elapsed_steps; + else + remain_steps = 0; + return remain_steps; +} + /// This swaps in a new buffer of forces. The assumption is that "startAt" is /// before "now", because we'll erase old data in this process. void RealTimeControlBuffer::setControlForcePlan( long startAt, long now, Eigen::MatrixXs forces) { + // The solve is too fast that problem is solved before expected time + // Pad forces in case forces number of colums is less than steps + if(forces.cols() != mNumSteps) + { + Eigen::MatrixXs padded_forces = Eigen::MatrixXs::Zero(mForceDim, mNumSteps); + padded_forces.block(0, 0, mForceDim, forces.cols()) = forces; + forces = padded_forces; + } + if (startAt > now) { long padMillis = startAt - now; int padSteps = (int)floor((s_t)padMillis / mMillisPerStep); // If we're trying to set the force plan too far out in the future, this - // whole exercise is a no-op + // whole exercise is a not allowed if (padSteps >= mNumSteps) { return; } // Otherwise, we're going to copy part of the existing plan + // Since the first time pointer of control force plan should + // always be earlier than actual time that get the buffer int currentStep = (int)floor((s_t)(now - mLastWroteBufferAt) / mMillisPerStep); int remainingSteps = mNumSteps - currentStep; @@ -134,6 +562,7 @@ void RealTimeControlBuffer::setControlForcePlan( // If we've overflowed our old buffer, this is bad, but recoverable. We'll // just not copy anything from our old plan, since it's all in the past now // anyways. + // Previous time the MPC doesn't catch up if (remainingSteps < 0) { mBufA = forces; @@ -141,12 +570,16 @@ void RealTimeControlBuffer::setControlForcePlan( return; } + // With in pad step uses previous force until now, use current force int copySteps = padSteps; int zeroSteps = 0; + // usestep will use new force int useSteps = mNumSteps - padSteps; if (padSteps > remainingSteps) { copySteps = remainingSteps; + // There are some forces in the middle that need to set to zero since no information + // can be provided either from incoming force or original plan zeroSteps = padSteps - remainingSteps; useSteps = mNumSteps - padSteps; } @@ -201,6 +634,254 @@ void RealTimeControlBuffer::setControlForcePlan( } } +// Big Bug fixed hopefully the performance will be better +void RealTimeControlBuffer::setControlLawPlan(long startAt, long now, + std::vector ks, + std::vector Ks, + std::vector states, + std::vector alphas) +{ + // remove the last state + states.pop_back(); + // Pad ks, Ks, states, alphas + int size_err = mNumSteps - ks.size(); + for(int i = 0; i < size_err; i++) + { + ks.push_back(Eigen::VectorXs::Zero(mForceDim)); + Ks.push_back(Eigen::MatrixXs::Zero(mForceDim, mStateDim)); + states.push_back(Eigen::VectorXs::Zero(mStateDim)); + alphas.push_back(0.0); + } + assert(ks.size() == Ks.size() && states.size() == Ks.size() && Ks.size() == alphas.size()); + if (startAt > now) + { + long padMillis = startAt - now; + int padSteps = (int)floor((s_t)padMillis / mMillisPerStep); + // If we're trying to set the force plan too far out in the future, this + // whole exercise is a no-op + if (padSteps >= mNumSteps) + { + return; + } + // Otherwise, we're going to copy part of the existing plan + int currentStep + = (int)floor((s_t)(now - mLastWroteLawBufferAt) / mMillisPerStep); + int remainingSteps = mNumSteps - currentStep; + mLastWroteLawBufferAt = now; + + // If we've overflowed our old buffer, this is bad, but recoverable. We'll + // just not copy anything from our old plan, since it's all in the past now + // anyways. + if (remainingSteps < 0) + { + mkBufA = Eigen::MatrixXs::Zero(mForceDim, ks.size()); + mxBufA = Eigen::MatrixXs::Zero(mStateDim, states.size()); + mAlphaBufA = Eigen::VectorXs::Zero(alphas.size()); + mKBufA = Ks; + for(int i = 0; i < ks.size(); i++) + { + mkBufA.col(i) = ks[i]; + mxBufA.col(i) = states[i]; + mAlphaBufA(i) = alphas[i]; + } + + mActiveBufferLaw = BUF_A; + return; + } + + int copySteps = padSteps; + int zeroSteps = 0; + int useSteps = mNumSteps - padSteps; + if (padSteps > remainingSteps) + { + copySteps = remainingSteps; + zeroSteps = padSteps - remainingSteps; + useSteps = mNumSteps - padSteps; + } + assert(copySteps + zeroSteps + useSteps == mNumSteps); + + if (mActiveBufferLaw == UNINITIALIZED) + { + /* + mBufA.block(0, 0, mForceDim, copySteps).setZero(); + mBufA.block(0, copySteps, mForceDim, zeroSteps).setZero(); + mBufA.block(0, copySteps + zeroSteps, mForceDim, useSteps) + = forces.block(0, 0, mForceDim, useSteps); + mActiveBuffer = BUF_A; + */ + // For k + mkBufA.block(0, 0, mForceDim, copySteps).setZero(); + mkBufA.block(0, copySteps, mForceDim, zeroSteps).setZero(); + for(int i = 0; i < useSteps; i++) + { + mkBufA.col(i + copySteps + zeroSteps) = ks[i]; + } + // For states + mxBufA.block(0, 0, mStateDim, copySteps).setZero(); + mxBufA.block(0, copySteps, mStateDim, zeroSteps).setZero(); + for(int i = 0; i < useSteps; i++) + { + mxBufA.col(i + copySteps + zeroSteps) = states[i]; + } + + // For Alpha + mAlphaBufA.segment(0, copySteps).setZero(); + mAlphaBufA.segment(copySteps,zeroSteps).setZero(); + for(int i = 0; i < useSteps;i++) + { + mAlphaBufA(i + copySteps + zeroSteps) = alphas[i]; + } + // For K + for(int i = 0; i < copySteps + zeroSteps; i++) + { + mKBufA[i] = Eigen::MatrixXs::Zero(mForceDim, mStateDim); + } + for(int i = 0; i < useSteps; i++) + { + mKBufA[i + copySteps + zeroSteps] = Ks[i]; + } + mActiveBufferLaw = BUF_A; + } + else if (mActiveBufferLaw == BUF_B) + { + /* + mBufA.block(0, 0, mForceDim, copySteps) + = mBufB.block(0, mNumSteps - copySteps, mForceDim, copySteps); + mBufA.block(0, copySteps, mForceDim, zeroSteps).setZero(); + mBufA.block(0, copySteps + zeroSteps, mForceDim, useSteps) + = forces.block(0, 0, mForceDim, useSteps); + mActiveBuffer = BUF_A; + */ + // For k + mkBufA.block(0, 0, mForceDim, copySteps) + = mkBufB.block(0, mNumSteps - copySteps, mForceDim, copySteps); + mkBufA.block(0, copySteps, mForceDim, zeroSteps).setZero(); + for(int i = 0; i < useSteps;i++) + { + mkBufA.col(i + copySteps + zeroSteps) = ks[i]; + } + // For state + mxBufA.block(0, 0, mStateDim, copySteps) + = mxBufB.block(0, mNumSteps - copySteps, mStateDim, copySteps); + mxBufA.block(0, copySteps, mStateDim, zeroSteps).setZero(); + for(int i = 0; i < useSteps;i++) + { + mxBufA.col(i + copySteps + zeroSteps) = states[i]; + } + // For Alpha + mAlphaBufA.segment(0, copySteps) = mAlphaBufB.segment(mNumSteps - copySteps, copySteps); + mAlphaBufA.segment(copySteps, zeroSteps).setZero(); + for(int i = 0; i < useSteps; i++) + { + mAlphaBufA(i + copySteps + zeroSteps) = alphas[i]; + } + // For K + for(int i = 0;i < copySteps + zeroSteps; i++) + { + if(i < copySteps) + { + mKBufA[i] = mKBufB[mNumSteps - copySteps + i]; + } + else + { + mKBufA[i] = Eigen::MatrixXs::Zero(mForceDim, mStateDim); + } + } + for(int i = 0; i < useSteps;i++) + { + mKBufA[i + copySteps + zeroSteps] = Ks[i]; + } + // change active buffer + mActiveBufferLaw = BUF_A; + + } + else if (mActiveBufferLaw == BUF_A) + { + /* + mBufB.block(0, 0, mForceDim, copySteps) + = mBufA.block(0, mNumSteps - copySteps, mForceDim, copySteps); + mBufB.block(0, copySteps, mForceDim, zeroSteps).setZero(); + mBufB.block(0, copySteps + zeroSteps, mForceDim, useSteps) + = forces.block(0, 0, mForceDim, useSteps); + mActiveBuffer = BUF_B; + */ + // For k + mkBufB.block(0, 0, mForceDim, copySteps) + = mkBufA.block(0, mNumSteps - copySteps, mForceDim, copySteps); + mkBufB.block(0, copySteps, mForceDim, zeroSteps).setZero(); + for(int i = 0; i < useSteps;i++) + { + mkBufB.col(i + copySteps + zeroSteps) = ks[i]; + } + + // For state + mxBufB.block(0, 0, mStateDim, copySteps) + = mxBufA.block(0, mNumSteps - copySteps, mStateDim, copySteps); + mxBufB.block(0, copySteps, mStateDim, zeroSteps).setZero(); + for(int i = 0; i < useSteps;i++) + { + mxBufB.col(i + copySteps + zeroSteps) = states[i]; + } + // For Alpha + mAlphaBufB.segment(0, copySteps) = mAlphaBufA.segment(mNumSteps - copySteps, copySteps); + mAlphaBufB.segment(copySteps, zeroSteps).setZero(); + for(int i = 0; i < useSteps; i++) + { + mAlphaBufB(i + copySteps + zeroSteps) = alphas[i]; + } + // For K + for(int i = 0;i < copySteps + zeroSteps; i++) + { + if(i < copySteps) + { + mKBufB[i] = mKBufA[mNumSteps - copySteps + i]; + } + else + { + mKBufB[i] = Eigen::MatrixXs::Zero(mForceDim, mStateDim); + } + } + for(int i = 0; i < useSteps;i++) + { + mKBufB[i + copySteps + zeroSteps] = Ks[i]; + } + // change active buffer + mActiveBufferLaw = BUF_B; + } + } + else + { + mLastWroteLawBufferAt = startAt; + if (mActiveBufferLaw == UNINITIALIZED || mActiveBufferLaw == BUF_B) + { + mKBufA = Ks; + for(int i = 0; i < ks.size(); i++) + { + mkBufA.col(i) = ks[i]; + mxBufA.col(i) = states[i]; + mAlphaBufA(i) = alphas[i]; + } + // Crucial for lock-free behavior: copy the buffer BEFORE setting the + // active buffer. Not a huge deal if we're a bit off here in the + // optimizer, but not ideal. + mActiveBufferLaw = BUF_A; + } + else + { + mKBufB = Ks; + for(int i = 0; i < ks.size(); i++) + { + mkBufB.col(i) = ks[i]; + mxBufB.col(i) = states[i]; + mAlphaBufB(i) = alphas[i]; + } + // Crucial for lock-free behavior: copy the buffer BEFORE setting the + // active buffer. Not a huge deal if we're a bit off here in the + // optimizer, but not ideal. + mActiveBufferLaw = BUF_B; + } + } +} /// This retrieves the state of the world at a given time, assuming that we've /// been applying forces from the buffer since the last state that we fully /// observed. @@ -235,16 +916,49 @@ void RealTimeControlBuffer::estimateWorldStateAt( // In the future, project assuming planned forces if (at > mControlLog.last()) { - world->setControlForces(getPlannedForce(at, true)); + if(!mUseiLQR) + { + Eigen::VectorXs action = getPlannedForce(at, true); + if(action.size() == world->getNumDofs()) + world->setControlForces(action); + else + world->setAction(action); + } + else + { + // TODO: Eric This may be quite problematic since it report that we require time from past.. + // Which means the 'at' time is before the last time the control force was written to the buffer + + Eigen::VectorXs action = getPlannedForce(at, true); + Eigen::VectorXs k = getPlannedk(at); + Eigen::VectorXs x = getPlannedState(at); + Eigen::MatrixXs K = getPlannedK(at); + Eigen::VectorXs stateErr = world->getState() - x; + s_t alpha = getPlannedAlpha(at); + if(stateErr.norm() < 0.1) + { + action = (action + alpha * k + + K*(world->getState() - x).cwiseMin(mActionBound)).cwiseMax(-mActionBound); + } + world->setAction(action); + } + } // In the past, project using known forces read from the buffer + // Here no need to differentiate iLQR and Trajectory Opt since they + // all set the executed force here else { - world->setControlForces(mControlLog.get(at)); + Eigen::VectorXs action = mControlLog.get(at); + if(action.size() == world->getNumDofs()) + world->setControlForces(action); + else + world->setAction(action); } world->step(); } } +// TODO: Need to add a version that estimate the world state using control law /// This rescales the timestep size. This is useful because larger timesteps /// mean fewer time steps per real unit of time, and thus we can run our @@ -307,6 +1021,7 @@ void RealTimeControlBuffer::manuallyRecordObservedForce( /// This is a helper to rescale the timestep size of a buffer while leaving /// the data otherwise unchanged. +/// TODO: Eric Enable rescale of K buffers void RealTimeControlBuffer::rescaleBuffer( Eigen::MatrixXs& buf, int oldMillisPerStep, int newMillisPerStep) { @@ -335,5 +1050,25 @@ void RealTimeControlBuffer::rescaleBuffer( buf = newBuf; } +long RealTimeControlBuffer::getLastWriteBufferTime() +{ + return mLastWroteBufferAt; +} + +long RealTimeControlBuffer::getLastWriteBufferLawTime() +{ + return mLastWroteLawBufferAt; +} + +void RealTimeControlBuffer::setiLQRFlag(bool ilqr_flag) +{ + mUseiLQR = ilqr_flag; +} + +void RealTimeControlBuffer::setActionBound(s_t bound) +{ + mActionBound = bound; +} + } // namespace realtime } // namespace dart \ No newline at end of file diff --git a/dart/realtime/RealTimeControlBuffer.hpp b/dart/realtime/RealTimeControlBuffer.hpp index 26d0cc3de..a64c710a9 100644 --- a/dart/realtime/RealTimeControlBuffer.hpp +++ b/dart/realtime/RealTimeControlBuffer.hpp @@ -27,13 +27,21 @@ enum BufferSwitchEnum class RealTimeControlBuffer { public: - RealTimeControlBuffer(int forceDim, int steps, int millisPerStep); + RealTimeControlBuffer(int forceDim, int steps, int millisPerStep, int stateDim=1); /// Gets the force at a given timestep. This HAS SIDE EFFECTS! We actually /// keep track of what forces were read, and assume that they're "immediately" /// applied to the real world after they're read. Eigen::VectorXs getPlannedForce(long time, bool dontLog = false); + Eigen::VectorXs getPlannedk(long time, bool dontLog = false); + + Eigen::MatrixXs getPlannedK(long time, bool dontLog = false); + + Eigen::VectorXs getPlannedState(long time, bool dontLog = false); + + s_t getPlannedAlpha(long time, bool dontLog = false); + /// This gets planned forces starting at `start`, and continuing for the /// length of our buffer size `mSteps`. This is useful for initializing MPC /// runs. It supports walking off the end of known future, and assumes 0 @@ -41,11 +49,31 @@ class RealTimeControlBuffer void getPlannedForcesStartingAt( long start, Eigen::Ref forcesOut); + void getPlannedkStartingAt( + long start, Eigen::Ref kOut); + + void getPlannedKStartingAt( + long start, std::vector &KOut); + + void getPlannedStateStartingAt( + long start, Eigen::Ref stateOut); + + void getPlannedAlphaStartingAt(long start, Eigen::Ref alphaOut); + + size_t getRemainSteps(long start); + /// This swaps in a new buffer of forces. If "startAt" is after "now", this /// will copy enough of the current buffer into our updated buffer to keep the /// current trajectory. void setControlForcePlan(long startAt, long now, Eigen::MatrixXs forces); + void setControlLawPlan(long startAt, + long now, + std::vector ks, + std::vector Ks, + std::vector states, + std::vector alphas); + /// This retrieves the state of the world at a given time, assuming that we've /// been applying forces from the buffer since the last state that we fully /// observed. @@ -70,8 +98,18 @@ class RealTimeControlBuffer /// which comes up in distributed MPC. void manuallyRecordObservedForce(long time, Eigen::VectorXs observation); + /// For debug only + long getLastWriteBufferTime(); + + long getLastWriteBufferLawTime(); + + void setiLQRFlag(bool ilqr_flag); + + void setActionBound(s_t bound); + protected: int mForceDim; + int mStateDim; int mNumSteps; int mMillisPerStep; @@ -83,19 +121,55 @@ class RealTimeControlBuffer /// This controls which of our buffers is currently active BufferSwitchEnum mActiveBuffer; + /// This controls which of our k K Buffers is currently active + BufferSwitchEnum mActiveBufferLaw; + /// This is the A buffer of forces Eigen::MatrixXs mBufA; /// This is the B buffer of forces Eigen::MatrixXs mBufB; + // This is the A buffer of k, which is feed forward gain in iLQR + Eigen::MatrixXs mkBufA; + + // This is the B buffer of k, which is feed forward gain in iLQR + Eigen::MatrixXs mkBufB; + + // This is the A buffer of K, which is feedback gain in iLQR + std::vector mKBufA; + + // This is the B buffer of K, which is feedback gain in iLQR + std::vector mKBufB; + + // This is the A buffer of State + + Eigen::MatrixXs mxBufA; + + // This is the B buffer of state + + Eigen::MatrixXs mxBufB; + + // This is the A buffer of Alpha, which is line search learning rate in iLQR + Eigen::VectorXs mAlphaBufA; + + // This is the B buffer of Alpha, , which is line search learning rate in iLQR + Eigen::VectorXs mAlphaBufB; + /// This is the time when the last buffer was written to long mLastWroteBufferAt; + /// This is the time when the last buffer of control law was written to + long mLastWroteLawBufferAt; + /// This keeps a log of all the control outputs we send, so that we can get /// the current state on request, even if we last had an observation a while /// ago. ControlLog mControlLog; + + bool mUseiLQR = false; + + s_t mActionBound = 1000; }; } // namespace realtime diff --git a/dart/realtime/SSID.cpp b/dart/realtime/SSID.cpp index 7f3d96bd8..c9720841b 100644 --- a/dart/realtime/SSID.cpp +++ b/dart/realtime/SSID.cpp @@ -21,14 +21,30 @@ SSID::SSID( std::shared_ptr loss, int planningHistoryMillis, Eigen::VectorXs sensorDims, - int steps) + int steps, + s_t scale) : mRunning(false), + mVerbose(false), + mParamChanged(true), + mSteadySolutionFound(false), mWorld(world), + // wrt mass should be safe + mWorldSlow(world->clone()), mLoss(loss), mPlanningHistoryMillis(planningHistoryMillis), + mPlanningHistoryMillisSlow(planningHistoryMillis * 20), mSensorDims(sensorDims), mControlLog(VectorLog(world->getNumDofs())), - mPlanningSteps(steps) + mPlanningSteps(steps), + mPlanningStepsSlow(steps * 20), + mScale(scale), + mMassDim(world->getMassDims()), + mDampingDim(world->getDampingDims()), + mSpringDim(world->getSpringDims()), + mParam_Solution(Eigen::VectorXs::Zero(mMassDim+mDampingDim+mSpringDim)), + mValue(Eigen::VectorXs::Zero(mMassDim+mDampingDim+mSpringDim)), + mCumValue(Eigen::VectorXs::Zero(mMassDim+mDampingDim+mSpringDim)), + mTemperature(Eigen::VectorXs::Ones(mMassDim+mDampingDim+mSpringDim)) { for(int i=0;i(); ipoptOptimizer->setCheckDerivatives(false); ipoptOptimizer->setSuppressOutput(true); - ipoptOptimizer->setTolerance(1e-30); - ipoptOptimizer->setIterationLimit(100); + ipoptOptimizer->setTolerance(1e-20); + ipoptOptimizer->setIterationLimit(20); ipoptOptimizer->setRecordFullDebugInfo(false); ipoptOptimizer->setRecordIterations(false); ipoptOptimizer->setLBFGSHistoryLength(5); ipoptOptimizer->setSilenceOutput(true); mOptimizer = ipoptOptimizer; + + std::shared_ptr ipoptOptimizerSlow + = std::make_shared(); + ipoptOptimizerSlow->setCheckDerivatives(false); + ipoptOptimizerSlow->setSuppressOutput(true); + ipoptOptimizerSlow->setTolerance(1e-15); + ipoptOptimizerSlow->setIterationLimit(50); + ipoptOptimizerSlow->setRecordFullDebugInfo(false); + ipoptOptimizerSlow->setRecordIterations(false); + ipoptOptimizerSlow->setLBFGSHistoryLength(5); + ipoptOptimizerSlow->setSilenceOutput(true); + mOptimizerSlow = ipoptOptimizerSlow; } /// This updates the loss function that we're going to move in real time to @@ -73,12 +101,22 @@ void SSID::setOptimizer(std::shared_ptr optimizer) mOptimizer = optimizer; } +void SSID::setSlowOptimizer(std::shared_ptr optimizer) +{ + mOptimizerSlow = optimizer; +} + /// This returns the current optimizer that MPC is using std::shared_ptr SSID::getOptimizer() { return mOptimizer; } +std::shared_ptr SSID::getSlowOptimizer() +{ + return mOptimizerSlow; +} + /// This sets the problem that MPC will use. This will override the default /// problem. This should be called before start(). void SSID::setProblem(std::shared_ptr problem) @@ -86,6 +124,11 @@ void SSID::setProblem(std::shared_ptr problem) mProblem = problem; } +void SSID::setSlowProblem(std::shared_ptr problem) +{ + mProblemSlow = problem; +} + /// This registers a function that can be used to estimate the initial state /// for the inference system from recent sensor history and the timestamp void SSID::setInitialPosEstimator( @@ -137,6 +180,7 @@ void SSID::start() if (mRunning) return; mRunning = true; + std::cout << "Start Fast Thread!" << std::endl; mOptimizationThread = std::thread(&SSID::optimizationThreadLoop, this); } @@ -152,10 +196,8 @@ void SSID::stop() /// This runs inference to find mutable values, starting at `startTime` void SSID::runInference(long startTime) { - // registerLock(); long startComputeWallTime = timeSinceEpochMillis(); - - int millisPerStep = static_cast(ceil(mWorld->getTimeStep() * 1000.0)); + int millisPerStep = static_cast(ceil(mScale*mWorld->getTimeStep() * 1000.0)); int steps = static_cast( ceil(static_cast(mPlanningHistoryMillis) / millisPerStep)); @@ -163,42 +205,26 @@ void SSID::runInference(long startTime) { std::shared_ptr singleshot = std::make_shared(mWorld, *mLoss.get(), steps, false); - //multishot->setParallelOperationsEnabled(true); mProblem = singleshot; } - //std::cout<<"Problem Created"<pinForce(i, forceHistory.col(i)); } - //std::cout<<"ForcePinned"<setMetadata("forces", forceHistory); mProblem->setMetadata("sensors", poseHistory); mProblem->setMetadata("velocities",velHistory); mProblem->setStartPos(mInitialPosEstimator(poseHistory, startTime)); - // TODO: Set initial velocity mProblem->setStartVel(mInitialVelEstimator(velHistory, startTime)); // Then actually run the optimization - //std::cout<<"Ready to Optimize"<optimize(mProblem.get()); - //std::cout<<"Optimization End"<getPosesConst().col(steps - 1); Eigen::VectorXs vel = cache->getVelsConst().col(steps - 1); - Eigen::VectorXs mass = mWorld->getMasses(); - + // Here the masses should be the concatenation of all registered mass nodes + paramMutexLock(); + Eigen::VectorXs param = mParam_Solution; + paramMutexUnlock(); + mValue = getTrajConditionNumbers(poseHistory, velHistory); // Should allow different value for each dofs should be identified independently + for (auto listener : mInferListeners) { - listener(startTime, pos, vel, mass, computeDurationWallTime); + listener(startTime, pos, vel, param, computeDurationWallTime, mSteadySolutionFound); } } -Eigen::VectorXs SSID::runPlotting(long startTime, s_t upper, s_t lower,int samples) +// Run plotting function should be idenpotent +// Here it only support 1 body node plotting, but that make sense +std::pair SSID::runPlotting(long startTime, s_t upper, s_t lower,int samples) { - int millisPerStep = static_cast(ceil(mWorld->getTimeStep() * 1000.0)); + int millisPerStep = static_cast(ceil(mScale*mWorld->getTimeStep() * 1000.0)); int steps = static_cast( ceil(static_cast(mPlanningHistoryMillis) / millisPerStep)); @@ -269,14 +301,17 @@ Eigen::VectorXs SSID::runPlotting(long startTime, s_t upper, s_t lower,int sampl s_t loss = mProblem->getLoss(mWorld); losses(0) = loss; } - - return losses; + + std::pair result; + result.first = losses; + result.second = poseHistory; + return result; } Eigen::MatrixXs SSID::runPlotting2D(long startTime, Eigen::Vector3s upper, Eigen::Vector3s lower, int x_samples,int y_samples, size_t rest_dim) { - int millisPerStep = static_cast(ceil(mWorld->getTimeStep() * 1000.0)); + int millisPerStep = static_cast(ceil(mScale * mWorld->getTimeStep() * 1000.0)); int steps = static_cast( ceil(static_cast(mPlanningHistoryMillis) / millisPerStep)); @@ -361,12 +396,23 @@ void SSID::saveCSVMatrix(std::string filename, Eigen::MatrixXs matrix) /// This registers a listener to get called when we finish replanning void SSID::registerInferListener( std::function< - void(long, Eigen::VectorXs, Eigen::VectorXs, Eigen::VectorXs, long)> + void(long, Eigen::VectorXs, Eigen::VectorXs, Eigen::VectorXs, long, bool)> inferListener) { mInferListeners.push_back(inferListener); } +void SSID::updateFastThreadBuffer(Eigen::VectorXs new_solution, Eigen::VectorXs new_value) +{ + mPrev_solutions.push_back(new_solution); + mPrev_values.push_back(new_value); + if(mPrev_solutions.size() > mPrev_Length) + { + mPrev_solutions.erase(mPrev_solutions.begin()); + mPrev_values.erase(mPrev_values.begin()); + } +} + /// This is the function for the optimization thread to run when we're live void SSID::optimizationThreadLoop() { @@ -377,25 +423,359 @@ void SSID::optimizationThreadLoop() sigaddset(&sigset, SIGINT); sigaddset(&sigset, SIGTERM); pthread_sigmask(SIG_BLOCK, &sigset, nullptr); - /* - while (mRunning) + while(mRunning) { long startTime = timeSinceEpochMillis(); - if (mControlLog.availableHistoryBefore(startTime) > mPlanningHistoryMillis) + if(mControlLog.availableStepsBefore(startTime)>mPlanningSteps+1) { + + paramMutexLock(); + if(mInitialize) + { + // NOTE: Parameter solution by design support different types of parameters + mParam_Solution.segment(0, mMassDim) = mWorld->getMasses(); + mParam_Solution.segment(mMassDim, mDampingDim) = mWorld->getDampings(); + mParam_Solution.segment(mMassDim+mDampingDim, mSpringDim) = mWorld->getSprings(); + } + mWorld->setMasses(mParam_Solution.segment(0, mMassDim)); + mWorld->setDampings(mParam_Solution.segment(mMassDim, mDampingDim)); + mWorld->setSprings(mParam_Solution.segment(mMassDim+mDampingDim, mSpringDim)); + + paramMutexUnlock(); runInference(startTime); - std::cout<<"Gap Time:"<getMasses(); + new_solution.segment(mMassDim, mDampingDim) = mWorld->getDampings(); + new_solution.segment(mMassDim + mDampingDim, mSpringDim) = mWorld->getSprings(); + + if(!mUseSmoothing) + { + mParam_Solution = new_solution; + //std::cout << "New Solution: " << new_solution(0) << new_solution(1) << std::endl; + continue; + } + // We have a stable solution, then detect changes + if(mSteadySolutionFound && mUseConfidence) + { + if(detectEWiseChangeParams().second) + { + + std::cout << "+++++++++++++++" << std::endl; + std::cout << "Change Detected" << std::endl; + std::cout << "+++++++++++++++" << std::endl; + + + mParamChanged = true; + mSteadySolutionFound = false; + paramMutexLock(); + // Flush all the solutions in the buffer + if(mUseConfidence) + { + mControlLog.discardBefore(startTime); + for(int i = 0; i < mSensorLogs.size(); i++) + { + mSensorLogs[i].discardBefore(startTime); + } + mPrev_solutions.clear(); + mPrev_values.clear(); + mCumValue = Eigen::VectorXs::Zero(mMassDim+mDampingDim+mSpringDim); + } + mInitialize = true; + paramMutexUnlock(); + } + } + updateFastThreadBuffer(new_solution, mValue); + // We don't have a stable solution, we hope to find one + if(mParamChanged==true || mSteadySolutionFound==false) + { + // Estimate the result by weighted average + paramMutexLock(); + if(mVerbose) + { + std::cout << "Param Changed Use Fast Result!" + << mPrev_solutions[mPrev_solutions.size()-1](0) <<" " + << mPrev_solutions[mPrev_solutions.size()-1](1) <<" " + << mPrev_solutions[mPrev_solutions.size()-1](2) << std::endl; + } + if(!mInitialize) + { + // Sliding weighted average + Eigen::VectorXs conf = computeConfidenceFromValue(mValue); + if(mVerbose || true) // TODO: remove the override + { + std::cout << "Confidence: \n" << conf << std::endl; + } + Eigen::VectorXs prev_solution = mParam_Solution; + if(mUseConfidence) + { + if(conf.mean()>mConfidence_thresh) // Confidence score is not a sensitive hyper-param, we can use one for all parameters + { + mParam_Solution = (mValue.cwiseProduct((mCumValue+mValue).cwiseInverse())).cwiseProduct(new_solution) + + (mCumValue.cwiseProduct((mCumValue+mValue).cwiseInverse())).cwiseProduct(mParam_Solution); + mCumValue += mValue; + } + // TODO : Need to use different mParam_change_thresh + // If a parameter is detected to change, the other parameter is in its low confidence area. Then we should change the parameter that detected to change + // with high confidence score while maintain the same other parameters + // if((mParam_Solution-prev_solution).cwiseAbs().maxCoeff() < mParam_change_thresh && conf.mean() > mConfidence_thresh) + if(detectEWiseStable(mParam_Solution, prev_solution, conf).second) + { + mSteadySolutionFound = true; + mParamChanged = false; + if(mVerbose) + { + std::cout << "=====================" << std::endl; + std::cout << "Steady Solution Found" << std::endl; + std::cout << "=====================" << std::endl; + } + + } + } + + if(!mUseConfidence) + { + mParam_Solution = estimateSolution(); + } + // How to determine a stable solution has been found + // May be if we reject the low confidence result we don't need steady solution, in that case we need to define confidence + // of a particular solution, need to rescale the confidence + } + else + { + mParam_Solution = new_solution; + } + paramMutexUnlock(); + } + if(mInitialize) + { + mInitialize = false; + } } } - */ - while(mRunning) - { - long startTime = timeSinceEpochMillis(); - if(mControlLog.availableStepsBefore(startTime)>mPlanningSteps+1) - { - runInference(startTime); - } - } +} + +void SSID::setVerbose(bool verbose) +{ + mVerbose = verbose; +} + +/// The Change of Parameter should be handled independently for each degree of freedom +/// For simplicity it is a global function right now +bool SSID::detectChangeParams() +{ + Eigen::VectorXs mean_params = estimateSolution(); + Eigen::VectorXs confidence = estimateConfidence(); + if(mVerbose) + { + std::cout << "Confidence: " << confidence.mean() << std::endl; + } + paramMutexLock(); + if((mean_params - mParam_Solution).cwiseAbs().maxCoeff() > mParam_change_thresh && + (confidence.mean() > mConfidence_thresh|| !mUseConfidence)) + { + paramMutexUnlock(); + return true; + } + paramMutexUnlock(); + return false; +} + +std::pair, bool> SSID::detectEWiseChangeParams() +{ + Eigen::VectorXs mean_params = estimateSolution(); + Eigen::VectorXs confidence = estimateConfidence(); + if(mVerbose) + { + std::cout << "Confidences:\n" << confidence << std::endl; + } + paramMutexLock(); + Eigen::VectorXs params_diff = (mean_params - mParam_Solution).cwiseAbs(); + std::vector params_change_flag; + bool param_changed = false; + for(int i=0;i < params_diff.size();i++) + { + // NOTE: Since confience score is relatively not sensitive, we use one threshold for all + if(params_diff(i) > mEwise_params_change_thresh(i) + && (confidence(i) > mConfidence_thresh || !mUseConfidence)) + { + params_change_flag.push_back(true); + param_changed = true; + } + else + { + params_change_flag.push_back(false); + } + } + paramMutexUnlock(); + return std::pair, bool>(params_change_flag, param_changed); +} + +std::pair, bool> SSID::detectEWiseStable( + Eigen::VectorXs current, + Eigen::VectorXs reference, + Eigen::VectorXs confidence) +{ + Eigen::VectorXs diff = (current - reference).cwiseAbs(); + std::vector stable_flags; + bool stable_flag = true; + for(int i=0;i mEwise_params_change_thresh(i) || confidence(i) <= mConfidence_thresh) + { + stable_flags.push_back(false); + stable_flag = false; + } + else + { + stable_flags.push_back(true); + } + } + return std::pair, bool>(stable_flags, stable_flag); +} + +/// The condition number is with respect to a particular node +s_t SSID::getTrajConditionNumberOfMassIndex(Eigen::MatrixXs poses, Eigen::MatrixXs vels, size_t index) +{ + size_t steps = poses.cols(); + s_t cond = 0; + s_t dt = mWorld->getTimeStep(); + Eigen::VectorXs init_pose = mWorld->getPositions(); + Eigen::VectorXs init_vel = mWorld->getVelocities(); + for(int i = 1; i < steps; i++) + { + mWorld->setPositions(poses.col(i)); + mWorld->setVelocities(vels.col(i)); + Eigen::VectorXs acc = (vels.col(i) - vels.col(i-1)) / dt; + //Eigen::VectorXs acc = Eigen::VectorXs::Ones(vels.rows()) * dt / dt; + Eigen::MatrixXs Ak = mWorld->getSkeleton(mRobotSkelIndex)->getLinkAkMatrixIndex(index); + Eigen::MatrixXs Jvk = mWorld->getSkeleton(mRobotSkelIndex)->getLinkJvkMatrixIndex(index); + cond += (Ak * acc + Jvk.transpose() * mWorld->getGravity()).norm(); + } + mWorld->setPositions(init_pose); + mWorld->setVelocities(init_vel); + return cond/steps; +} + +Eigen::Vector3s SSID::getTrajConditionNumberOfCOMIndex(Eigen::MatrixXs poses, Eigen::MatrixXs vels, size_t index) +{ + size_t steps = poses.cols(); + Eigen::Vector3s cond = Eigen::Vector3s::Zero(); + s_t dt = mWorld->getTimeStep(); + Eigen::VectorXs init_state = mWorld->getState(); + mWorld->setPositions(poses.col(0)); + mWorld->setVelocities(vels.col(0)); + Eigen::MatrixXs hat_Jw = mWorld->getSkeleton(mRobotSkelIndex)->getLinkLocalJwkMatrixIndex(index); + Eigen::Matrix3s R = mWorld->getSkeleton(mRobotSkelIndex)->getLinkRMatrixIndex(index); + for(int i = 1; i < steps; i++) + { + mWorld->setPositions(poses.col(i)); + mWorld->setVelocities(vels.col(i)); + Eigen::VectorXs acc = (vels.col(i) - vels.col(i-1)) / dt; + Eigen::MatrixXs new_R = mWorld->getSkeleton(mRobotSkelIndex)->getLinkRMatrixIndex(index); + Eigen::MatrixXs new_hat_Jw = mWorld->getSkeleton(mRobotSkelIndex)->getLinkLocalJwkMatrixIndex(index); + Eigen::MatrixXs dR = (new_R - R) / dt; + Eigen::MatrixXs d_hat_Jw = (new_hat_Jw - hat_Jw) / dt; + R = new_R; + hat_Jw = new_hat_Jw; + // This should be: 3 x 3 matrix + Eigen::MatrixXs S = R * vector2skew(hat_Jw * acc) + + dR * vector2skew(hat_Jw * vels.col(i)) + + R * vector2skew(d_hat_Jw * vels.col(i)); + + // This should be: N x 3 matrix + Eigen::MatrixXs G = hat_Jw.transpose() * vector2skew(R.transpose() * mWorld->getGravity()); + //Eigen::VectorXs G = + + cond(0) += S.col(0).norm() + G.col(0).norm(); + cond(1) += S.col(1).norm() + G.col(1).norm(); + cond(2) += S.col(2).norm() + G.col(2).norm(); + } + mWorld->setState(init_state); // Idempotent + return cond / steps; +} + +// Should implement condition number of trajectory +Eigen::Vector3s SSID::getTrajConditionNumberOfMOIIndex(Eigen::MatrixXs poses, Eigen::MatrixXs vels, size_t index) +{ + // Whether is is well conditioned is determined by the average of three diagonal terms + // Compute Diagonal Matrix: + size_t steps = poses.cols(); + Eigen::Vector3s cond = Eigen::Vector3s::Zero(); + s_t dt = mWorld->getTimeStep(); + Eigen::VectorXs init_state = mWorld->getState(); + for(int i = 1; i < steps; i++) + { + mWorld->setPositions(poses.col(i)); + mWorld->setVelocities(vels.col(i)); + Eigen::VectorXs acc = (vels.col(i) - vels.col(i-1)) / dt; + Eigen::MatrixXs Jw = mWorld->getSkeleton(mRobotSkelIndex)->getLinkJwkMatrixIndex(index); + Eigen::MatrixXs Jv = mWorld->getSkeleton(mRobotSkelIndex)->getLinkJvkMatrixIndex(index); + Eigen::MatrixXs diag = (Jw * acc).asDiagonal(); + Eigen::MatrixXs Ck = Jw.transpose() * diag; + cond(0) += Ck.col(0).norm(); + cond(1) += Ck.col(1).norm(); + cond(2) += Ck.col(2).norm(); + } + mWorld->setState(init_state); // Idempotent + return cond/steps; +} + +/// Here index represent joint index +s_t SSID::getTrajConditionNumberOfDampingIndex(Eigen::MatrixXs vels, size_t index) +{ + size_t steps = vels.cols(); + s_t cond = 0; + for(int i = 0; i < steps; i++) + { + cond += abs(vels(index, i)); + } + return cond/steps; +} + +s_t SSID::getTrajConditionNumberOfSpringIndex(Eigen::MatrixXs poses, size_t index) +{ + size_t steps = poses.cols(); + s_t cond = 0; + for(int i = 0; i < steps; i++) + { + cond += abs(poses(index, i) - mWorld->getRestPositionIndex(index)); + } + return cond/steps; +} + +// NOTE: This function can adapt to elementwise different type of parameters, with fixed order +Eigen::VectorXs SSID::getTrajConditionNumbers(Eigen::MatrixXs poses, Eigen::MatrixXs vels) +{ + Eigen::VectorXs conds = Eigen::VectorXs::Zero(mMassDim + mDampingDim + mSpringDim); + int cur = 0; + for(int i = 0; i < mSSIDMassNodeIndices.size(); i++) + { + conds(cur) = getTrajConditionNumberOfMassIndex(poses, vels, mSSIDMassNodeIndices(i)); + cur += 1; + } + for(int i = 0; i < mSSIDCOMNodeIndices.size(); i++) + { + conds.segment(cur, 3) = getTrajConditionNumberOfCOMIndex(poses, vels, mSSIDCOMNodeIndices(i)); + cur += 3; + } + for(int i = 0; i < mSSIDMOINodeIndices.size(); i++) + { + conds.segment(cur, 3) = getTrajConditionNumberOfMOIIndex(poses, vels, mSSIDMOINodeIndices(i)); + cur += 3; + } + for(int i = 0; i < mSSIDDampingJointIndices.size(); i++) + { + conds(cur) = getTrajConditionNumberOfDampingIndex(vels, mSSIDDampingJointIndices(i)); + cur += 1; + } + for(int i = 0; i < mSSIDSpringJointIndices.size(); i++) + { + conds(cur) = getTrajConditionNumberOfSpringIndex(poses, mSSIDSpringJointIndices(i)); + cur += 1; + } + return conds; } void SSID::attachMutex(std::mutex &mutex_lock) @@ -404,6 +784,68 @@ void SSID::attachMutex(std::mutex &mutex_lock) mLockRegistered = true; } +void SSID::attachParamMutex(std::mutex &mutex_lock) +{ + mParamMutex = &mutex_lock; + mParamLockRegistered = true; +} + + +// This will use previous solutions to detect the change +// NOTE: Since mPrev_values is evaluated element-wisely it should be fine +Eigen::VectorXs SSID::estimateSolution() +{ + Eigen::VectorXs solution = Eigen::VectorXs::Zero(mMassDim + mDampingDim + mSpringDim); + Eigen::VectorXs totalValue = Eigen::VectorXs::Zero(mMassDim + mDampingDim + mSpringDim); + Eigen::VectorXs uniformValue = Eigen::VectorXs::Ones(mMassDim + mDampingDim + mSpringDim); + //Eigen::VectorXs totalConfidence = estimateConfidence(); + + for(int i = 0; i < mPrev_values.size(); i++) + { + if(mUseHeuristicWeight) + { + solution += mPrev_solutions[i].cwiseProduct(mPrev_values[i]); + totalValue += mPrev_values[i]; + } + else + { + solution += mPrev_solutions[i].cwiseProduct(uniformValue); + totalValue += uniformValue; + } + } + solution = solution.cwiseProduct(totalValue.cwiseInverse()); + // if(totalConfidence.minCoeff() > mConfidence_thresh && mUseConfidence && findStable) + // { + // mSteadySolutionFound = true; + // mParamChanged = false; + // std::cout << "Steady Solution Found!!" << std::endl; + // } + return solution; +} + +// Here confidence should be not related to trajectory length since it is a relative value +// Should be group-wise +Eigen::VectorXs SSID::estimateConfidence() +{ + Eigen::VectorXs totalValue = Eigen::VectorXs::Zero(mMassDim + mDampingDim + mSpringDim); + for(int i = 0; i < mPrev_values.size(); i++) + { + totalValue += mPrev_values[i]; + } + return computeConfidenceFromValue(totalValue/mPrev_values.size()); +} + +// NOTE: This is already element-wise, need to customize on temperature side +Eigen::VectorXs SSID::computeConfidenceFromValue(Eigen::VectorXs value) +{ + Eigen::VectorXs confidence = Eigen::VectorXs::Zero(mMassDim+mDampingDim+mSpringDim); + for(int i = 0; i < mMassDim+mDampingDim+mSpringDim; i++) + { + confidence(i) = tanh(value(i)/mTemperature(i)); + } + return confidence; +} + void SSID::registerLock() { if(mLockRegistered) @@ -420,5 +862,98 @@ void SSID::registerUnlock() } } +void SSID::paramMutexLock() +{ + if(mParamLockRegistered) + mParamMutex->lock(); +} + +void SSID::paramMutexUnlock() +{ + if(mParamLockRegistered) + mParamMutex->unlock(); +} + +void SSID::setBufferLength(int length) +{ + mPrev_Length = length; +} + +void SSID::setSSIDMassIndex(Eigen::VectorXi indices) +{ + mSSIDMassNodeIndices = indices; +} + + +void SSID::setSSIDCOMIndex(Eigen::VectorXi indices) +{ + mSSIDCOMNodeIndices = indices; +} + +void SSID::setSSIDMOIIndex(Eigen::VectorXi indices) +{ + mSSIDMOINodeIndices = indices; +} + +void SSID::setSSIDDampIndex(Eigen::VectorXi indices) +{ + mSSIDDampingJointIndices = indices; +} + +void SSID::setSSIDSpringIndex(Eigen::VectorXi indices) +{ + mSSIDSpringJointIndices = indices; +} + +void SSID::setTemperature(Eigen::VectorXs temp) +{ + assert(temp.size() == mMassDim+mDampingDim+mSpringDim); + mTemperature = temp; +} + +Eigen::VectorXs SSID::getTemperature() +{ + return mTemperature; +} + +void SSID::setThreshs(s_t param_change, s_t conf) +{ + mParam_change_thresh = param_change; + mConfidence_thresh = conf; +} + +void SSID::setEWiseThreshs(Eigen::VectorXs param_changes, s_t conf) +{ + mEwise_params_change_thresh = param_changes; + mConfidence_thresh = conf; +} + +void SSID::useConfidence() +{ + mUseConfidence = true; +} + +void SSID::useHeuristicWeight() +{ + mUseHeuristicWeight = true; +} + +void SSID::useSmoothing() +{ + mUseSmoothing = true; +} + +Eigen::Matrix3s SSID::vector2skew(Eigen::Vector3s vec) +{ + Eigen::Matrix3s skew = Eigen::Matrix3s::Zero(); + skew(2,1) = vec(0); + skew(1,2) = -vec(0); + skew(2, 0) = -vec(1); + skew(0, 2) = vec(1); + skew(1, 0) = vec(2); + skew(0, 1) = -vec(2); + return skew; +} + } // namespace realtime } // namespace dart \ No newline at end of file diff --git a/dart/realtime/SSID.hpp b/dart/realtime/SSID.hpp index 84e81cb6f..b9500352c 100644 --- a/dart/realtime/SSID.hpp +++ b/dart/realtime/SSID.hpp @@ -35,7 +35,8 @@ class SSID std::shared_ptr loss, int planningHistoryMillis, Eigen::VectorXs sensorDims, - int steps); + int steps, + s_t scale); /// This updates the loss function that we're going to move in real time to /// minimize. This can happen quite frequently, for example if our loss @@ -47,13 +48,22 @@ class SSID /// optimizer. This should be called before start(). void setOptimizer(std::shared_ptr optimizer); + /// This sets slow optimizer + void setSlowOptimizer(std::shared_ptr optimizer); + /// This returns the current optimizer that MPC is using std::shared_ptr getOptimizer(); + /// This returns the slow optimizer + std::shared_ptr getSlowOptimizer(); + /// This sets the problem that MPC will use. This will override the default /// problem. This should be called before start(). void setProblem(std::shared_ptr problem); + /// This set slow problem + void setSlowProblem(std::shared_ptr problem); + /// This registers a function that can be used to estimate the initial state /// for the inference system from recent sensor history and the timestamp void setInitialPosEstimator( @@ -67,6 +77,9 @@ class SSID /// This returns the current problem definition that MPC is using std::shared_ptr getProblem(); + /// This returns slow problem + std::shared_ptr getSlowProblem(); + /// This logs that the sensor output is a specific vector now void registerSensorsNow(Eigen::VectorXs sensors, int sensor_id); @@ -80,16 +93,54 @@ class SSID /// This logs that our controls were this value at this time void registerControls(long now, Eigen::VectorXs controls); + /// This determine the condition number of trajectory to a node + s_t getTrajConditionNumberOfMassIndex(Eigen::MatrixXs poses, Eigen::MatrixXs vels, size_t index); + + /// This determine the condition number of center of mass of a body node + Eigen::Vector3s getTrajConditionNumberOfCOMIndex(Eigen::MatrixXs poses, Eigen::MatrixXs vels, size_t index); + + /// This determine the condition number vector of the diagonal term in the inertia matrix + Eigen::Vector3s getTrajConditionNumberOfMOIIndex(Eigen::MatrixXs poses, Eigen::MatrixXs vels, size_t index); + + s_t getTrajConditionNumberOfDampingIndex(Eigen::MatrixXs vels, size_t index); + + s_t getTrajConditionNumberOfSpringIndex(Eigen::MatrixXs poses, size_t index); + + /// This determine the condition number of trajectory to all id node + Eigen::VectorXs getTrajConditionNumbers(Eigen::MatrixXs poses, Eigen::MatrixXs vels); + + /// This set the index of param + void setSSIDMassIndex(Eigen::VectorXi indices); + + void setSSIDCOMIndex(Eigen::VectorXi indices); + + void setSSIDMOIIndex(Eigen::VectorXi indices); + + void setSSIDDampIndex(Eigen::VectorXi indices); + + void setSSIDSpringIndex(Eigen::VectorXi indices); + + Eigen::Matrix3s vector2skew(Eigen::Vector3s vec); + /// This starts our main thread and begins running optimizations void start(); + /// This starts the secondary thread of slow ssid which uses long trajectory + void startSlow(); + /// This stops our main thread, waits for it to finish, and then returns void stop(); + /// This stops the secondary thread of slow ssid which uses long trajectory + void stopSlow(); + /// This runs inference to find mutable values, starting at `startTime` void runInference(long startTime); - Eigen::VectorXs runPlotting(long startTime, s_t upper, s_t lower, int samples); + /// This runs slow inference + void runSlowInference(long startTime); + + std::pair runPlotting(long startTime, s_t upper, s_t lower, int samples); Eigen::MatrixXs runPlotting2D(long startTime, Eigen::Vector3s upper, Eigen::Vector3s lower, int x_samples, int y_samples, size_t rest_dim); @@ -98,23 +149,112 @@ class SSID /// This registers a listener to get called when we finish replanning void registerInferListener( std::function< - void(long, Eigen::VectorXs, Eigen::VectorXs, Eigen::VectorXs, long)> + void(long, Eigen::VectorXs, Eigen::VectorXs, Eigen::VectorXs, long, bool)> inferListener); // wrapper for locking the buffer void attachMutex(std::mutex &mutex_lock); + void attachParamMutex(std::mutex &mutex_lock); + void registerLock(); void registerUnlock(); + void paramMutexLock(); + + void paramMutexUnlock(); + + void setBufferLength(int length); + + void setTemperature(Eigen::VectorXs temp); + + Eigen::VectorXs getTemperature(); + + // This is a big question + // We assume that the current solution is common among two thread + // We need to compare mean of recent fast thread against slow thread + // Also, for fast thread, the initial guess can be provided by slow thread + bool detectChangeParams(); + + std::pair, bool> detectEWiseChangeParams(); + + std::pair, bool> detectEWiseStable(Eigen::VectorXs current, Eigen::VectorXs reference, Eigen::VectorXs confidence); + + void updateFastThreadBuffer(Eigen::VectorXs new_solution, Eigen::VectorXs new_weight); + + Eigen::VectorXs estimateSolution(); + + Eigen::VectorXs estimateConfidence(); + + Eigen::VectorXs computeConfidenceFromValue(Eigen::VectorXs value); + + void setThreshs(s_t param_change, s_t conf); + + void setEWiseThreshs(Eigen::VectorXs param_changes, s_t conf); + + void useConfidence(); + + void useHeuristicWeight(); + + void useSmoothing(); + + void setVerbose(bool verbose); + protected: /// This is the function for the optimization thread to run when we're live void optimizationThreadLoop(); bool mRunning; + bool mVerbose; + std::shared_ptr mWorld; + // The World for Slow SSID may be there are some thread safety problems + std::shared_ptr mWorldSlow; std::shared_ptr mLoss; int mPlanningHistoryMillis; int mPlanningSteps; + + // For Slow thread + int mPlanningHistoryMillisSlow; + int mPlanningStepsSlow; + + s_t mScale; + // For dimension of different system parameters + size_t mMassDim; + size_t mDampingDim; + size_t mSpringDim; + + // For fast thread + int mPrev_Length = 3; + std::vector mPrev_solutions; + std::vector mPrev_values; + Eigen::VectorXs mParam_Solution; + Eigen::VectorXs mParam_Steady; + s_t mParam_change_thresh = 0.05; + s_t mConfidence_thresh = 0.5; + + // For elementwise + Eigen::VectorXs mEwise_params_change_thresh; + + Eigen::VectorXs mTemperature; + size_t mRobotSkelIndex = 0; + Eigen::VectorXi mSSIDMassNodeIndices; + Eigen::VectorXi mSSIDCOMNodeIndices; + Eigen::VectorXi mSSIDMOINodeIndices; + Eigen::VectorXi mSSIDDampingJointIndices; + Eigen::VectorXi mSSIDSpringJointIndices; + Eigen::VectorXs mValue; + Eigen::VectorXs mCumValue; + + // Some Flags for control indication + bool mSteadySolutionFound; + bool mParamChanged; + bool mInitialize = true; + bool mSlowInit = true; + bool mUseConfidence = false; + bool mUseHeuristicWeight = false; + bool mUseSmoothing = false; + + // Vector Logs for control Eigen::VectorXs mSensorDims; std::vector mSensorLogs; VectorLog mControlLog; @@ -123,15 +263,27 @@ class SSID std::shared_ptr mProblem; std::shared_ptr mSolution; std::thread mOptimizationThread; + + std::shared_ptr mOptimizerSlow; + std::shared_ptr mProblemSlow; + std::shared_ptr mSolutionSlow; + std::thread mOptimizationThreadSlow; std::mutex* mRegisterMutex; + std::mutex* mParamMutex; bool mLockRegistered = false; - Eigen::VectorXs mParameters; + bool mParamLockRegistered = false; + Eigen::VectorXs mParameters; // TODO : Should involve both mass and damping // These are listeners that get called when we finish replanning std::vector > + long, Eigen::VectorXs, Eigen::VectorXs, Eigen::VectorXs, long, bool)>> mInferListeners; + // These are listeners that get called when we finish the slow ssid thread + std::vector> + mInferListenersSlow; + // This is the function that estimates our initial state before launching // learning std::function mInitialPosEstimator; diff --git a/dart/realtime/TargetReachingCost.cpp b/dart/realtime/TargetReachingCost.cpp new file mode 100644 index 000000000..4b88266dd --- /dev/null +++ b/dart/realtime/TargetReachingCost.cpp @@ -0,0 +1,408 @@ +#include +#include "dart/math/MathTypes.hpp" +#include "dart/realtime/TargetReachingCost.hpp" + +namespace dart { +using namespace trajectory; + +namespace realtime { + +TargetReachingCost::TargetReachingCost( + Eigen::VectorXs runningStateWeight, + Eigen::VectorXs runningActionWeight, + Eigen::VectorXs finalStateWeight, + std::shared_ptr world): + mRunningStateWeight(runningStateWeight), + mRunningActionWeight(runningActionWeight), + mFinalStateWeight(finalStateWeight), + mStateDim(runningStateWeight.size()), + mActionDim(runningActionWeight.size()), + mWorld(world) +{ + std::cout << "State and Action Dim in Cost Fn: " << mStateDim << " " << mActionDim << std::endl; + mRunningActionWeight = runningActionWeight; +} + +std::vector TargetReachingCost::ilqrGradientEstimator( + const TrajectoryRollout* rollout, + s_t& total_cost, + WRTFLAG wrt) +{ + total_cost = computeLoss(rollout); + int steps = rollout->getPosesConst().cols(); + Eigen::MatrixXs grad; + if(wrt == WRTFLAG::X) + { + grad = Eigen::MatrixXs::Zero(mStateDim, steps); + computeGradX(rollout, grad); + } + else if(wrt == WRTFLAG::U) + { + grad = Eigen::MatrixXs::Zero(mActionDim, steps-1); + computeGradU(rollout, grad); + } + else + assert(false && "Shouldn't reach here"); + std::vector output; + + for(int i = 0; i < grad.cols(); i++) + { + Eigen::VectorXs col = grad.col(i); + output.push_back(col); + } + return output; +} + +std::vector TargetReachingCost::ilqrHessianEstimator( + const TrajectoryRollout* rollout, + WRTFLAG wrt) +{ + std::vector hess; + switch (wrt) + { + case WRTFLAG::U: + case WRTFLAG::X: + assert(false && "Should not use grad flag"); + break; + case WRTFLAG::XX: + computeHessXX(rollout, hess); + break; + case WRTFLAG::UU: + computeHessUU(rollout, hess); + break; + case WRTFLAG::UX: + computeHessUX(rollout, hess); + break; + case WRTFLAG::XU: + computeHessXU(rollout, hess); + break; + } + return hess; +} + +s_t TargetReachingCost::loss(const TrajectoryRollout* rollout) +{ + return computeLoss(rollout); +} + +s_t TargetReachingCost::lossGrad(const TrajectoryRollout* rollout, TrajectoryRollout* gradWrtRollout) +{ + int steps = rollout->getPosesConst().cols(); + Eigen::MatrixXs grad_x = Eigen::MatrixXs::Zero(mStateDim, steps); + Eigen::MatrixXs grad_u = Eigen::MatrixXs::Zero((int)(mStateDim/2), steps); + computeGradX(rollout, grad_x); + computeGradForce(rollout, grad_u.block(0, 0, (int)(mStateDim/2), steps-1)); + gradWrtRollout->getPoses().setZero(); + gradWrtRollout->getVels().setZero(); + gradWrtRollout->getControlForces().setZero(); + gradWrtRollout->getPoses().block(0, 0, (int)(mStateDim/2), steps) + = grad_x.block(0, 0, (int)(mStateDim/2), steps); + gradWrtRollout->getVels().block(0, 0, (int)(mStateDim/2), steps) + = grad_x.block((int)(mStateDim/2), 0, (int)(mStateDim/2), steps); + gradWrtRollout->getControlForces().block(0, 0, (int)(mStateDim/2), steps) + = grad_u; + return computeLoss(rollout); +} + +std::shared_ptr TargetReachingCost::getLossFn() +{ + boost::function + loss_(boost::bind(&TargetReachingCost::loss, this, _1)); + boost::function + lossGrad_(boost::bind(&TargetReachingCost::lossGrad, this, _1, _2)); + return std::make_shared(loss_, lossGrad_); +} + +void TargetReachingCost::setTarget(Eigen::VectorXs target) +{ + assert(target.size() == mStateDim); + mTarget = target; +} + +void TargetReachingCost::setTimeStep(s_t timestep) +{ + assert(timestep > 0.001); + dt = timestep; +} + +/// Must be set +void TargetReachingCost::setSSIDMassNodeIndex(Eigen::VectorXi mass_indices) +{ + mMass_indices = mass_indices; + for(int i = 0; i < mass_indices.size(); i++) + { + mAks.push_back(std::vector()); + } +} + +void TargetReachingCost::setSSIDSpringJointIndex(Eigen::VectorXi spring_indices) +{ + mSpring_indices = spring_indices; +} + +void TargetReachingCost::enableSSIDLoss(s_t weight) +{ + mUseSSIDHeuristic = true; + mSSIDHeuristicWeight = weight; +} + +void TargetReachingCost::setSSIDHeuristicWeight(s_t weight) +{ + mSSIDHeuristicWeight = weight; +} + +// For the Velocity we can use loss or not use +s_t TargetReachingCost::computeLoss(const TrajectoryRollout* rollout) +{ + s_t loss = 0; + int steps = rollout->getPosesConst().cols(); + // Compute Running State and Loss from target reaching + Eigen::VectorXs init_state = mWorld->getState(); + for(int i = 0; i < mAks.size(); i++) + { + mAks[i].clear(); + } + for(int i = 0; i < steps - 1; i++) + { + Eigen::VectorXs state = Eigen::VectorXs::Zero(mStateDim); + Eigen::VectorXs action = mWorld->mapToActionSpaceVector(rollout->getControlForcesConst().col(i)); + + state.segment(0, (int)(mStateDim/2)) = rollout->getPosesConst().col(i); + state.segment((int)(mStateDim/2), (int)(mStateDim/2)) = rollout->getVelsConst().col(i); + + loss += (mRunningStateWeight.asDiagonal() * ((state - mTarget).cwiseAbs2())).sum() * dt; + loss += (mRunningActionWeight.asDiagonal() * (action.cwiseAbs2())).sum() * dt; + if(mUseSSIDHeuristic) + { + // if(i>=1) + // { + // Eigen::VectorXs prev_state = Eigen::VectorXs::Zero(mStateDim); + // prev_state.segment(0, (int)(mStateDim/2)) = rollout->getPosesConst().col(i-1); + // prev_state.segment((int)(mStateDim/2), (int)(mStateDim/2)) = rollout->getVelsConst().col(i-1); + // loss += computeSSIDMassLoss(state, prev_state); + // } + // TODO : Add other heuristics + loss = (1-mSSIDHeuristicWeight) * loss + mSSIDHeuristicWeight * computeSSIDSpringStiffLoss(state); + // std::cout<< "Loss: " << computeSSIDSpringStiffLoss(state) << std::endl; + } + + } + mWorld->setState(init_state); + // std::cout<< "None Final Loss: " << loss << std::endl; + // Add Final State Error + Eigen::VectorXs finalState = Eigen::VectorXs::Zero(mStateDim); + finalState.segment(0, (int)(mStateDim/2)) = rollout->getPosesConst().col(steps - 1); + finalState.segment((int)(mStateDim/2), (int)(mStateDim/2)) = rollout->getVelsConst().col(steps - 1); + s_t final_loss = (mFinalStateWeight.asDiagonal() * ((finalState - mTarget).cwiseAbs2())).sum(); + loss += (1 - mSSIDHeuristicWeight) * final_loss; + return loss; +} + +void TargetReachingCost::computeGradX(const TrajectoryRollout* rollout, Eigen::Ref grads) +{ + int steps = rollout->getPosesConst().cols(); + assert(grads.cols() == steps && grads.rows() == mStateDim); + // Compute Grad for running state + for(int i = 0; i < steps - 1; i++) + { + Eigen::VectorXs state = Eigen::VectorXs::Zero(mStateDim); + //std::cout<< "State Dim: " << mStateDim << "Half Dim: " << (int)(mStateDim/2) << std::endl; + state.segment(0, (int)(mStateDim/2)) = rollout->getPosesConst().col(i); + state.segment((int)(mStateDim/2), (int)(mStateDim/2)) = rollout->getVelsConst().col(i); + + grads.col(i) = 2*mRunningStateWeight.asDiagonal() * (state - mTarget) * dt; + if(mUseSSIDHeuristic) + { + // if(i>= 1) + // { + // Eigen::VectorXs prev_state = Eigen::VectorXs::Zero(mStateDim); + // prev_state.segment(0, (int)(mStateDim/2)) = rollout->getPosesConst().col(i-1); + // prev_state.segment((int)(mStateDim/2), (int)(mStateDim/2)) = rollout->getVelsConst().col(i-1); + // grads.col(i) += computeSSIDMassGrad(state, prev_state, i); + // } + grads.col(i) =(1-mSSIDHeuristicWeight)*grads.col(i) + mSSIDHeuristicWeight * computeSSIDSpringStiffGrad(state); + // std::cout<< "Grad: " << computeSSIDSpringStiffGrad(state) << std::endl; + + } + } + // Compute Grad for final state + Eigen::VectorXs state = Eigen::VectorXs::Zero(mStateDim); + state.segment(0, (int)(mStateDim/2)) = rollout->getPosesConst().col(steps - 1); + state.segment((int)(mStateDim/2), (int)(mStateDim/2)) = rollout->getVelsConst().col(steps - 1); + grads.col(steps - 1) = (1 - mSSIDHeuristicWeight) * 2 * mFinalStateWeight.asDiagonal() * (state - mTarget); +} + +void TargetReachingCost::computeGradU(const TrajectoryRollout* rollout, + Eigen::Ref grads) +{ + int steps = rollout->getPosesConst().cols(); + assert(grads.cols() == steps - 1 && grads.rows() == mActionDim); + + // Compute Grad for Running action + for(int i = 0; i < steps - 1; i++) + { + Eigen::VectorXs action; + action = mWorld->mapToActionSpaceVector(rollout->getControlForcesConst().col(i)); + grads.col(i) = 2 * mRunningActionWeight.asDiagonal() * action * dt; + + } +} + +// Here compute gradients of actions which will not have final steps +void TargetReachingCost::computeGradForce(const TrajectoryRollout* rollout, + Eigen::Ref grads) +{ + int steps = rollout->getPosesConst().cols(); + assert(grads.cols() == steps - 1 && grads.rows() == (int)(mStateDim/2)); + + // Compute Grad for Running action + for(int i = 0; i < steps - 1; i++) + { + Eigen::VectorXs force; + force = rollout->getControlForcesConst().col(i); + grads.col(i) = 2 * mWorld->mapToForceSpaceVector(mRunningActionWeight).asDiagonal() * force * dt; + + } +} + +// Compute Hessian of XX which should have length of steps +// Assume hess is an empty vector +void TargetReachingCost::computeHessXX( + const TrajectoryRollout* rollout, std::vector &hess) +{ + assert(hess.size() == 0); + int steps = rollout->getPosesConst().cols(); + Eigen::MatrixXs hess_xx; + for(int i = 0; i < steps - 1; i++) + { + hess_xx = 2*mRunningStateWeight.asDiagonal() * dt; + if(mUseSSIDHeuristic) + { + // if(i>=1) + // { + // hess_xx += computeSSIDMassHess(i); + // } + hess_xx = hess_xx*(1-mSSIDHeuristicWeight) + mSSIDHeuristicWeight * computeSSIDSpringStiffHess(); + // std::cout<< "Hess: " << computeSSIDSpringStiffHess() << std::endl; + } + hess.push_back(hess_xx); + } + Eigen::MatrixXs final_hess_xx =(1-mSSIDHeuristicWeight) * 2*mFinalStateWeight.asDiagonal(); + hess.push_back(final_hess_xx); +} + +// There is no cross terms between x and u hence the hessian should be all zero +void TargetReachingCost::computeHessXU( + const TrajectoryRollout* rollout, std::vector &hess) +{ + assert(hess.size() == 0); + int steps = rollout->getPosesConst().cols(); + + for(int i = 0; i < steps - 1; i++) + { + hess.push_back(Eigen::MatrixXs::Zero(mActionDim, mStateDim) * dt); + } +} + +// There is no cross terms between u and x hence the hessian should be all zero +void TargetReachingCost::computeHessUX( + const TrajectoryRollout* rollout, std::vector &hess) +{ + assert(hess.size() == 0); + int steps = rollout->getPosesConst().cols(); + for(int i = 0; i < steps - 1; i++) + { + hess.push_back(Eigen::MatrixXs::Zero(mStateDim,mActionDim) * dt); + } +} + +void TargetReachingCost::computeHessUU( + const TrajectoryRollout* rollout, std::vector &hess) +{ + assert(hess.size() == 0); + int steps = rollout->getPosesConst().cols(); + Eigen::MatrixXs hess_uu; + + for(int i = 0; i < steps - 1; i++) + { + hess_uu = 2*mRunningActionWeight.asDiagonal() * dt; + hess.push_back(hess_uu); + } +} + +s_t TargetReachingCost::computeSSIDMassLoss(Eigen::VectorXs state, Eigen::VectorXs prev_state) +{ + s_t loss = 0; + int dofs = (int)(state.size() / 2); + mWorld->setState(state); + for(int j = 0; j < mMass_indices.size(); j++) + { + Eigen::MatrixXs Ak = mWorld->getLinkAkMatrixIndex(mMass_indices(j)); + mAks[j].push_back(Ak); + loss += (Ak * (state.segment(dofs, dofs) - prev_state.segment(dofs, dofs))).cwiseAbs2().sum() * dt; + } + + return loss; +} + +// TODO: Eric After deadline implement such function on all parameters +// s_t computeSSIDDampCoeffLoss() + +s_t TargetReachingCost::computeSSIDSpringStiffLoss(Eigen::VectorXs state) +{ + s_t loss = 0; + Eigen::VectorXs err = Eigen::VectorXs::Zero(mSpring_indices.size()); + for(int j = 0; j < mSpring_indices.size(); j++) + { + s_t rest_pose = mWorld->getRestPositionIndex(mSpring_indices(j)); + err(j) = state(mSpring_indices(j)) - rest_pose; + } + loss -= err.norm(); + return loss; +} + +Eigen::VectorXs TargetReachingCost::computeSSIDMassGrad(Eigen::VectorXs state, Eigen::VectorXs prev_state, int cur) +{ + Eigen::VectorXs grad = Eigen::VectorXs::Zero(state.size()); + int dofs = (int)(state.size() / 2); + for(int j = 0; j < mMass_indices.size(); j++) + { + Eigen::VectorXs new_grad = 2 * (mAks[j][cur-1].transpose() * mAks[j][cur-1]) * (state.segment(dofs, dofs) - prev_state.segment(dofs, dofs)) * dt; + grad += new_grad; + } + return grad; +} + +Eigen::VectorXs TargetReachingCost::computeSSIDSpringStiffGrad(Eigen::VectorXs state) +{ + Eigen::VectorXs grad = Eigen::VectorXs::Zero(state.size()); + for(int j = 0; j < mSpring_indices.size(); j++) + { + grad(mSpring_indices(j)) = 2 * (state(mSpring_indices(j)) - mWorld->getRestPositionIndex(mSpring_indices(j))); + } + return -grad; +} + +Eigen::MatrixXs TargetReachingCost::computeSSIDMassHess(int cur) +{ + Eigen::MatrixXs hess = Eigen::MatrixXs::Zero(mStateDim, mStateDim); + for(int j = 0; j < mMass_indices.size(); j++) + { + hess.block((int)(mStateDim/2), (int)(mStateDim/2), (int)(mStateDim/2), (int)(mStateDim/2)) + += mAks[j][cur-1].transpose() * mAks[j][cur-1] * dt; + } + return hess; +} + +Eigen::MatrixXs TargetReachingCost::computeSSIDSpringStiffHess() +{ + Eigen::MatrixXs hess = Eigen::MatrixXs::Zero(mStateDim, mStateDim); + for(int j = 0; j < mSpring_indices.size(); j++) + { + hess(mSpring_indices(j), mSpring_indices(j)) = 1; + } + return -hess; +} + +} +} \ No newline at end of file diff --git a/dart/realtime/TargetReachingCost.hpp b/dart/realtime/TargetReachingCost.hpp new file mode 100644 index 000000000..3d0a784da --- /dev/null +++ b/dart/realtime/TargetReachingCost.hpp @@ -0,0 +1,175 @@ +#ifndef DART_REALTIME_TargetReachingCost +#define DART_REALTIME_TargetReachingCost + +#include +#include +#include +#include +#include "dart/trajectory/TrajectoryConstants.hpp" +#include "dart/trajectory/TrajectoryRollout.hpp" +#include "dart/simulation/World.hpp" +#include "dart/simulation/SmartPointer.hpp" +#include "dart/trajectory/LossFn.hpp" +#include "dart/math/MathTypes.hpp" + +namespace dart{ + +namespace trajectory { +class LossFn; +class TrajectoryRollout; +class TrajectoryRolloutReal; +} + +namespace realtime { + +enum WRTFLAG { + X, + U, + XX, + UX, + UU, + XU +}; + +class TargetReachingCost +{ +public: + // Constructor using weight of different states + TargetReachingCost( + Eigen::VectorXs runningStateWeight, + Eigen::VectorXs runningActionWeight, + Eigen::VectorXs finalStateWeight, + std::shared_ptr world); + + // API for iLQR Gradient and Loss compute + // It should call protected function + std::vector ilqrGradientEstimator(const trajectory::TrajectoryRollout* rollout, + s_t& total_cost, + WRTFLAG wrt); + + // API for iLQR Hessian compute + // It should call protected function + std::vector ilqrHessianEstimator(const trajectory::TrajectoryRollout* rollout, + WRTFLAG wrt); + + // API for IP-OPT LossFn + // It should call protected function + s_t loss(const trajectory::TrajectoryRollout* rollout); + + // API for IP-OPT LossAndGradienFn + // It should call protected function + s_t lossGrad(const trajectory::TrajectoryRollout* rollout, + trajectory::TrajectoryRollout* gradWrtRollout); + + // API to get Loss Function for IP-OPT + std::shared_ptr getLossFn(); + + // set target + void setTarget(Eigen::VectorXs target); + + void setTimeStep(s_t timestep); + + // TODO: Change the name + void setSSIDMassNodeIndex(Eigen::VectorXi indices); + + // TODO: Implement it + void setSSIDCOMNodeIndex(Eigen::VectorXi indices); + + // void setSSIDMOINodeIndex(Eigen::VectorXi indices); + + void setSSIDDampJointIndex(Eigen::VectorXi indices); + + void setSSIDSpringJointIndex(Eigen::VectorXi indices); + + void enableSSIDLoss(s_t weight); + + void setSSIDHeuristicWeight(s_t weight); + + s_t computeLoss(const trajectory::TrajectoryRollout* rollout); + + // Compute Loss and Gradient + void computeGradX(const trajectory::TrajectoryRollout* rollout, Eigen::Ref grads); + + void computeGradU(const trajectory::TrajectoryRollout* rollout, Eigen::Ref grads); + + void computeGradForce(const trajectory::TrajectoryRollout* rollout, Eigen::Ref grads); + // Compute Hessian from trajectory + void computeHessXX(const trajectory::TrajectoryRollout* rollout, std::vector &hess); + + void computeHessUU(const trajectory::TrajectoryRollout* rollout, std::vector &hess); + + void computeHessUX(const trajectory::TrajectoryRollout* rollout, std::vector &hess); + + void computeHessXU(const trajectory::TrajectoryRollout* rollout, std::vector &hess); + +protected: + + // Related to Heuristic Gradient and weight computation + s_t computeSSIDMassLoss(Eigen::VectorXs state, Eigen::VectorXs prev_state); + + s_t computeSSIDCOMLoss(Eigen::VectorXs state, Eigen::VectorXs prev_state); + + s_t computeSSIDDampCoeffLoss(Eigen::VectorXs state); + + s_t computeSSIDSpringStiffLoss(Eigen::VectorXs state); + + Eigen::VectorXs computeSSIDMassGrad(Eigen::VectorXs state, Eigen::VectorXs prev_state, int cur); + + Eigen::VectorXs computeSSIDCOMGrad(Eigen::VectorXs state, Eigen::VectorXs prev_state); + + Eigen::VectorXs computeSSIDDampCoeffGrad(Eigen::VectorXs state); + + Eigen::VectorXs computeSSIDSpringStiffGrad(Eigen::VectorXs state); + + Eigen::MatrixXs computeSSIDMassHess(int cur); + + Eigen::MatrixXs computeSSIDCOMHess(Eigen::VectorXs state, Eigen::VectorXs prev_state); + + Eigen::MatrixXs computeSSIDDampCoeffHess(Eigen::VectorXs state); + + Eigen::MatrixXs computeSSIDSpringStiffHess(); + + // Internal information + Eigen::VectorXs mRunningStateWeight; + + Eigen::VectorXs mRunningActionWeight; + + Eigen::VectorXs mRunningActionWeightFull; + + Eigen::VectorXs mFinalStateWeight; + + std::shared_ptr mWorld; + + // Target + + Eigen::VectorXs mTarget; + + int mStateDim; + + int mActionDim; + + s_t dt = 1.0; + + // SSID Heuristic + bool mUseSSIDHeuristic = false; + + s_t mSSIDHeuristicWeight = 0; + + Eigen::VectorXi mMass_indices; + + Eigen::VectorXi mCOM_indices; + + Eigen::VectorXi mDamp_indices; + + Eigen::VectorXi mSpring_indices; + + std::vector> mAks; + +}; + +} +} + + + +#endif \ No newline at end of file diff --git a/dart/realtime/VectorLog.cpp b/dart/realtime/VectorLog.cpp index 8a6101682..9ada72774 100644 --- a/dart/realtime/VectorLog.cpp +++ b/dart/realtime/VectorLog.cpp @@ -121,6 +121,7 @@ void VectorLog::discardBefore(long time) newObservations.push_back(mObservations[i]); } mObservations = newObservations; + mStartTime = time; } } // namespace realtime diff --git a/dart/realtime/iLQRLocal.cpp b/dart/realtime/iLQRLocal.cpp new file mode 100644 index 000000000..602c19a0e --- /dev/null +++ b/dart/realtime/iLQRLocal.cpp @@ -0,0 +1,1366 @@ +#include "dart/realtime/iLQRLocal.hpp" + +#include +#include +#include +#include + +#include "dart/performance/PerformanceLog.hpp" +#include "dart/proto/SerializeEigen.hpp" +#include "dart/realtime/Millis.hpp" +#include "dart/realtime/RealTimeControlBuffer.hpp" +#include "dart/simulation/World.hpp" +#include "dart/trajectory/IPOptOptimizer.hpp" +#include "dart/trajectory/LossFn.hpp" +#include "dart/trajectory/MultiShot.hpp" +#include "dart/trajectory/Solution.hpp" +#include "dart/neural/NeuralUtils.hpp" +#include "dart/neural/BackpropSnapshot.hpp" + +#include "signal.h" + +namespace dart { + +using namespace trajectory; + +namespace realtime { + +// For initialization the steps size need to be created to the maximum possible steps +LQRBuffer::LQRBuffer( + int steps, + size_t nDofs, + size_t nControls, + Extrapolate_Method extrapolate) +{ + std::cout << "LQRBuffer Initializing ..." < existK; + for(int i = 0; i < mMaxSteps; i++) + { + existK.push_back(Eigen::MatrixXs::Zero(control_dim, state_dim)); + } + buffer.getPlannedForcesStartingAt(timestamp, existForce); + buffer.getPlannedkStartingAt(timestamp, existk); + buffer.getPlannedKStartingAt(timestamp, existK); + buffer.getPlannedStateStartingAt(timestamp, existx); + size_t existSteps = buffer.getRemainSteps(timestamp); + size_t i = 0; + for(;i < existSteps; i++) + { + U[i] = existForce.col(i); + k[i] = existk.col(i); + K[i] = existK[i]; + X[i] = existx.col(i); + } + size_t last = i; + switch(ext) + { + case ZERO: + for(; i < nsteps; i++) + { + U[i] = Eigen::VectorXs::Zero(control_dim); + k[i] = Eigen::VectorXs::Zero(control_dim); + K[i] = Eigen::MatrixXs::Zero(control_dim, state_dim); + X[i] = Eigen::VectorXs::Zero(state_dim); + } + X[nsteps] = Eigen::VectorXs::Zero(state_dim); + break; + case LAST: + if(last==0) + { + for(; i < nsteps; i++) + { + U[i] = Eigen::VectorXs::Zero(control_dim); + k[i] = Eigen::VectorXs::Zero(control_dim); + K[i] = Eigen::MatrixXs::Zero(control_dim, state_dim); + X[i] = Eigen::VectorXs::Zero(state_dim); + } + X[nsteps] = Eigen::VectorXs::Zero(state_dim); + break; + } + else + { + for(; i < nsteps; i++) + { + U[i].segment(0, control_dim) = U[last]; + k[i].segment(0, control_dim) = Eigen::VectorXs::Zero(control_dim); + K[i].block(0, 0, control_dim, state_dim) = Eigen::MatrixXs::Zero(control_dim, state_dim); + X[i].segment(0, state_dim) = X[last]; + } + X[nsteps].segment(0, state_dim) = X[last]; + break; + } + case RANDOM: + for(; i < nsteps; i++) + { + U[i] = 0.1 * Eigen::VectorXs::Random(control_dim); + k[i] = Eigen::VectorXs::Zero(control_dim); + K[i] = Eigen::MatrixXs::Zero(control_dim, state_dim); + // Random State may break constraints + X[i] = Eigen::VectorXs::Zero(state_dim); + } + X[nsteps] = Eigen::VectorXs::Zero(state_dim); + break; + default: + assert(false && "Should not reach here"); + } +} + +void LQRBuffer::updateL(std::vector Lx_new, std::vector Lu_new, + std::vector Lxx_new, std::vector Luu_new, + std::vector Lux_new) +{ + Lx = Lx_new; + Lu = Lu_new; + Lxx = Lxx_new; + Luu = Luu_new; + Lux = Lux_new; +} + +void LQRBuffer::setNewActionPlan(long timestamp, RealTimeControlBuffer* buffer) +{ + // Need to compute how many step in U has been invalid + long current_time = timeSinceEpochMillis(); + Eigen::MatrixXs control_force = Eigen::MatrixXs::Zero(control_dim, nsteps); + for(int i = 0; i < nsteps; i++) + { + control_force.col(i) = U[i]; + } + // This assume the vector indicate plan right after timestamp + buffer->setControlForcePlan( + timestamp, + current_time, + control_force); +} + +void LQRBuffer::setNewControlLaw(long timestamp, RealTimeControlBuffer* buffer) +{ + // Need to compute how many step in U has been invalid + long current_time = timeSinceEpochMillis(); + + // This assume the vector indicate plan right after timestamp + // Till this point, we can guarantee that X is the best trajectory rollout from forward + buffer->setControlLawPlan( + timestamp, + current_time, + k, + K, + X, + alpha); +} + +void LQRBuffer::updateF(std::vector Fx_new, std::vector Fu_new) +{ + Fx = Fx_new; + Fu = Fu_new; +} + +bool LQRBuffer::validateXnew() +{ + for(size_t i=0; i < Xnew.size(); i++) + { + if(Xnew[i].hasNaN()) + return false; + } + return true; +} + +/// Non-continuous X is caused by inaccurate ssid. +bool LQRBuffer::detectXContinuity() +{ + s_t err = 0; + bool flag = true; + for(size_t i = 0; i < X.size()-1; i++) + { + err = (X[i+1] - X[i]).lpNorm(); + if(err > 1.0) + { + if(mVerbose) + { + std::cout << "State: \n" << X[i+1] << "\n" << X[i] << "\n" << i << std::endl; + } + flag = false; + } + } + return flag; +} + +void LQRBuffer::setVerbose(bool verbose) +{ + mVerbose = verbose; +} + +bool LQRBuffer::getVerbose() +{ + return mVerbose; +} + +void LQRBuffer::setNumSteps(size_t steps) +{ + nsteps = steps; +} + +iLQRLocal::iLQRLocal( + std::shared_ptr world, + size_t nControls, + int planningHorizonMillis, + s_t scale) + : mRunning(false), + mWorld(world), + mObservationLog( + timeSinceEpochMillis(), + world->getPositions(), + world->getVelocities(), + world->getMasses()), + mEnableLinesearch(true), + mEnableOptimizationGuards(false), + mRecordIterations(false), + mPlanningHorizonMillis(planningHorizonMillis), + mPlanningCandidateHorizonMillis(planningHorizonMillis), + mMillisPerStep(scale*1000 * world->getTimeStep()), + mSteps((int)ceil((s_t)planningHorizonMillis / mMillisPerStep)), + mMaxSteps(mSteps), + mShotLength(50), + mMaxIterations(5), + mMillisInAdvanceToPlan(0), + mLastOptimizedTime(0L), + mBuffer(RealTimeControlBuffer(nControls, mSteps, mMillisPerStep, world->getNumDofs() * 2)), + mSilent(false), + // Below are iLQR related information + mAlpha_reset_value(0.2), + mAlpha(0.2), + mAlpha_opt(1.0), + mPatience_reset_value(8), + mPatience(8), + mDelta0(2.0), + mDelta(2.0), + mMU_MIN(1e-6), + mMU(100.*1e-6), + mMU_reset_value(100.*1e-6), + mCost(std::numeric_limits::max()), + mlqrBuffer(LQRBuffer(mSteps, world->getNumDofs(), nControls, Extrapolate_Method::ZERO)), + mLast_U(Eigen::VectorXs::Zero(nControls)), + mActionDim(nControls), + mStateDim(world->getNumDofs() * 2), + mRollout(createRollout(mSteps, world->getNumDofs(), world->getMassDims(), world->getDampingDims(), world->getSpringDims())) +{ +} + +/// set planning steps +void iLQRLocal::setPlanningHorizon(size_t planningHorizonMillis) +{ + mSteps = (int)ceil((s_t)planningHorizonMillis / mMillisPerStep); + mPlanningHorizonMillis = planningHorizonMillis; + mRollout = createRollout(mSteps, mWorld->getNumDofs(), mWorld->getMassDims(), mWorld->getDampingDims(), mWorld->getSpringDims()); + mlqrBuffer.setNumSteps(mSteps); +} + +void iLQRLocal::setCandidateHorizon(size_t candidateHorizonMillis) +{ + mPlanningCandidateHorizonMillis = candidateHorizonMillis; +} + +/// Set Cost function +void iLQRLocal::setCostFn(std::shared_ptr costFn) +{ + mCostFn = costFn; + mLoss = mCostFn->getLossFn(); +} + +void iLQRLocal::setMappedCostFn(std::shared_ptr costFn) +{ + mMappedCostFn = costFn; + mLoss = mMappedCostFn->getLossFn(); +} + +/// This sets the optimizer that iLQRLocal will use. This will override the +/// default optimizer. This should be called before start(). +void iLQRLocal::setOptimizer(std::shared_ptr optimizer) +{ + mOptimizer = optimizer; +} + +/// This returns the current optimizer that iLQRLocal is using +std::shared_ptr iLQRLocal::getOptimizer() +{ + return mOptimizer; +} + +/// This sets the problem that iLQRLocal will use. This will override the default +/// problem. This should be called before start(). +void iLQRLocal::setProblem(std::shared_ptr problem) +{ + mProblem = problem; +} + +/// This returns the current problem definition that iLQRLocal is using +std::shared_ptr iLQRLocal::getProblem() +{ + return mProblem; +} + +// Need manually log +Eigen::VectorXs iLQRLocal::getControlAction(long now) +{ + return mBuffer.getPlannedForce(now, true); +} +/// This gets the force to apply to the world at this instant. If we haven't +/// computed anything for this instant yet, this just returns 0s. +Eigen::VectorXs iLQRLocal::getControlForce(long now) +{ + return mWorld->mapToForceSpaceVector(mBuffer.getPlannedForce(now)); +} + +s_t iLQRLocal::getControlAlpha(long now) +{ + return mBuffer.getPlannedAlpha(now); +} + +/// This gets the force from LQR control law and current state +Eigen::VectorXs iLQRLocal::computeForce(Eigen::VectorXs state, long now) +{ + Eigen::MatrixXs K = getControlK(now); + Eigen::VectorXs k = getControlk(now); + Eigen::VectorXs x = getControlState(now); + Eigen::VectorXs u = getControlAction(now); + s_t alpha = getControlAlpha(now); + Eigen::VectorXs stateErr = state - x; + Eigen::VectorXs action; + if(stateErr.norm() < 0.1) + { + action = (u + alpha*k + + K * (state - x) + ).cwiseMin(mActionBound).cwiseMax(-mActionBound); + } + else + { + action = u; + } + // Eigen::VectorXs action = u; + // std::cout << "Last Time Buffer: " << mBuffer.getLastWriteBufferTime() + // << " Law Buffer: " << mBuffer.getLastWriteBufferLawTime() <<" Action: " << a << std::endl; + // std::cout << "State Error: \n" << stateErr << std::endl; + // std::cout << "State: \n" << state << std::endl; + // std::cout << "Hypothetical State: \n" << x << std::endl; + // std::cout << "Action: " << action << std::endl; + // std::cout << "k: \n" << k << std::endl; + mBuffer.manuallyRecordObservedForce(now, action); + return mWorld->mapToForceSpaceVector(action); +} + +/// This returns how many millis we have left until we've run out of plan. +/// This can be a negative number, if we've run past our plan. +long iLQRLocal::getRemainingPlanBufferMillis() +{ + return mBuffer.getPlanBufferMillisAfter(timeSinceEpochMillis()); +} + +/// This can completely silence log output +void iLQRLocal::setSilent(bool silent) +{ + mSilent = silent; +} + +/// This enables linesearch on the IPOPT sub-problems. Defaults to true. This +/// increases the stability of solutions, but can lead to spikes in solution +/// times. +void iLQRLocal::setEnableLineSearch(bool enabled) +{ + mEnableLinesearch = enabled; +} + + +void iLQRLocal::disableAdaptiveTime() +{ + mAdaptiveTime = false; +} + +void iLQRLocal::disableAdaptiveHorizon() +{ + mAdaptiveHorizon = false; +} + +/// This enables "guards" on the IPOPT sub-problems. Defaults to false. This +/// means that every IPOPT sub-problem always returns the best explored +/// trajectory, even if it subsequently explored other states. This increases +/// the stability of solutions, but can lead to getting stuck in local minima. +void iLQRLocal::setEnableOptimizationGuards(bool enabled) +{ + mEnableOptimizationGuards = enabled; +} + +/// Defaults to false. This records every iteration of IPOPT in the log, so we +/// can debug it. This should only be used on iLQRLocal that's running for a +/// short time. Otherwise the log will grow without bound. +void iLQRLocal::setRecordIterations(bool enabled) +{ + mRecordIterations = enabled; +} + +/// This gets the current maximum number of iterations that IPOPT will be +/// allowed to run during an optimization. +int iLQRLocal::getMaxIterations() +{ + return mMaxIterations; +} + +/// This sets the current maximum number of iterations that IPOPT will be +/// allowed to run during an optimization. iLQRLocal reserves the right to change +/// this value during runtime depending on timing and performance values +/// observed during running. +void iLQRLocal::setMaxIterations(int maxIters) +{ + mMaxIterations = maxIters; +} + +/// This records the current state of the world based on some external sensing +/// and inference. This resets the error in our model just assuming the world +/// is exactly following our simulation. +void iLQRLocal::recordGroundTruthState( + long time, Eigen::VectorXs pos, Eigen::VectorXs vel, Eigen::VectorXs mass) +{ + mObservationLog.observe(time, pos, vel, mass); +} + +/// This optimizes a block of the plan, starting at `startTime` +/// startTime = current + millisAdvanceToPlan +void iLQRLocal::optimizePlan(long startTime) +{ + // We don't allow time to go backwards, because that leads to all sorts of + // issues. We can get called for a time before a time we already optimized + // for, because of dilating buffers in front of our current time. If that + // happens, just pretent like we were asked for the latest time we were + // optimizing for. + if (startTime < mLastOptimizedTime) + { + startTime = mLastOptimizedTime; + } + + if (mSolution == nullptr || variableChange()) + { + PerformanceLog::initialize(); + PerformanceLog* log = PerformanceLog::startRoot("TrajOptLocal loop"); + + std::shared_ptr worldClone = mWorld->clone(); + PerformanceLog* estimateState = log->startRun("Estimate State"); + + mBuffer.estimateWorldStateAt(worldClone, &mObservationLog, startTime); + estimateState->end(); + // std::cout<<"Optimization Stage"<startRun("Create Default IPOPT"); + + std::shared_ptr ipoptOptimizer + = std::make_shared(); + ipoptOptimizer->setCheckDerivatives(false); + ipoptOptimizer->setSuppressOutput(true); + ipoptOptimizer->setRecoverBest(mEnableOptimizationGuards); + ipoptOptimizer->setTolerance(1e-3); //1e-3 + ipoptOptimizer->setIterationLimit(mMaxIterations); + ipoptOptimizer->setDisableLinesearch(!mEnableLinesearch); + ipoptOptimizer->setRecordFullDebugInfo(false); + ipoptOptimizer->setRecordIterations(false); + if (mSilent) + { + ipoptOptimizer->setSilenceOutput(true); + } + mOptimizer = ipoptOptimizer; + + createOpt->end(); + } + + if (!mProblem || variableChange()) + { + std::shared_ptr multishot = std::make_shared( + worldClone, *mLoss.get(), mSteps, mShotLength, false); + multishot->setParallelOperationsEnabled(true); + mProblem = multishot; + mVarchange = false; + } + + PerformanceLog* optimizeTrack = log->startRun("Optimize"); + //std::cout<<"MPC Optimization Start"<optimize(mProblem.get()); + //std::cout<<"MPC Optimization end"<end(); + + mLastOptimizedTime = startTime; + + // Here mBuffer should have access to mapping of control force + Eigen::MatrixXs actions = worldClone->mapToActionSpace(mProblem->getRolloutCache(worldClone)->getControlForcesConst()); + // No need to pad here automatically paded inside + mBuffer.setControlForcePlan( + startTime, + timeSinceEpochMillis(), + actions); + + log->end(); + + // std::cout << PerformanceLog::finalize()["iLQRLocal loop"]->prettyPrint() + // << std::endl; + } + else + { + std::shared_ptr worldClone = mWorld->clone(); + // std::cout<<"Re-optimization stage "<(floor(static_cast(diff) / mMillisPerStep)); + int roundedDiff = steps * mMillisPerStep; + long roundedStartTime = mLastOptimizedTime + roundedDiff; + long totalPlanTime = mSteps * mMillisPerStep; + // How many percent of the planned force has been executed + s_t percentage = (s_t)roundedDiff * 100.0 / totalPlanTime; + + if (!mSilent) + { + std::cout << "Advancing plan by " << roundedDiff << "ms = " << steps + << " steps, " << (percentage) << "% of total " << totalPlanTime + << "ms plan time" << std::endl; + } + + long startComputeWallTime = timeSinceEpochMillis(); + + mBuffer.estimateWorldStateAt( + worldClone, &mObservationLog, roundedStartTime); + + // Make sure that in the force buffer, the pointer of starting point is pointed to current + mProblem->advanceSteps( + worldClone, + worldClone->getPositions(), + worldClone->getVelocities(), + steps); + + // Optimizer based partially on initial guess of where force supposed to be + mSolution->reoptimize(); + + // std::cout << "iLQRLocal::optimizePlan() mBuffer.setControlForcePlan()" << + // std::endl; + + // Update the control force plan from current time stamp. + // The new optimization problem is started at start time, however since the replanning + // Process take an non trivial amount of time, the force to be set is from now. + Eigen::MatrixXs actions = worldClone->mapToActionSpace(mProblem->getRolloutCache(worldClone)->getControlForcesConst()); + mBuffer.setControlForcePlan( + startTime, + timeSinceEpochMillis(), + actions); + + long computeDurationWallTime + = timeSinceEpochMillis() - startComputeWallTime; + + // Call any listeners that might be waiting on us + // Currently the Listeners are trajectory plotter + for (auto listener : mReplannedListeners) + { + listener( + startTime, + mProblem->getRolloutCache(worldClone), + computeDurationWallTime); + } + + if (!mSilent) + { + s_t factorOfSafety = 0.5; + std::cout << " -> We were allowed " + << (int)floor(roundedDiff * factorOfSafety) + << "ms to solve this problem (" << roundedDiff + << "ms new planning * " << factorOfSafety + << " factor of safety), and it took us " + << computeDurationWallTime << "ms" << std::endl; + } + // Here is when optimization finished, supposingly this time should equals to now + // And the real world can immediately get forced that has been planned + mLastOptimizedTime = roundedStartTime; + } +} + +/// This adjusts parameters to make sure we're keeping up with real time. We +/// can compute how many (ms / step) it takes us to optimize plans. Sometimes +/// we can decrease (ms / step) by increasing the length of the optimization +/// and increasing the parallelism. We can also change the step size in the +/// physics engine to produce less accurate results, but keep up with the +/// world in fewer steps. +/// TODO: (Eric) THIS FUNCITON IS HARD CODED!!! For diffenrent environment +/// it may cause serious problems accordingly +void iLQRLocal::adjustPerformance(long lastOptimizeTimeMillis) +{ + mMillisInAdvanceToPlan = 1.2 * lastOptimizeTimeMillis; + std::cout << "Estimated Planning time: " << mMillisInAdvanceToPlan << "From: " << lastOptimizeTimeMillis << std::endl; + if (mMillisInAdvanceToPlan > 200) + mMillisInAdvanceToPlan = 200; + if(!mAdaptiveTime) + mMillisInAdvanceToPlan = 20; // Which will be problematic when facing the change of parameters +} + +/// This starts our main thread and begins running optimizations +void iLQRLocal::start() +{ + if (mRunning) + return; + mRunning = true; + mOptimizationThread = std::thread(&iLQRLocal::optimizationThreadLoop, this); +} + +/// This starts our main thread and begins running iLQR optimizations +void iLQRLocal::ilqrstart() +{ + if (mRunning) + return; + mRunning = true; + // mBuffer.setiLQRFlag(true); + mOptimizationThread = std::thread(&iLQRLocal::iLQRoptimizationThreadLoop, this); +} + +/// This stops our main thread, waits for it to finish, and then returns +void iLQRLocal::stop() +{ + if (!mRunning) + return; + mRunning = false; + mOptimizationThread.join(); +} + +/// This stops our main thread, waits for it to finish, and then returns +void iLQRLocal::ilqrstop() +{ + if(!mRunning) + return; + mRunning = false; + mOptimizationThread.join(); +} + +/// This returns the main record we've been keeping of our optimization up to +/// this point +std::shared_ptr iLQRLocal::getCurrentSolution() +{ + return mSolution; +} + +/// ========================================================================== +/// ============ Here are functions for iLQR ================================= +/// This will run forward pass from start time and record info such as Jacobian +/// of each timestep and gradient as well as hessian from cost function +/// Assume the initial state from world coordinate is starting position +bool iLQRLocal::ilqrForward(simulation::WorldPtr world) +{ + Eigen::VectorXs pos = world->getPositions(); + Eigen::VectorXs vel = world->getVelocities(); + Eigen::VectorXs mass = world->getMasses(); + + std::vector Lx; + std::vector Lu; + std::vector Lxx; + std::vector Luu; + std::vector Lux; + + + while(mPatience > 0) + { + // Reset Control Force + // Let New State and control force equals to old one. + // Which is equivalent to deep copy. + mlqrBuffer.resetXUNew(); + + // set control force and initial condition + world->setPositions(pos); + world->setVelocities(vel); + world->setMasses(mass); + mlqrBuffer.Xnew[0].segment(0, world->getNumDofs()) = pos; + mlqrBuffer.Xnew[0].segment(world->getNumDofs(), world->getNumDofs()) = vel; + + // Rollout Trajectory + TrajectoryRolloutReal rollout = createRollout(mSteps, world->getNumDofs(), world->getMassDims(), world->getDampingDims(), world->getSpringDims()); + s_t loss = 0; + + // Executing the trajectory according to control law + getTrajectory(world, &rollout, &mlqrBuffer); + + // Require states be stored in rollout + // Assume loss except the final loss are multiplied by dt + if(mCostFn!=nullptr) + { + Lx = mCostFn->ilqrGradientEstimator(&rollout, loss, WRTFLAG::X); + Lu = mCostFn->ilqrGradientEstimator(&rollout, loss, WRTFLAG::U); + Lxx = mCostFn->ilqrHessianEstimator(&rollout, WRTFLAG::XX); + Luu = mCostFn->ilqrHessianEstimator(&rollout, WRTFLAG::UU); + Lux = mCostFn->ilqrHessianEstimator(&rollout, WRTFLAG::UX); + } + else + { + Lx = mMappedCostFn->ilqrGradientEstimator(&rollout, loss, WRTFLAG::X); + Lu = mMappedCostFn->ilqrGradientEstimator(&rollout, loss, WRTFLAG::U); + Lxx = mMappedCostFn->ilqrHessianEstimator(&rollout, WRTFLAG::XX); + Luu = mMappedCostFn->ilqrHessianEstimator(&rollout, WRTFLAG::UU); + Lux = mMappedCostFn->ilqrHessianEstimator(&rollout, WRTFLAG::UX); + } + + // Sanity Check + if(!mlqrBuffer.validateXnew()) + { + mAlpha *= 0.5; + mPatience--; + mNaN_Flag = true; + continue; + } + else if((mCost - loss) < 0) + { + mAlpha *= 0.5; + mPatience--; + mNaN_Flag = false; + continue; + } + else // Good to leave, Guarantee the loss is lower than prev_cost + { + mCost = loss; //Update cost with current loss + mPatience = mPatience_reset_value; + mAlpha_opt = mAlpha; + mAlpha = mAlpha_reset_value; + mRollout = rollout; + mNaN_Flag = false; + // std::cout << "Loss Set: " << loss << std::endl; + break; + } + } + if(mPatience == 0) + { + // std::cout << "Forward Pass Run Out of Patience, exiting ..." << std::endl; + // The out of patience is due to nan + if(mNaN_Flag) + { + std::cout << "nan_detected" << std::endl; + return false; + } + else // The out of patience is due to loss not decrease + { + // std::cout << "Cost Doesn't decrease, exiting..." << std::endl; + // Record new gradient and Hessian + // mlqrBuffer.updateXUOld(); + // mlqrBuffer.updateL(Lx, Lu, Lxx, Luu, Lux); + // This should not affect the number of patience since it will break; + mPatience += 1; + return false; + } + } + // Record new gradient and Hessian + mlqrBuffer.updateXUOld(); + mlqrBuffer.updateL(Lx, Lu, Lxx, Luu, Lux); + mlqrBuffer.updateAlpha(mAlpha_opt); + return true; +} + +bool iLQRLocal::ilqrBackward() +{ + // s_t dt = world->getTimeStep(); + bool done = false; + bool early_termination = false; + // The loop should exit either done or early terminate + while(!(early_termination || done)) + { + Eigen::VectorXs Vx = mlqrBuffer.Lx[mSteps]; + Eigen::MatrixXs Vxx = mlqrBuffer.Lxx[mSteps]; + for(int cursor = mSteps - 1; cursor >= 0; cursor--) + { + // Build up Q matrices + Eigen::VectorXs Qx = mlqrBuffer.Lx[cursor] + mlqrBuffer.Fx[cursor].transpose() * Vx; + Eigen::VectorXs Qu = mlqrBuffer.Lu[cursor] + mlqrBuffer.Fu[cursor].transpose() * Vx; + Eigen::MatrixXs Qxx = mlqrBuffer.Lxx[cursor] + + mlqrBuffer.Fx[cursor].transpose() * Vxx * mlqrBuffer.Fx[cursor]; + Eigen::MatrixXs Qux = mlqrBuffer.Lux[cursor].transpose() + + mlqrBuffer.Fu[cursor].transpose() * Vxx * mlqrBuffer.Fx[cursor]; + Eigen::MatrixXs Quu = mlqrBuffer.Luu[cursor] + + mlqrBuffer.Fu[cursor].transpose() * Vxx * mlqrBuffer.Fu[cursor]; + Eigen::MatrixXs I = Eigen::MatrixXs::Identity(Vxx.rows(),Vxx.cols()); + Eigen::MatrixXs Quubar = mlqrBuffer.Luu[cursor] + + mlqrBuffer.Fu[cursor].transpose() * (Vxx + mMU * I) * mlqrBuffer.Fu[cursor]; + Eigen::MatrixXs Quxbar = mlqrBuffer.Lux[cursor] + + (mlqrBuffer.Fu[cursor].transpose() * (Vxx + mMU * I) * mlqrBuffer.Fx[cursor]).transpose(); + if (abs((Quubar).determinant()) < 1e-20) + { + if(mPatience == 0) + { + early_termination = true; + // If the Q matrix kept non-invertible then it should be treated as NaN + std::cout << "Regularize Patience limit met, exiting... " << std::endl; + break; + } + else + { + std::cout << "Warning Singular Quu, iteration: "<< cursor + <<" -repeating backward pass with increased mu. Patience: " + < mDelta0) + { + mDelta *= mDelta0; + } + else + { + mDelta = mDelta0; + } + if(mMU * mDelta > mMU_MIN) + { + mMU *= mDelta; + } + else + { + mMU = mMU_MIN; + } + mPatience -= 1; + break; + } + } + else + { + // Compute K matrix + //std::cout << "Reached Correct Backward pass"<< cursor << std::endl; + Eigen::MatrixXs Quubar_inv = Quubar.inverse(); + mlqrBuffer.K[cursor] = -Quubar_inv * Quxbar.transpose(); + mlqrBuffer.k[cursor] = -Quubar_inv * Qu; + // Update Vxx and Vx + Vx = Qx + mlqrBuffer.K[cursor].transpose() * Quu * mlqrBuffer.k[cursor] + + mlqrBuffer.K[cursor].transpose() * Qu + + Qux.transpose() * mlqrBuffer.k[cursor]; + Vxx = Qxx + mlqrBuffer.K[cursor].transpose() * Quu * mlqrBuffer.K[cursor] + + mlqrBuffer.K[cursor].transpose() * Qux + + Qux.transpose() * mlqrBuffer.K[cursor]; + if(cursor == 0) + done = true; + } + } + } + mPatience = mPatience_reset_value; + // The only reason for early termination is that the backward process is keep getting NaN + if(early_termination) + { + mNaN_Flag = true; + return false; + } + // Progress Delta for next iteration + // Decrease mu + if(1.0/mDelta0 > mDelta / mDelta0) + { + mDelta /= mDelta0; + } + else + { + mDelta = 1.0/mDelta0; + } + + if(mMU * mDelta > mMU_MIN) + { + mMU = mMU * mDelta; + } + else + { + mMU = 0.0; + } + mNaN_Flag = false; + return true; +} + +/// This function rollout the trajectory and save all the Jacobians to Buffer +/// as well as record the loss +/// This function automatically update the trajectory +void iLQRLocal::getTrajectory(simulation::WorldPtr world, + TrajectoryRollout* rollout, + LQRBuffer* lqrBuffer) +{ + // Need to store the state and actions in rollout + Eigen::MatrixXs actions = Eigen::MatrixXs::Zero(mActionDim, mSteps); + for(int i = 0; i < mSteps; i++) + { + // Compute New actions + lqrBuffer->Unew[i] = (lqrBuffer->Unew[i] + mAlpha * lqrBuffer->k[i] + + lqrBuffer->K[i] * (lqrBuffer->Xnew[i] - lqrBuffer->X[i])).cwiseMin(mActionBound).cwiseMax(-mActionBound); + // Record state and action on rollout + // TODO: Need to make this robot skeleton + rollout->getPoses().col(i) = world->getPositions(); + rollout->getVels().col(i) = world->getVelocities(); + actions.col(i) = lqrBuffer->Unew[i]; + + // Set the control force just computed from control law + world->setAction(actions.col(i)); + // Step the world + std::shared_ptr snapshot = neural::forwardPass(world, false); + // set new state + lqrBuffer->Xnew[i+1].segment(0, world->getNumDofs()) = snapshot->getPostStepPosition(); + lqrBuffer->Xnew[i+1].segment(world->getNumDofs(), world->getNumDofs()) = snapshot->getPostStepVelocity(); + // set new Jacobian for linearization + lqrBuffer->Fx[i] = world->getStateJacobian(); + // Need to reassemble the matrix by actuated dofs + lqrBuffer->Fu[i] = world->getActionJacobian(); + } + // Final Step + rollout->getPoses().col(mSteps) = world->getPositions(); + rollout->getVels().col(mSteps) = world->getVelocities(); + rollout->getControlForces() = world->mapToForceSpace(actions); +} + +bool iLQRLocal::ilqroptimizePlan(long startTime) +{ + long startComputeWallTime = timeSinceEpochMillis(); + if (startTime < mLastOptimizedTime) + { + startTime = mLastOptimizedTime; + } + // Get action from mBuffer according to time + if(mPlanningCandidateHorizonMillis!=mPlanningHorizonMillis) + { + setPlanningHorizon(mPlanningCandidateHorizonMillis); + std::cout << "<<<<<<<<<<<<<<<<<<<<<<<" << std::endl; + std::cout << "Horizon Changed! " << mPlanningCandidateHorizonMillis <::max(); + mlqrBuffer.readNewActionPlan(startTime, mBuffer); + + // Copy the world + std::shared_ptr worldClone = mWorld->clone(); + + // Run some iterations of forward and backward + bool forward_flag = true; + bool backward_flag = true; + s_t last_cost = mCost; + // This Process takes non trivial amount of time + // By the time is finished or failed the real world has taken lots of steps + for(int iter = 0; iter < mMaxIterations; iter++) + { + // This function will forward world to the current point + mBuffer.estimateWorldStateAt(worldClone, &mObservationLog, startTime); + forward_flag = ilqrForward(worldClone); + if(!forward_flag || iter == mMaxIterations - 1) + { + break; + } + else + { + backward_flag = ilqrBackward(); + } + + if(!backward_flag) + { + std::cout << "Backward Terminated, Exiting ..." << std::endl; + break; + } + if(abs(last_cost - mCost) <= mTolerence) + { + break; + } + } + // Here inside the forward loop there actually mechanism which guarantee, the cost + // function should always decrease for MPC since the trajectory may be different and starting + // state is different, it may not be the case + // we need to enable mechanism that each optimization it will return the correspoding control laws + // that have lowest cost, and across optimization the lowest cost will be reset + // Hence in this case: + // If NaN detected in forward pass or non-invertible matrix detected in backward pass, the system + // need to be helted since the simulator has already crashed... + // If the forward pass the cost does not decay it should break the iteration whlie not executing the + // backward pass. + if(!mNaN_Flag) + { + // Update the mBuffer according to current time + bool flag = mlqrBuffer.detectXContinuity(); + if(!flag && mlqrBuffer.getVerbose()) + { + std::cout << "=====================" << std::endl; + std::cout << "X is not Continuous!!" << std::endl; + std::cout << "=====================" << std::endl; + } + mlqrBuffer.setNewActionPlan(startTime, &mBuffer); + mlqrBuffer.setNewControlLaw(startTime, &mBuffer); + + // Reset Parameters + mPatience = mPatience_reset_value; + mMU = mMU_reset_value; + mDelta = mDelta0; + mAlpha = mAlpha_reset_value; + // Invoke callback functions + long computeDurationWallTime = timeSinceEpochMillis() - startComputeWallTime; + // Here mRollout is the last valid rollout from forward pass + for (auto listener : mReplannedListeners) + { + listener(startTime, &mRollout, computeDurationWallTime); + } + return true; + } + return false; +} + +void iLQRLocal::setPatience(int patience) +{ + mPatience = patience; + mPatience_reset_value = patience; +} + +void iLQRLocal::setTolerence(s_t tolerence) +{ + mTolerence = tolerence; +} + +int iLQRLocal::getPatience() +{ + return mTolerence; +} + +void iLQRLocal::setAlpha(s_t alpha) +{ + mAlpha = alpha; + mAlpha_opt = alpha; + mAlpha_reset_value = alpha; +} + +s_t iLQRLocal::getAlpha() +{ + return mAlpha; +} + +void iLQRLocal::setMU(s_t mu) +{ + mMU = mu; +} + +s_t iLQRLocal::getMU() +{ + return mMU; +} + +TrajectoryRolloutReal iLQRLocal::createRollout(size_t steps, size_t dofs, size_t mass_dim, size_t damping_dim, size_t spring_dim) +{ + Eigen::MatrixXs pos = Eigen::MatrixXs::Zero(dofs, steps+1); + std::unordered_map pos_map; + pos_map.emplace("identity", pos); + Eigen::MatrixXs vel = Eigen::MatrixXs::Zero(dofs, steps+1); + std::unordered_map vel_map; + vel_map.emplace("identity", vel); + Eigen::MatrixXs forces = Eigen::MatrixXs::Zero(dofs, steps); + std::unordered_map force_map; + force_map.emplace("identity", forces); + Eigen::VectorXs mass = Eigen::VectorXs::Zero(mass_dim); + Eigen::VectorXs damping = Eigen::VectorXs::Zero(damping_dim); + Eigen::VectorXs spring = Eigen::VectorXs::Zero(spring_dim); + std::unordered_map meta; + TrajectoryRolloutReal rollout = TrajectoryRolloutReal(pos_map, vel_map, force_map, mass, damping, spring, meta); + return rollout; +} + +Eigen::VectorXs iLQRLocal::getControlk(long now) +{ + return mBuffer.getPlannedk(now); +} + +Eigen::MatrixXs iLQRLocal::getControlK(long now) +{ + return mBuffer.getPlannedK(now); +} + +Eigen::VectorXs iLQRLocal::getControlState(long now) +{ + return mBuffer.getPlannedState(now); +} + +void iLQRLocal::setTargetReachingCostFn(std::shared_ptr costFn) +{ + mCostFn = costFn; + mLoss = costFn->getLossFn(); +} + +/// Set action hard bound that any action computed by control law will be clipped to this. +void iLQRLocal::setActionBound(s_t actionBound) +{ + assert(actionBound >= 0); + mActionBound = actionBound; + mBuffer.setActionBound(actionBound); +} + +void iLQRLocal::setPredictUsingFeedback(bool feedback_flag) +{ + mBuffer.setiLQRFlag(feedback_flag); +} + +Eigen::MatrixXs iLQRLocal::assembleJacobianMatrix(Eigen::MatrixXs B) +{ + size_t ndim = mActuatedJoint.size(); + Eigen::MatrixXs Bnew = Eigen::MatrixXs::Zero(ndim, ndim); + for(size_t i = 0; i < Bnew.cols(); i++) + { + for(size_t j = 0; j < Bnew.cols(); j++) + { + Bnew(i,j) = B((size_t)mActuatedJoint(i),(size_t)mActuatedJoint(j)); + } + } + return Bnew; +} + +std::vector iLQRLocal::getStatesFromiLQRBuffer() +{ + return mlqrBuffer.X; +} + +std::vector iLQRLocal::getActionsFromiLQRBuffer() +{ + return mlqrBuffer.U; +} + +void iLQRLocal::setCurrentCost(s_t cost) +{ + mCost = cost; +} + +s_t iLQRLocal::getCurrentCost() +{ + return mCost; +} +/// This registers a listener to get called when we finish replanning +void iLQRLocal::registerReplanningListener( + std::function + replanListener) +{ + mReplannedListeners.push_back(replanListener); +} + +/// This launches a server on the specified port +void iLQRLocal::serve(int port) +{ + std::string server_address("0.0.0.0:" + std::to_string(port)); + + grpc::EnableDefaultHealthCheckService(true); + // grpc::reflection::InitProtoReflectionServerBuilderPlugin(); + grpc::ServerBuilder builder; + // Listen on the given address without any authentication mechanism. + builder.AddListeningPort(server_address, grpc::InsecureServerCredentials()); + // Register "service" as the instance through which we'll communicate with + // clients. In this case it corresponds to an *synchronous* service. + RPCWrapperiLQRLocal wrapper(*this); + + builder.RegisterService(&wrapper); + // Finally assemble the server. + std::unique_ptr server(builder.BuildAndStart()); + std::cout << "Server listening on " << server_address << std::endl; + + // Wait for the server to shutdown. Note that some other thread must be + // responsible for shutting down the server for this call to ever return. + server->Wait(); +} + +/////////////////////////////////////////////////////////////////////// +/// Implements the gRPC API +/////////////////////////////////////////////////////////////////////// + +RPCWrapperiLQRLocal::RPCWrapperiLQRLocal(iLQRLocal& local) : mLocal(local) +{ +} + +/// Remotely start the compute running +grpc::Status RPCWrapperiLQRLocal::Start( + grpc::ServerContext* /* context */, + const proto::MPCStartRequest* /* request */, + proto::MPCStartReply* /* response */) +{ + mLocal.start(); + return grpc::Status::OK; +} + +/// Remotely stop the compute running +grpc::Status RPCWrapperiLQRLocal::Stop( + grpc::ServerContext* /* context */, + const proto::MPCStopRequest* /* request */, + proto::MPCStopReply* /* response */) +{ + mLocal.stop(); + return grpc::Status::OK; +} + +/// Remotely listen for replanning updates +grpc::Status RPCWrapperiLQRLocal::ListenForUpdates( + grpc::ServerContext* /* context */, + const proto::MPCListenForUpdatesRequest* /* request */, + grpc::ServerWriter* writer) +{ + proto::MPCListenForUpdatesReply reply; + mLocal.registerReplanningListener( + [&](long startTime, + const trajectory::TrajectoryRollout* rollout, + long duration) { + reply.mutable_rollout()->Clear(); + rollout->serialize(*reply.mutable_rollout()); + reply.set_starttime(startTime); + reply.set_replandurationmillis(duration); + writer->Write(reply); + }); + + while (true) + { + // spin + } +} + +/// Remotely listen for replanning updates +grpc::Status RPCWrapperiLQRLocal::RecordGroundTruthState( + grpc::ServerContext* /* context */, + const proto::MPCRecordGroundTruthStateRequest* request, + proto::MPCRecordGroundTruthStateReply* /* reply */) +{ + // std::cout << "gRPC server: RecordGroundTruthState" << std::endl; + mLocal.recordGroundTruthState( + request->time(), + deserializeVector(request->pos()), + deserializeVector(request->vel()), + deserializeVector(request->mass())); + return grpc::Status::OK; +} + +/// Remotely listen for replanning updates +grpc::Status RPCWrapperiLQRLocal::ObserveForce( + grpc::ServerContext* /* context */, + const proto::MPCObserveForceRequest* request, + proto::MPCObserveForceReply* /* reply */) +{ + // std::cout << "gRPC server: ObserveForce" << std::endl; + // TODO: Change according to change in dim + mLocal.mBuffer.manuallyRecordObservedForce( + request->time(), deserializeVector(request->force())); + return grpc::Status::OK; +} + +/// This is the function for the optimization thread to run when we're live +void iLQRLocal::optimizationThreadLoop() +{ + // block signals in this thread and subsequently + // spawned threads, so they're guaranteed to go to the server thread + sigset_t sigset; + sigemptyset(&sigset); + sigaddset(&sigset, SIGINT); + sigaddset(&sigset, SIGTERM); + pthread_sigmask(SIG_BLOCK, &sigset, nullptr); + + while (mRunning) + { + long startTime = timeSinceEpochMillis(); + optimizePlan(startTime + mMillisInAdvanceToPlan); + long endTime = timeSinceEpochMillis(); + // std::cout <<"Time Per Run: "<< (float)(endTime - startTime)/1000 << std::endl; + adjustPerformance(endTime - startTime); + } +} + +/// This is the function which use iLQR as trajectory optimization +void iLQRLocal::iLQRoptimizationThreadLoop() +{ + // block signals in this thread and subsequentially + // spawned threads, so they're guaranteed to go to the server thread + sigset_t sigset; + sigemptyset(&sigset); + sigaddset(&sigset, SIGINT); + sigaddset(&sigset, SIGTERM); + pthread_sigmask(SIG_BLOCK, &sigset, nullptr); + + while(mRunning) + { + long startTime = timeSinceEpochMillis(); + bool status = ilqroptimizePlan(startTime + mMillisInAdvanceToPlan); + //std::cout << "Planning Continue..." << std::endl; + if(!status) + { + std::cout << "Simulator Crashed by giving NaN..." << std::endl; + // mRunning = false; + // break; + } + long endTime = timeSinceEpochMillis(); + // std::cout <<"Time Per Run: "<< (float)(endTime - startTime)/1000 << std::endl; + adjustPerformance(endTime - startTime); + } +} + +bool iLQRLocal::variableChange() +{ + return mVarchange; +} + +void iLQRLocal::setParameterChange(Eigen::VectorXs params) +{ + if(mInitialized == false) + { + mPre_parameter = params; + mInitialized = true; + mVarchange = true; + } + else + { + assert(params.size() == mPre_parameter.size()); + if((params-mPre_parameter).norm()>0.001) + { + mVarchange = true; + } + mPre_parameter = params; + } +} + +} // namespace realtime +} // namespace dart \ No newline at end of file diff --git a/dart/realtime/iLQRLocal.hpp b/dart/realtime/iLQRLocal.hpp new file mode 100644 index 000000000..12634ce67 --- /dev/null +++ b/dart/realtime/iLQRLocal.hpp @@ -0,0 +1,392 @@ +#ifndef DART_REALTIME_iLQRLocal +#define DART_REALTIME_iLQRLocal + +#include +#include + +#include + +#include "dart/proto/MPC.grpc.pb.h" +#include "dart/realtime/MPC.hpp" +#include "dart/realtime/RealTimeControlBuffer.hpp" +#include "dart/realtime/ObservationLog.hpp" +#include "dart/math/MathTypes.hpp" +#include "dart/realtime/TargetReachingCost.hpp" +#include "dart/realtime/MappedTargetReachingCost.hpp" +#include "dart/simulation/SmartPointer.hpp" +#include "dart/neural/BackpropSnapshot.hpp" + +using namespace grpc; + +namespace dart { + +namespace simulation { +class World; +} + +namespace neural { + class BackpropSnapShot; +} + +namespace trajectory { +class LossFn; +class Solution; +class Problem; +class Optimizer; +class TrajectoryRollout; +class TrajectoryRolloutReal; +} + +namespace realtime { + +enum Extrapolate_Method { + ZERO, + LAST, + RANDOM}; + +class LQRBuffer +{ +public: + LQRBuffer( + int steps, + size_t nDofs, + size_t nControls, + Extrapolate_Method extrapolate); + + void resetXUNew(); + + void updateXUOld(); + + void updateAlpha(s_t a); + + void readNewActionPlan(long timestamp, RealTimeControlBuffer buffer); + + // Write New Action to Buffer + void setNewActionPlan(long timestamp, RealTimeControlBuffer *buffer); + + // This will simutaneously update K, k ,state, alpha + void setNewControlLaw(long timestamp, RealTimeControlBuffer *buffer); + + void updateL(std::vector Lx_new, std::vector Lu_new, + std::vector Lxx_new, std::vector Luu_new, + std::vector Lux_new); + + void updateF(std::vector Fx_new, std::vector Fu_new); + + bool validateXnew(); + + bool detectXContinuity(); + + void setVerbose(bool verbose); + + bool getVerbose(); + void setNumSteps(size_t steps); + + + // Parameters + size_t nsteps; + size_t mMaxSteps; + size_t control_dim; + size_t state_dim; + Extrapolate_Method ext; + bool mVerbose = false; + + // system related + std::vector X; + std::vector Xnew; + std::vector U; + std::vector Unew; + std::vector K; + std::vector k; + std::vector alpha; + // jacobians + std::vector Fx; + std::vector Fu; + + // Gradients + std::vector Lx; + std::vector Lu; + + // Hessians + std::vector Lxx; + std::vector Lux; + std::vector Luu; +}; + + +class iLQRLocal final : public MPC +{ +public: + iLQRLocal( + std::shared_ptr world, + size_t nControls, + int planningHorizonMillis, + s_t scale); + + /// This set cost function and corresponding loss function + void setCostFn(std::shared_ptr costFn); + + /// This set mapped function and corresponding loss function + void setMappedCostFn(std::shared_ptr costFn); + + /// This sets the optimizer for trajectory optimization + void setOptimizer(std::shared_ptr optimizer); + + /// This returns the current optimizer that MPC is using + std::shared_ptr getOptimizer(); + + /// This sets the problem for MPC + void setProblem(std::shared_ptr problem); + + /// This returns the problem + std::shared_ptr getProblem(); + + /// This get Control Force from current timestep + Eigen::VectorXs getControlForce(long now) override; + + /// This get feed forward term k from current timestep + Eigen::VectorXs getControlk(long now); + + /// This get State based on time step + Eigen::VectorXs getControlState(long now); + + /// This get feedback matrix from current timestep + Eigen::MatrixXs getControlK(long now); + + s_t getControlAlpha(long now); + + Eigen::VectorXs getControlAction(long now); + + Eigen::VectorXs computeForce(Eigen::VectorXs state, long now); + + /// This returns how many millis we have left until we've run out of plan + long getRemainingPlanBufferMillis() override; + + /// This can completely silence log output + void setSilent(bool silent); + + /// This enable linesearch on the IPOPT sub-problem or iLQR. Defaults to true + void setEnableLineSearch(bool enabled); + + /// This enable optimization guards of trajectory optimization. + void setEnableOptimizationGuards(bool enabled); + + /// Record iterations + int getMaxIterations(); + + /// This sets the current maximum number of iterations for trajopt or iLQR + void setMaxIterations(int maxIters); + + /// This set record iteration + void setRecordIterations(bool enabled); + + /// This record the current state + void recordGroundTruthState( + long time, + Eigen::VectorXs pos, + Eigen::VectorXs vel, + Eigen::VectorXs mass) override; + + /// This optimize the plan starting at starttime + void optimizePlan(long startTime); + + /// This will adjust parameters to make sure we are upto date + void adjustPerformance(long lastOptimizeTimeMillis); + + std::shared_ptr getCurrentSolution(); + + /// ========================================================= + /// ==== Here are funcitons for iLQR ======================== + /// This will run forward pass from starttime and record info + /// such as Jacobian of each step and gradient as well as + /// hessian from cost function + bool ilqrForward(simulation::WorldPtr world); + + bool ilqrBackward(); + + bool ilqroptimizePlan(long startTime); + + void setTargetReachingCostFn(std::shared_ptr costFn); + + /// Get the jacobian for linearization + void getTrajectory(simulation::WorldPtr world, + trajectory::TrajectoryRollout* rollout, + LQRBuffer* lqrBuffer); + + void setPatience(int patience); + + void setTolerence(s_t tolerence); + + int getPatience(); + + /// set initial action learning rate, prevent numerical error + void setAlpha(s_t alpha); + + s_t getAlpha(); + + void setMU(s_t mu); + + s_t getMU(); + + /// ========================================================== + + void registerReplanningListener( + std::function + replanListener) override; + + void serve(int port); + + bool variableChange(); + + void setParameterChange(Eigen::VectorXs params); + + void start() override; + + void ilqrstart(); + + void stop() override; + + void ilqrstop(); + + // For Debug + std::vector getStatesFromiLQRBuffer(); + + std::vector getActionsFromiLQRBuffer(); + + void setCurrentCost(s_t cost); + + s_t getCurrentCost(); + + trajectory::TrajectoryRolloutReal createRollout(size_t steps, size_t dofs, size_t mass_dim, size_t damping_dim , size_t spring_dim); + + void setActionBound(s_t actionBound); + + void setPredictUsingFeedback(bool feedback_flag); + + void disableAdaptiveTime(); + + void disableAdaptiveHorizon(); + + void setPlanningHorizon(size_t planningHorizonMillis); + + void setCandidateHorizon(size_t candidateHorizonMillis); + +protected: + void optimizationThreadLoop(); + + void iLQRoptimizationThreadLoop(); + + Eigen::MatrixXs assembleJacobianMatrix(Eigen::MatrixXs B); + + bool mRunning; + std::shared_ptr mWorld; + std::shared_ptr mLoss; + ObservationLog mObservationLog; + + // Meta Config + bool mEnableLinesearch; + bool mEnableOptimizationGuards; + bool mRecordIterations; + + int mPlanningHorizonMillis; + int mPlanningCandidateHorizonMillis; + int mMillisPerStep; + int mSteps; + int mMaxSteps; + int mShotLength; + int mMaxIterations; + int mMillisInAdvanceToPlan; + long mLastOptimizedTime; + // Cost function instance + std::shared_ptr mCostFn = nullptr; + std::shared_ptr mMappedCostFn = nullptr; + + RealTimeControlBuffer mBuffer; + LQRBuffer mlqrBuffer; + s_t mCost; + // Some Parameters of ilqr + Eigen::VectorXi mActuatedJoint; + s_t mAlpha_reset_value; + s_t mAlpha; + s_t mAlpha_opt; + int mPatience_reset_value; + int mPatience; + int mDelta0; + int mDelta; + s_t mMU_MIN; + s_t mMU; + s_t mMU_reset_value; + s_t mTolerence; + int mActionDim; + int mStateDim; + s_t mActionBound = 1000; // Default 1000 or numerical issue will occur + + // Some Flags for iteration + bool mNaN_Flag = false; + bool mAdaptiveTime = true; + bool mAdaptiveHorizon = true; + trajectory::TrajectoryRolloutReal mRollout; + + + std::thread mOptimizationThread; + bool mSilent; + // Some history track for compare and set in SSID + Eigen::VectorXs mPre_parameter; + bool mInitialized = false; + bool mVarchange = false; + + Eigen::VectorXs mLast_U; + + /// For Trajectory Optimization + std::shared_ptr mOptimizer; + std::shared_ptr mSolution; + std::shared_ptr mProblem; + + std::vector> + mReplannedListeners; + + friend class RPCWrapperiLQRLocal; +}; + +class RPCWrapperiLQRLocal : public proto::MPCService::Service +{ +public: + RPCWrapperiLQRLocal(iLQRLocal& local); +/// Remotely start the compute running + grpc::Status Start( + grpc::ServerContext* context, + const proto::MPCStartRequest* request, + proto::MPCStartReply* response) override; + + /// Remotely stop the compute running + grpc::Status Stop( + grpc::ServerContext* context, + const proto::MPCStopRequest* request, + proto::MPCStopReply* response) override; + + /// Remotely listen for replanning updates + grpc::Status ListenForUpdates( + grpc::ServerContext* context, + const proto::MPCListenForUpdatesRequest* request, + grpc::ServerWriter* writer) override; + + /// Remotely listen for replanning updates + grpc::Status RecordGroundTruthState( + grpc::ServerContext* context, + const proto::MPCRecordGroundTruthStateRequest* request, + proto::MPCRecordGroundTruthStateReply* reply) override; + + /// Remotely listen for replanning updates + grpc::Status ObserveForce( + grpc::ServerContext* context, + const proto::MPCObserveForceRequest* request, + proto::MPCObserveForceReply* reply) override; + +protected: + iLQRLocal& mLocal; +}; + +} // namespace realtime +} // namespace dart + +#endif \ No newline at end of file diff --git a/dart/realtime/saved_data/plotter.py b/dart/realtime/saved_data/plotter.py deleted file mode 100644 index 2c2a67efe..000000000 --- a/dart/realtime/saved_data/plotter.py +++ /dev/null @@ -1,45 +0,0 @@ -import numpy as np -import matplotlib.pyplot as plt -import argparse -import os - -def load_np_array(file,samples): - lines = file.readlines() - n_lines = len(lines) - data = np.zeros((n_lines,samples)) - for i,line in enumerate(lines): - entries = line.split(' ') - entries[-1] = entries[-1][:-1] - cur = 0 - for e in entries: - if e != '': - data[i,cur] = float(e) - cur += 1 - return data -parser = argparse.ArgumentParser() -parser.add_argument('--loss',type=str,default='Losses') -parser.add_argument('--solution',type=str,default='Solutions') -parser.add_argument('--exp_name',type=str,default=None,required=True) -parser.add_argument('--xlabel',type=str,default='mass') -parser.add_argument('--upper',type=float,default= 2.0) -parser.add_argument('--lower',type=float,default=-2.0) -args = parser.parse_args() - - -file = open(f'raw_data/{args.loss}.txt') -np_data = load_np_array(file,200) -file.close() -file = open(f'raw_data/{args.solution}.txt') -solutions = load_np_array(file,1) -file.close() -np.savez(f'np_files/{args.exp_name}.npz',loss=np_data,solution=solutions) - -x = np.linspace(start=args.lower,stop=args.upper,num=200) -os.makedirs(f'images/{args.exp_name}',exist_ok=True) -for i in range(np_data.shape[0]): - plt.clf() - plt.plot(x,np_data[i][::-1]) - plt.axvline(x=float(solutions[i]),color='red') - plt.xlabel(args.xlabel) - plt.ylabel('loss') - plt.savefig(f'images/{args.exp_name}/fig{i}.png',dpi=200) diff --git a/dart/realtime/saved_data/plotter2d.py b/dart/realtime/saved_data/plotter2d.py deleted file mode 100644 index 1bb0eb8ed..000000000 --- a/dart/realtime/saved_data/plotter2d.py +++ /dev/null @@ -1,40 +0,0 @@ -import numpy as np -import matplotlib.pyplot as plt -import argparse -import os - -parser = argparse.ArgumentParser() -parser.add_argument('--loss',type=str,default='Losses') -parser.add_argument('--solution',type=str,default='Solutions') -parser.add_argument('--exp_name',type=str,default=None,required=True) -parser.add_argument('--rest_dim',type=int,default=2) -parser.add_argument('--x_upper',type=float,default= 2.0) -parser.add_argument('--x_lower',type=float,default= -2.0) -parser.add_argument('--y_upper',type=float,default= 2.0) -parser.add_argument('--y_lower',type=float,default= -2.0) -args = parser.parse_args() - -np_data = [] -csv_files = os.listdir(f'raw_data/{args.loss}/') -for f in csv_files: - np_data.append(np.genfromtxt(f'raw_data/{args.loss}/{f}',delimiter=',').T) - -solutions = np.genfromtxt(f'raw_data/{args.solution}.csv',delimiter=',') - -np.savez(f'np_files/{args.exp_name}_2d.npz',loss=np_data,solution=solutions) - -os.makedirs(f'images/{args.exp_name}',exist_ok=True) -for i in range(len(np_data)): - plt.clf() - mean = np_data[i].mean() - plt.imshow(np_data[i].clip(0,mean), - origin='lower', - extent=[args.x_lower,args.x_upper, - args.y_lower,args.y_upper], - cmap='hot') - cold_index = [0,1,2] - cold_index.remove(args.rest_dim) - plt.plot(solutions[i][cold_index[0]], - solutions[i][cold_index[1]], - 'bo') - plt.savefig(f'images/{args.exp_name}/fig{i}.png',dpi=200) diff --git a/dart/realtime/saved_data/timeplots/cartpole_10_identified_0.csv b/dart/realtime/saved_data/timeplots/cartpole_10_identified_0.csv new file mode 100644 index 000000000..d10a3a040 --- /dev/null +++ b/dart/realtime/saved_data/timeplots/cartpole_10_identified_0.csv @@ -0,0 +1,188 @@ +1.5, 1, 0.4 +1.5, 1, 0.4 +1.5, 1, 0.4 +1.5, 1, 0.4 +1.5, 1, 0.4 +0.843905514924099, 0.389646308709534, 1.00000000465745 +0.973010414545712, 0.422403026656524, 0.104747470535978 +0.973010414545712, 0.422403026656524, 0.104747470535978 +0.97568187664495, 0.418579986514003, 0.136749044187557 +0.973047994541749, 0.422677580011141, 0.172305947393818 +0.974239700329569, 0.365392881714531, 0.220355522041956 +0.996721614033456, 0.379271579960143, 0.252012462906868 +0.996721614033456, 0.379271579960143, 0.252012462906868 +0.996721614033456, 0.379271579960143, 0.252012462906868 +0.996721614033456, 0.379271579960143, 0.252012462906868 +0.998542869483574, 0.42961479260879, 0.293457479681043 +1.00538214542993, 0.479742505070536, 0.339483970832739 +1.01920153001298, 0.469948350100889, 0.280685124870597 +1.02911983313956, 0.466127896868369, 0.241697823255431 +1.03227253573717, 0.470529822008333, 0.229131604125275 +1.03227253573717, 0.470529822008333, 0.229131604125275 +1.03590094725915, 0.471880092595154, 0.218523444971198 +1.04155301523317, 0.472568445714292, 0.210556562801555 +1.02786459485123, 0.480882407550541, 0.205090191823935 +1.03324573194057, 0.481371116177579, 0.202186478931948 +1.0392665048537, 0.484475135978155, 0.199158618939551 +1.02257532640623, 0.502604622271697, 0.189050158284401 +0.982127556590424, 0.516534259322655, 0.18142613084854 +0.956058309257451, 0.524504761669427, 0.17747918871253 +0.88881637186037, 0.536299247379435, 0.172438443574834 +0.902566656671007, 0.52756792218809, 0.173512546267142 +0.902566656671007, 0.52756792218809, 0.173512546267142 +0.902566656671007, 0.52756792218809, 0.173512546267142 +0.902566656671007, 0.52756792218809, 0.173512546267142 +0.902566656671007, 0.52756792218809, 0.173512546267142 +0.902566656671007, 0.52756792218809, 0.173512546267142 +0.902566656671007, 0.52756792218809, 0.173512546267142 +0.902566656671007, 0.52756792218809, 0.173512546267142 +0.902566656671007, 0.52756792218809, 0.173512546267142 +0.902566656671007, 0.52756792218809, 0.173512546267142 +0.902566656671007, 0.52756792218809, 0.173512546267142 +0.905919760903362, 0.595695824711155, 0.169124238142811 +0.905919760903362, 0.595695824711155, 0.169124238142811 +0.913269220492295, 0.646729858296287, 0.163891232532748 +0.922905663296881, 0.693356925295671, 0.158028081020683 +0.93389882115619, 0.739131316071642, 0.151369630411907 +0.946730555578099, 0.781842585164104, 0.144213077845235 +0.96612823846214, 0.818378078746678, 0.136665863566499 +0.990913831525365, 0.856701425404813, 0.128790431508105 +1.02151160720366, 0.903420274261856, 0.121145269683636 +1.0555254652529, 1.00686848568921, 0.132327441160394 +1.07949661179876, 1.12276621037032, 0.166202584139581 +1.10365969349752, 1.22314554327663, 0.179246515315979 +1.12453964934433, 1.20962543588602, 0.192270033846368 +1.15651296019171, 1.3470491071128, 0.192846075560616 +1.15651296019171, 1.3470491071128, 0.192846075560616 +1.17330156528324, 1.3106105225526, 0.205449104741822 +1.1865081066035, 1.27747248340075, 0.215229605527116 +1.19464516101163, 1.2645190792876, 0.222062803879916 +1.19464516101163, 1.2645190792876, 0.222062803879916 +1.19464516101163, 1.2645190792876, 0.222062803879916 +1.19464516101163, 1.2645190792876, 0.222062803879916 +1.18108600960557, 1.33440594040279, 0.230256246233079 +1.18565276766898, 1.4201433469745, 0.242364839361372 +1.19387883553689, 1.46335613740792, 0.242994683488073 +1.20948643014255, 1.54999372317682, 0.2524767379766 +1.23278410871073, 1.65976966075298, 0.264815289541309 +1.26435336962077, 1.78597955864045, 0.278369963372988 +1.29423205667208, 1.82664053678531, 0.271961077627384 +1.31401924804214, 1.8589038934834, 0.268000606600594 +1.32904637516895, 1.89213700813774, 0.26136257642549 +1.34844593252228, 1.93794627309582, 0.251764901458543 +1.34844593252228, 1.93794627309582, 0.251764901458543 +1.34916779776126, 1.93355975607033, 0.249741000183492 +1.34565452649847, 1.9311708825042, 0.248187673739749 +1.34235760622985, 1.9402945728854, 0.2463584679655 +1.31828414101976, 1.96660775674644, 0.243537989220377 +1.3078957679053, 1.9979144219869, 0.240737839368007 +1.31457256292645, 1.9794559894777, 0.243992772969918 +1.27889952075858, 1.99805701826554, 0.243154988346134 +1.27889952075858, 1.99805701826554, 0.243154988346134 +1.27889952075858, 1.99805701826554, 0.243154988346134 +1.27889952075858, 1.99805701826554, 0.243154988346134 +1.27889952075858, 1.99805701826554, 0.243154988346134 +1.27996868206694, 2.0062752847149, 0.242007439147294 +1.28400401864514, 2.00710077884388, 0.239931552464979 +1.28400401864514, 2.00710077884388, 0.239931552464979 +1.28824736672843, 2.00782970249542, 0.237320165062212 +1.2949163714081, 2.00483736610315, 0.23407221027399 +1.30464906499904, 1.99885362835441, 0.230196558480097 +1.31663072549097, 1.99048716662015, 0.225781996771011 +1.33120414585748, 1.97959815911566, 0.220750994228948 +1.34973348338638, 1.97305343275573, 0.21695270228983 +1.36548606785618, 1.99086349384139, 0.223226442871627 +1.3795729083979, 2.01806367952129, 0.230587939205995 +1.3795729083979, 2.01806367952129, 0.230587939205995 +1.39609323744232, 2.00442749882727, 0.233648208243338 +1.40955346318572, 1.9777643030834, 0.239587047601027 +1.41848211543909, 1.94171449413716, 0.245392465077768 +1.42595490028725, 1.91700534283488, 0.249774935549817 +1.42922669950004, 1.9132201378571, 0.251733771176395 +1.42922669950004, 1.9132201378571, 0.251733771176395 +1.42922669950004, 1.9132201378571, 0.251733771176395 +1.42922669950004, 1.9132201378571, 0.251733771176395 +1.42922669950004, 1.9132201378571, 0.251733771176395 +1.40939135053004, 1.91905405713469, 0.250238085335729 +1.41347058443925, 1.93988950521219, 0.2520121704273 +1.4201317910814, 1.94476982202397, 0.248967025109241 +1.4309800460835, 1.95045179049525, 0.245283964710683 +1.44694990038893, 1.95776026777725, 0.240884342101642 +1.46857894882358, 1.99899664310785, 0.244611244111751 +1.47928266384349, 2.01151530310419, 0.242513445601173 +1.48990798760597, 2.03390242056551, 0.238306794998626 +1.48990798760597, 2.03390242056551, 0.238306794998626 +1.49275726991762, 2.03335073161997, 0.236555325306019 +1.49208180827444, 2.03133897089345, 0.23546036738829 +1.48906053634836, 2.03160485497586, 0.234594879407028 +1.48450799291613, 2.03967658300705, 0.233295480138584 +1.48705627801903, 2.05688791165739, 0.231643195775983 +1.49121557877175, 2.06794602672468, 0.230521948786265 +1.49121557877175, 2.06794602672468, 0.230521948786265 +1.49121557877175, 2.06794602672468, 0.230521948786265 +1.49121557877175, 2.06794602672468, 0.230521948786265 +1.49121557877175, 2.06794602672468, 0.230521948786265 +1.48598432971319, 2.07175818116498, 0.229882297242813 +1.4796983733442, 2.07310018099135, 0.22861050634494 +1.47454702048272, 2.07296896624278, 0.226937006816413 +1.46830497867783, 2.07123886533433, 0.224826165835889 +1.45852851270629, 2.06811188429512, 0.222298866580058 +1.45852851270629, 2.06811188429512, 0.222298866580058 +1.44133019298625, 2.06356198168278, 0.219241589809907 +1.41092812758866, 2.0676829445584, 0.217757893065615 +1.38923163867959, 2.08891194942754, 0.221063792264358 +1.39532551453789, 2.12851537123917, 0.221977726190493 +1.37714961918086, 2.13375404147939, 0.223261108882798 +1.37714961918086, 2.13375404147939, 0.223261108882798 +1.36882385602961, 2.13565610164424, 0.224481088221209 +1.3696330093768, 2.1200792359208, 0.227585614782046 +1.36836387194923, 2.13120129388701, 0.226912980475743 +1.37011783850027, 2.12092161736992, 0.228759768819194 +1.37011783850027, 2.12092161736992, 0.228759768819194 +1.37011783850027, 2.12092161736992, 0.228759768819194 +1.34326981954409, 2.12444838485186, 0.228096669751348 +1.33170851436146, 2.12710201456081, 0.226859645925801 +1.33474263400223, 2.13215408801062, 0.225218825922005 +1.33621771168214, 2.13981543029873, 0.22302741402341 +1.33338945776223, 2.151571648278, 0.220209363084976 +1.32528902697048, 2.16641406239621, 0.216440521487514 +1.32528902697048, 2.16641406239621, 0.216440521487514 +1.32019983720008, 2.18628158881705, 0.214413245134586 +1.31505867553038, 2.20377937546652, 0.210249571563322 +1.29395669022562, 2.20210301489663, 0.208291708639057 +1.23796741462275, 2.19872463933722, 0.207141594802361 +1.23484326846924, 2.20243266447289, 0.205217670232987 +1.23081653797262, 2.20682544157717, 0.203616853254273 +1.21828598677779, 2.21211379896423, 0.202268990525814 +1.18850651320678, 2.21582615311074, 0.20146246302769 +1.16549088444523, 2.21812440523903, 0.20120524671383 +1.16738314678571, 2.21497903126246, 0.20156164778012 +1.16738314678571, 2.21497903126246, 0.20156164778012 +1.16738314678571, 2.21497903126246, 0.20156164778012 +1.16738314678571, 2.21497903126246, 0.20156164778012 +1.16738314678571, 2.21497903126246, 0.20156164778012 +1.16738314678571, 2.21497903126246, 0.20156164778012 +1.16738314678571, 2.21497903126246, 0.20156164778012 +1.16738314678571, 2.21497903126246, 0.20156164778012 +1.16738314678571, 2.21497903126246, 0.20156164778012 +1.16738314678571, 2.21497903126246, 0.20156164778012 +1.16738314678571, 2.21497903126246, 0.20156164778012 +1.16738314678571, 2.21497903126246, 0.20156164778012 +1.16738314678571, 2.21497903126246, 0.20156164778012 +1.16738314678571, 2.21497903126246, 0.20156164778012 +1.16738314678571, 2.21497903126246, 0.20156164778012 +1.16738314678571, 2.21497903126246, 0.20156164778012 +1.16738314678571, 2.21497903126246, 0.20156164778012 +1.16738314678571, 2.21497903126246, 0.20156164778012 +1.16738314678571, 2.21497903126246, 0.20156164778012 +1.16738314678571, 2.21497903126246, 0.20156164778012 +1.16738314678571, 2.21497903126246, 0.20156164778012 +1.16738314678571, 2.21497903126246, 0.20156164778012 +1.16738314678571, 2.21497903126246, 0.20156164778012 +1.16738314678571, 2.21497903126246, 0.20156164778012 +1.16738314678571, 2.21497903126246, 0.20156164778012 +1.16738314678571, 2.21497903126246, 0.20156164778012 +1.16738314678571, 2.21497903126246, 0.20156164778012 +1.16738314678571, 2.21497903126246, 0.20156164778012 +1.16738314678571, 2.21497903126246, 0.20156164778012 +1.16738314678571, 2.21497903126246, 0.20156164778012 \ No newline at end of file diff --git a/dart/realtime/saved_data/timeplots/cartpole_10_real_0.csv b/dart/realtime/saved_data/timeplots/cartpole_10_real_0.csv new file mode 100644 index 000000000..1ced2d3b3 --- /dev/null +++ b/dart/realtime/saved_data/timeplots/cartpole_10_real_0.csv @@ -0,0 +1,188 @@ +1, 0.5, 0.2 +1, 0.5, 0.2 +1, 0.5, 0.2 +1, 0.5, 0.2 +1, 0.5, 0.2 +1, 0.5, 0.2 +1, 0.5, 0.2 +1, 0.5, 0.2 +1, 0.5, 0.2 +1, 0.5, 0.2 +1, 0.5, 0.2 +1, 0.5, 0.2 +1, 0.5, 0.2 +1, 0.5, 0.2 +1, 0.5, 0.2 +1, 0.5, 0.2 +1, 0.5, 0.2 +1, 0.5, 0.2 +1, 0.5, 0.2 +1, 0.5, 0.2 +1, 0.5, 0.2 +1, 0.5, 0.2 +1, 0.5, 0.2 +1, 0.5, 0.2 +1, 0.5, 0.2 +1, 0.5, 0.2 +1, 0.5, 0.2 +1, 0.5, 0.2 +1, 0.5, 0.2 +1, 0.5, 0.2 +3, 2.5, 0.2 +3, 2.5, 0.2 +3, 2.5, 0.2 +3, 2.5, 0.2 +3, 2.5, 0.2 +3, 2.5, 0.2 +3, 2.5, 0.2 +3, 2.5, 0.2 +3, 2.5, 0.2 +3, 2.5, 0.2 +3, 2.5, 0.2 +3, 2.5, 0.2 +3, 2.5, 0.2 +3, 2.5, 0.2 +3, 2.5, 0.2 +3, 2.5, 0.2 +3, 2.5, 0.2 +3, 2.5, 0.2 +3, 2.5, 0.2 +3, 2.5, 0.2 +3, 2.5, 0.2 +3, 2.5, 0.2 +3, 2.5, 0.2 +3, 2.5, 0.2 +3, 2.5, 0.2 +3, 2.5, 0.2 +3, 2.5, 0.2 +3, 2.5, 0.2 +3, 2.5, 0.2 +3, 2.5, 0.2 +3, 2.5, 0.2 +3, 2.5, 0.2 +3, 2.5, 0.2 +3, 2.5, 0.2 +3, 2.5, 0.2 +3, 2.5, 0.2 +3, 2.5, 0.2 +3, 2.5, 0.2 +3, 2.5, 0.2 +3, 2.5, 0.2 +3, 2.5, 0.2 +3, 2.5, 0.2 +3, 2.5, 0.2 +3, 2.5, 0.2 +3, 2.5, 0.2 +3, 2.5, 0.2 +3, 2.5, 0.2 +3, 2.5, 0.2 +3, 2.5, 0.2 +3, 2.5, 0.2 +3, 2.5, 0.2 +3, 2.5, 0.2 +3, 2.5, 0.2 +3, 2.5, 0.2 +3, 2.5, 0.2 +3, 2.5, 0.2 +3, 2.5, 0.2 +3, 2.5, 0.2 +3, 2.5, 0.2 +3, 2.5, 0.2 +3, 2.5, 0.2 +3, 2.5, 0.2 +3, 2.5, 0.2 +3, 2.5, 0.2 +3, 2.5, 0.2 +3, 2.5, 0.2 +3, 2.5, 0.2 +3, 2.5, 0.2 +3, 2.5, 0.2 +3, 2.5, 0.2 +3, 2.5, 0.2 +3, 2.5, 0.2 +3, 2.5, 0.2 +3, 2.5, 0.2 +3, 2.5, 0.2 +3, 2.5, 0.2 +3, 2.5, 0.2 +3, 2.5, 0.2 +3, 2.5, 0.2 +3, 2.5, 0.2 +3, 2.5, 0.2 +3, 2.5, 0.2 +3, 2.5, 0.2 +3, 2.5, 0.2 +3, 2.5, 0.2 +3, 2.5, 0.2 +3, 2.5, 0.2 +3, 2.5, 0.2 +3, 2.5, 0.2 +3, 2.5, 0.2 +1, 2.5, 0.1 +1, 2.5, 0.1 +1, 2.5, 0.1 +1, 2.5, 0.1 +1, 2.5, 0.1 +1, 2.5, 0.1 +1, 2.5, 0.1 +1, 2.5, 0.1 +1, 2.5, 0.1 +1, 2.5, 0.1 +1, 2.5, 0.1 +1, 2.5, 0.1 +1, 2.5, 0.1 +1, 2.5, 0.1 +1, 2.5, 0.1 +1, 2.5, 0.1 +1, 2.5, 0.1 +1, 2.5, 0.1 +1, 2.5, 0.1 +1, 2.5, 0.1 +1, 2.5, 0.1 +1, 2.5, 0.1 +1, 2.5, 0.1 +1, 2.5, 0.1 +1, 2.5, 0.1 +1, 2.5, 0.1 +1, 2.5, 0.1 +1, 2.5, 0.1 +1, 2.5, 0.1 +1, 2.5, 0.1 +1, 2.5, 0.1 +1, 2.5, 0.1 +1, 2.5, 0.1 +1, 2.5, 0.1 +1, 2.5, 0.1 +1, 2.5, 0.1 +1, 2.5, 0.1 +1, 2.5, 0.1 +1, 2.5, 0.1 +1, 2.5, 0.1 +1, 2.5, 0.1 +1, 2.5, 0.1 +1, 2.5, 0.1 +1, 2.5, 0.1 +1, 2.5, 0.1 +1, 2.5, 0.1 +1, 2.5, 0.1 +1, 2.5, 0.1 +1, 2.5, 0.1 +1, 2.5, 0.1 +1, 2.5, 0.1 +1, 2.5, 0.1 +1, 2.5, 0.1 +1, 2.5, 0.1 +1, 2.5, 0.1 +1, 2.5, 0.1 +1, 2.5, 0.1 +1, 2.5, 0.1 +1, 2.5, 0.1 +1, 2.5, 0.1 +1, 2.5, 0.1 +1, 2.5, 0.1 +1, 2.5, 0.1 +1, 2.5, 0.1 +1, 2.5, 0.1 +1, 2.5, 0.1 +1, 2.5, 0.1 +1, 2.5, 0.1 \ No newline at end of file diff --git a/dart/realtime/saved_data/timeplots/cartpole_20_identified_0.csv b/dart/realtime/saved_data/timeplots/cartpole_20_identified_0.csv new file mode 100644 index 000000000..b1b5bb69b --- /dev/null +++ b/dart/realtime/saved_data/timeplots/cartpole_20_identified_0.csv @@ -0,0 +1,188 @@ +1.5, 1 +1.5, 1 +1.5, 1 +1.5, 1 +1.5, 1 +1.00301936823057, 0.439822278543517 +0.995100528755759, 0.496772946947788 +0.995100528755759, 0.496772946947788 +0.995100528755759, 0.496772946947788 +0.995100528755759, 0.496772946947788 +0.995100528755759, 0.496772946947788 +0.995100528755759, 0.496772946947788 +0.995100528755759, 0.496772946947788 +0.995100528755759, 0.496772946947788 +0.995100528755759, 0.496772946947788 +0.995100528755759, 0.496772946947788 +0.995100528755759, 0.496772946947788 +0.995100528755759, 0.496772946947788 +0.995100528755759, 0.496772946947788 +0.995100528755759, 0.496772946947788 +0.995100528755759, 0.496772946947788 +0.995100528755759, 0.496772946947788 +0.995100528755759, 0.496772946947788 +0.995100528755759, 0.496772946947788 +0.995100528755759, 0.496772946947788 +0.995100528755759, 0.496772946947788 +0.995100528755759, 0.496772946947788 +0.995100528755759, 0.496772946947788 +0.995100528755759, 0.496772946947788 +0.995100528755759, 0.496772946947788 +0.995100528755759, 0.496772946947788 +0.995100528755759, 0.496772946947788 +0.995100528755759, 0.496772946947788 +0.995100528755759, 0.496772946947788 +0.995100528755759, 0.496772946947788 +0.199999989998183, 0.942427459628766 +0.199999989998183, 0.942427459628766 +2.89672401631317, 2.54038866383826 +2.92975518497433, 2.53535674564363 +2.92975518497433, 2.53535674564363 +2.9696192021163, 2.51368447694282 +2.98796226849116, 2.48241105076035 +2.98796226849116, 2.48241105076035 +2.98796226849116, 2.48241105076035 +2.98796226849116, 2.48241105076035 +2.98796226849116, 2.48241105076035 +2.98796226849116, 2.48241105076035 +2.98796226849116, 2.48241105076035 +2.98796226849116, 2.48241105076035 +2.98796226849116, 2.48241105076035 +2.98796226849116, 2.48241105076035 +2.98796226849116, 2.48241105076035 +2.98796226849116, 2.48241105076035 +2.98796226849116, 2.48241105076035 +2.98796226849116, 2.48241105076035 +2.98796226849116, 2.48241105076035 +2.98796226849116, 2.48241105076035 +3.01381470518725, 2.48697536243531 +2.98664904082779, 2.50669214461204 +2.98664904082779, 2.50669214461204 +2.98664904082779, 2.50669214461204 +2.98664904082779, 2.50669214461204 +2.98664904082779, 2.50669214461204 +2.98664904082779, 2.50669214461204 +2.98664904082779, 2.50669214461204 +2.98664904082779, 2.50669214461204 +2.98664904082779, 2.50669214461204 +2.98664904082779, 2.50669214461204 +2.70800783259935, 2.58870235731534 +2.91070196856172, 2.56772901043286 +2.92922472859348, 2.55841599638816 +2.92922472859348, 2.55841599638816 +2.92922472859348, 2.55841599638816 +2.92922472859348, 2.55841599638816 +2.92922472859348, 2.55841599638816 +2.92922472859348, 2.55841599638816 +2.92922472859348, 2.55841599638816 +2.92922472859348, 2.55841599638816 +2.92922472859348, 2.55841599638816 +2.92922472859348, 2.55841599638816 +2.92922472859348, 2.55841599638816 +2.92922472859348, 2.55841599638816 +2.88286500561428, 2.46799942014254 +2.88224828142724, 2.47014161206107 +2.88224828142724, 2.47014161206107 +2.88224828142724, 2.47014161206107 +2.88224828142724, 2.47014161206107 +2.88224828142724, 2.47014161206107 +3.03312813084749, 2.49163564603971 +3.03312813084749, 2.49163564603971 +3.02251683725634, 2.53239098907804 +3.02251683725634, 2.53239098907804 +3.02251683725634, 2.53239098907804 +3.02251683725634, 2.53239098907804 +3.02251683725634, 2.53239098907804 +3.02251683725634, 2.53239098907804 +3.02251683725634, 2.53239098907804 +3.02251683725634, 2.53239098907804 +3.02251683725634, 2.53239098907804 +3.02251683725634, 2.53239098907804 +2.45434714586072, 2.59544328432428 +2.74297005125592, 2.58554996206539 +2.82745363347737, 2.58365611018419 +2.82745363347737, 2.58365611018419 +2.82745363347737, 2.58365611018419 +3.03002496084044, 2.55593168080753 +2.99130983815935, 2.50463086151073 +2.99130983815935, 2.50463086151073 +2.99130983815935, 2.50463086151073 +2.99130983815935, 2.50463086151073 +2.99130983815935, 2.50463086151073 +2.99130983815935, 2.50463086151073 +2.99130983815935, 2.50463086151073 +2.99130983815935, 2.50463086151073 +2.99130983815935, 2.50463086151073 +2.99130983815935, 2.50463086151073 +2.4819965945426, 2.3746548876222 +2.63434485867656, 2.38196921733831 +3.64208767558534, 2.39503370911828 +3.64208767558534, 2.39503370911828 +2.86601966438918, 2.39659591766379 +2.96333474443958, 2.3991042599005 +2.96333474443958, 2.3991042599005 +2.96333474443958, 2.3991042599005 +2.96333474443958, 2.3991042599005 +2.96333474443958, 2.3991042599005 +2.96333474443958, 2.3991042599005 +1.00085039579623, 0.498894194701985 +1.01232565194921, 0.502772271331038 +1.01232565194921, 0.502772271331038 +1.01232565194921, 0.502772271331038 +1.01232565194921, 0.502772271331038 +1.01232565194921, 0.502772271331038 +1.01232565194921, 0.502772271331038 +1.01232565194921, 0.502772271331038 +1.01232565194921, 0.502772271331038 +1.01232565194921, 0.502772271331038 +1.01232565194921, 0.502772271331038 +1.01232565194921, 0.502772271331038 +1.01232565194921, 0.502772271331038 +1.01232565194921, 0.502772271331038 +1.01232565194921, 0.502772271331038 +1.01232565194921, 0.502772271331038 +1.01232565194921, 0.502772271331038 +1.01232565194921, 0.502772271331038 +1.01232565194921, 0.502772271331038 +1.01232565194921, 0.502772271331038 +1.01232565194921, 0.502772271331038 +1.01232565194921, 0.502772271331038 +1.01232565194921, 0.502772271331038 +1.01232565194921, 0.502772271331038 +1.01232565194921, 0.502772271331038 +1.01232565194921, 0.502772271331038 +1.01232565194921, 0.502772271331038 +1.01232565194921, 0.502772271331038 +1.01232565194921, 0.502772271331038 +1.01232565194921, 0.502772271331038 +1.01232565194921, 0.502772271331038 +1.01232565194921, 0.502772271331038 +1.01232565194921, 0.502772271331038 +1.01232565194921, 0.502772271331038 +1.01232565194921, 0.502772271331038 +1.01232565194921, 0.502772271331038 +1.01232565194921, 0.502772271331038 +1.01232565194921, 0.502772271331038 +1.01232565194921, 0.502772271331038 +1.01232565194921, 0.502772271331038 +1.01232565194921, 0.502772271331038 +1.01232565194921, 0.502772271331038 +1.01232565194921, 0.502772271331038 +1.01232565194921, 0.502772271331038 +1.01232565194921, 0.502772271331038 +1.01232565194921, 0.502772271331038 +1.01232565194921, 0.502772271331038 +1.01232565194921, 0.502772271331038 +1.01232565194921, 0.502772271331038 +1.01232565194921, 0.502772271331038 +1.01232565194921, 0.502772271331038 +1.01232565194921, 0.502772271331038 +1.01232565194921, 0.502772271331038 +1.01232565194921, 0.502772271331038 +1.01232565194921, 0.502772271331038 +1.01232565194921, 0.502772271331038 +1.01232565194921, 0.502772271331038 +1.01232565194921, 0.502772271331038 +1.01232565194921, 0.502772271331038 +1.01232565194921, 0.502772271331038 +1.22826301174187, 0.19999999 \ No newline at end of file diff --git a/dart/realtime/saved_data/timeplots/cartpole_20_real_0.csv b/dart/realtime/saved_data/timeplots/cartpole_20_real_0.csv new file mode 100644 index 000000000..9b96e6bf9 --- /dev/null +++ b/dart/realtime/saved_data/timeplots/cartpole_20_real_0.csv @@ -0,0 +1,188 @@ +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +3, 2.5 +3, 2.5 +3, 2.5 +3, 2.5 +3, 2.5 +3, 2.5 +3, 2.5 +3, 2.5 +3, 2.5 +3, 2.5 +3, 2.5 +3, 2.5 +3, 2.5 +3, 2.5 +3, 2.5 +3, 2.5 +3, 2.5 +3, 2.5 +3, 2.5 +3, 2.5 +3, 2.5 +3, 2.5 +3, 2.5 +3, 2.5 +3, 2.5 +3, 2.5 +3, 2.5 +3, 2.5 +3, 2.5 +3, 2.5 +3, 2.5 +3, 2.5 +3, 2.5 +3, 2.5 +3, 2.5 +3, 2.5 +3, 2.5 +3, 2.5 +3, 2.5 +3, 2.5 +3, 2.5 +3, 2.5 +3, 2.5 +3, 2.5 +3, 2.5 +3, 2.5 +3, 2.5 +3, 2.5 +3, 2.5 +3, 2.5 +3, 2.5 +3, 2.5 +3, 2.5 +3, 2.5 +3, 2.5 +3, 2.5 +3, 2.5 +3, 2.5 +3, 2.5 +3, 2.5 +3, 2.5 +3, 2.5 +3, 2.5 +3, 2.5 +3, 2.5 +3, 2.5 +3, 2.5 +3, 2.5 +3, 2.5 +3, 2.5 +3, 2.5 +3, 2.5 +3, 2.5 +3, 2.5 +3, 2.5 +3, 2.5 +3, 2.5 +3, 2.5 +3, 2.5 +3, 2.5 +3, 2.5 +3, 2.5 +3, 2.5 +3, 2.5 +3, 2.5 +3, 2.5 +3, 2.5 +3, 2.5 +3, 2.5 +3, 2.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 \ No newline at end of file diff --git a/dart/realtime/saved_data/timeplots/cartpole_2_identified_0.csv b/dart/realtime/saved_data/timeplots/cartpole_2_identified_0.csv new file mode 100644 index 000000000..4527afc6f --- /dev/null +++ b/dart/realtime/saved_data/timeplots/cartpole_2_identified_0.csv @@ -0,0 +1,188 @@ +1.5, 1 +1.5, 1 +2.93203948396802, 2.91216231698562 +2.92689074477734, 2.90738921957499 +2.92689074477734, 2.90738921957499 +2.92689074477734, 2.90738921957499 +2.92689074477734, 2.90738921957499 +1.1733412468572, 0.199999989999934 +0.950370529013541, 0.199999990000004 +0.950370529013541, 0.199999990000004 +0.950370529013541, 0.199999990000004 +0.950370529013541, 0.199999990000004 +1.08621253667932, 0.199999989998189 +1.08120721240206, 0.199999989998186 +1.08120721240206, 0.199999989998186 +1.08120721240206, 0.199999989998186 +1.08120721240206, 0.199999989998186 +0.922167672116887, 0.19999999000001 +0.938530036799762, 0.200002745380007 +0.938530036799762, 0.200002745380007 +5.00000004999928, 3.78779010272861 +5.00000004999951, 2.55657013749874 +5.00000004999954, 2.38051570529899 +0.647417962417426, 2.17698052260124 +0.451062664942586, 2.49171812092701 +5.00000004999926, 2.71013477386627 +4.62040422708615, 4.03204112234564 +4.62040422708615, 4.03204112234564 +4.62040422708615, 4.03204112234564 +4.62040422708615, 4.03204112234564 +4.62040422708615, 4.03204112234564 +4.62040422708615, 4.03204112234564 +1.42771482554366, 5.00000005000909 +0.233221240159718, 5.000000049989 +0.233221240159718, 5.000000049989 +0.233221240159718, 5.000000049989 +0.233221240159718, 5.000000049989 +5.00000004999855, 5.00000004999989 +5.00000004999855, 5.00000004999989 +5.00000005, 5.00000004999985 +5.00000004999968, 5.00000004999979 +5.00000004999968, 5.00000004999979 +5.00000005000885, 4.38412141138128 +4.3036990907461, 4.781439816542 +4.50979043420087, 4.56851293271317 +4.6525211422809, 4.61362750293541 +1.35685783149527, 0.47832698400392 +1.35717384972748, 0.527567033460489 +1.53074497814696, 0.452239811944641 +1.5964167546501, 0.419526977542186 +1.5964167546501, 0.419526977542186 +1.59641742389768, 0.419528780994569 +1.59641742389768, 0.419528780994569 +5.00000004999977, 0.408750348609046 +4.9520000494, 0.408750348609046 +4.9520000494, 0.408750348609046 +4.9520000494, 0.408750348609046 +4.9520000494, 0.408750348609046 +4.9520000494, 0.408750348609046 +1.76405488303716, 0.541612225363217 +1.71274936061647, 0.562947181598625 +1.71274936061647, 0.562947181598625 +1.44237864540041, 0.761233400286052 +0.603485883989715, 3.74859529703929 +1.19315812697484, 4.45205984844928 +1.66088825369054, 4.30121198751647 +1.86755487630809, 4.32051169943348 +5.00000005000875, 4.35564300792532 +0.564198826738298, 3.3913946572286 +1.17694171211434, 4.1558659561502 +1.43902410964891, 4.31032961223352 +1.93237226212027, 4.54849055459502 +2.14548491288988, 4.63074892606106 +2.23941038619799, 4.66132309480895 +5.00000004999812, 5.00000004999989 +5.00000004999812, 5.00000004999989 +5.00000004999812, 5.00000004999989 +5.00000004999812, 5.00000004999989 +5.00000004999812, 5.00000004999989 +5.00000004999812, 5.00000004999989 +5.00000004999812, 5.00000004999989 +5.00000004999812, 5.00000004999989 +5.00000004999967, 3.79729306334829 +5.00000004999967, 3.79729306334829 +4.89372730272058, 1.97725144771711 +1.85573649387833, 0.931058169612036 +0.763365257569585, 1.04231566076832 +0.907001152034998, 0.845272273641135 +2.06465837916778, 0.331374532376953 +2.06465837916778, 0.331374532376953 +2.06465837916778, 0.331374532376953 +2.49281569698874, 2.14597712373 +2.49281569698874, 2.14597712373 +2.49281569698874, 2.14597712373 +2.25265588728543, 0.431423048306645 +1.99550937303022, 0.473988083852863 +1.93220064084458, 0.487940457288005 +1.50332037092238, 0.676510288545316 +1.48733167174391, 0.683731187330737 +1.48733167174391, 0.683731187330737 +1.48733167174391, 0.683731187330737 +5.00000004999968, 4.25226104637793 +5.00000004999968, 4.26804352869517 +5.00000005000875, 4.53487693292385 +5.00000005, 4.37864228073936 +5.00000004999972, 5.00000005000063 +5.00000005, 5.00000004999986 +5.00000005, 5.00000004999986 +5.00000005, 5.00000004999986 +5.00000005, 5.00000004999986 +5.00000005, 5.00000004999986 +5.00000005, 5.00000004999986 +5.00000005, 5.00000004999986 +5.00000005, 5.00000004999986 +5.00000005, 5.00000004999986 +5.00000005, 5.00000004999986 +3.7080099613421, 5.0000000499999 +3.7080099613421, 5.0000000499999 +5.0000000499996, 3.69792813543727 +5.00000005000331, 3.56476919334453 +1.89723440387832, 1.06398555700061 +1.7331594754576, 0.881546395417073 +0.980056403895295, 0.199999990000005 +1.03329045766158, 0.19999999 +1.03329045766158, 0.19999999 +1.03329045766158, 0.19999999 +1.03329302475347, 0.210004731129831 +1.03329302475347, 0.210004731129831 +1.03329302475347, 0.210004731129831 +1.03329302475347, 0.210004731129831 +1.03329302475347, 0.210004731129831 +1.03329302475347, 0.210004731129831 +1.03329302475347, 0.210004731129831 +1.03329302475347, 0.210004731129831 +1.03329302475347, 0.210004731129831 +1.03329302475347, 0.210004731129831 +1.03329302475347, 0.210004731129831 +1.03329302475347, 0.210004731129831 +0.200008736214941, 0.980602848579103 +1.01961750148144, 2.68259864897346 +2.39818146354783, 2.68731819192697 +0.744479265966667, 2.27206243576819 +0.49711945092346, 2.07989943114424 +0.334031841418484, 1.76563221011946 +0.199999990000157, 4.07290873197421 +0.199999990000148, 3.00901070475694 +0.203691559245025, 3.00901070475694 +0.199999990000234, 2.37129217805395 +0.199999990001185, 2.71284642892651 +0.278506231255655, 1.97455167908327 +0.278506231255655, 1.97455167908327 +2.13584396149872, 2.47442461181175 +2.13584396149872, 2.47442461181175 +1.12518522394004, 3.08824897616158 +0.60348975180659, 3.14239494187689 +0.3914612745186, 3.34066154785672 +1.49743322567134, 5.00000004999177 +0.822606092008086, 0.916330806877683 +1.1550989937311, 0.424415030086286 +1.15509794665018, 0.424419128484952 +1.15509794665018, 0.424419128484952 +1.15509794665018, 0.424419128484952 +1.15509794665018, 0.424419128484952 +1.15509794665018, 0.424419128484952 +2.03306347712053, 5.00000004645212 +5.00000004999667, 5.00000005 +1.66168408426371, 5.00000004999616 +1.3761405096652, 0.199999989999841 +1.40606293328831, 0.199999990000022 +1.03712933618333, 0.59579634775552 +1.13199744848542, 0.500985852284446 +1.13199744848542, 0.500985852284446 +5.00000005001006, 4.96770229857474 +4.6823618132047, 4.68236529932103 +4.5524303451174, 4.55287728643679 +4.5524303451174, 4.55287728643679 +4.47178687334422, 4.47216475154398 +4.47178687334422, 4.47216475154398 +4.43650347321404, 4.43685181631594 +1.51419405300404, 0.736919167606924 +1.34192762619741, 0.417906774564856 +1.34192652651439, 0.417910568596661 +1.34192652651439, 0.417910568596661 +1.34192652651439, 0.417910568596661 +1.34192652651439, 0.417910568596661 +1.34192652651439, 0.417910568596661 +4.97227938577084, 0.205435066646946 +4.97227938577084, 0.205435066646946 \ No newline at end of file diff --git a/dart/realtime/saved_data/timeplots/cartpole_2_real_0.csv b/dart/realtime/saved_data/timeplots/cartpole_2_real_0.csv new file mode 100644 index 000000000..9b96e6bf9 --- /dev/null +++ b/dart/realtime/saved_data/timeplots/cartpole_2_real_0.csv @@ -0,0 +1,188 @@ +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +3, 2.5 +3, 2.5 +3, 2.5 +3, 2.5 +3, 2.5 +3, 2.5 +3, 2.5 +3, 2.5 +3, 2.5 +3, 2.5 +3, 2.5 +3, 2.5 +3, 2.5 +3, 2.5 +3, 2.5 +3, 2.5 +3, 2.5 +3, 2.5 +3, 2.5 +3, 2.5 +3, 2.5 +3, 2.5 +3, 2.5 +3, 2.5 +3, 2.5 +3, 2.5 +3, 2.5 +3, 2.5 +3, 2.5 +3, 2.5 +3, 2.5 +3, 2.5 +3, 2.5 +3, 2.5 +3, 2.5 +3, 2.5 +3, 2.5 +3, 2.5 +3, 2.5 +3, 2.5 +3, 2.5 +3, 2.5 +3, 2.5 +3, 2.5 +3, 2.5 +3, 2.5 +3, 2.5 +3, 2.5 +3, 2.5 +3, 2.5 +3, 2.5 +3, 2.5 +3, 2.5 +3, 2.5 +3, 2.5 +3, 2.5 +3, 2.5 +3, 2.5 +3, 2.5 +3, 2.5 +3, 2.5 +3, 2.5 +3, 2.5 +3, 2.5 +3, 2.5 +3, 2.5 +3, 2.5 +3, 2.5 +3, 2.5 +3, 2.5 +3, 2.5 +3, 2.5 +3, 2.5 +3, 2.5 +3, 2.5 +3, 2.5 +3, 2.5 +3, 2.5 +3, 2.5 +3, 2.5 +3, 2.5 +3, 2.5 +3, 2.5 +3, 2.5 +3, 2.5 +3, 2.5 +3, 2.5 +3, 2.5 +3, 2.5 +3, 2.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 +1, 0.5 \ No newline at end of file diff --git a/dart/simulation/World.cpp b/dart/simulation/World.cpp index 893dd72b2..1063d7da1 100644 --- a/dart/simulation/World.cpp +++ b/dart/simulation/World.cpp @@ -56,6 +56,8 @@ #include "dart/neural/NeuralUtils.hpp" #include "dart/neural/RestorableSnapshot.hpp" #include "dart/neural/WithRespectToMass.hpp" +#include "dart/neural/WithRespectToDamping.hpp" +#include "dart/neural/WithRespectToSpring.hpp" #include "dart/server/RawJsonUtils.hpp" namespace dart { @@ -86,6 +88,8 @@ World::World(const std::string& _name) mContactClippingDepth(0.03), mPenetrationCorrectionEnabled(false), mWrtMass(std::make_shared()), + mWrtDamping(std::make_shared()), + mWrtSpring(std::make_shared()), mUseFDOverride(false), mSlowDebugResultsAgainstFD(false), mConstraintEngineFn([this](bool _resetCommand) { @@ -126,6 +130,8 @@ WorldPtr World::clone() const // Copy the WithRespectToMass pointer, so we have the same object worldClone->mWrtMass = mWrtMass; + worldClone->mWrtDamping = mWrtDamping; + worldClone->mWrtSpring = mWrtSpring; auto cd = getConstraintSolver()->getCollisionDetector(); worldClone->getConstraintSolver()->setCollisionDetector( @@ -140,6 +146,8 @@ WorldPtr World::clone() const cloned_skel->setLinkCOMs(mSkeletons[i]->getLinkCOMs()); cloned_skel->setLinkMOIs(mSkeletons[i]->getLinkMOIs()); cloned_skel->setLinkBetas(mSkeletons[i]->getLinkBetas()); + cloned_skel->setDampingCoeffVector(mSkeletons[i]->getDampingCoeffVector()); + cloned_skel->setSpringStiffVector(mSkeletons[i]->getSpringStiffVector()); worldClone->addSkeleton(cloned_skel); } @@ -396,6 +404,18 @@ std::shared_ptr World::getWrtMass() return mWrtMass; } +//============================================================================== +std::shared_ptr World::getWrtDamping() +{ + return mWrtDamping; +} + +//============================================================================== +std::shared_ptr World::getWrtSpring() +{ + return mWrtSpring; +} + //============================================================================== /// This returns the world state as a JSON blob that we can render std::string World::toJson() @@ -737,6 +757,28 @@ dynamics::BodyNode* World::getBodyNodeByIndex(size_t index) return nodes[index]; } +dynamics::Joint* World::getJointIndex(size_t index) +{ + size_t cur = 0; + size_t skeleton_id = 0; + for(int i = 0; i < mSkeletons.size();i++) + { + cur += mSkeletons[i]->getNumJoints(); + if(cur > index) + { + cur -= mSkeletons[i]->getNumJoints(); + skeleton_id = i; + break; + } + } + return mSkeletons[skeleton_id]->getJoint(index - cur); +} + +s_t World::getRestPositionIndex(size_t index) +{ + return getDofs()[index]->getRestPosition(); +} + //============================================================================== std::size_t World::getNumSkeletons() const { @@ -1014,6 +1056,16 @@ std::size_t World::getMassDims() return mWrtMass->dim(this); } +std::size_t World::getDampingDims() +{ + return mWrtDamping->dim(this); +} + +std::size_t World::getSpringDims() +{ + return mWrtSpring->dim(this); +} + //============================================================================== /// This will prevent mass from being tuned void World::clearTunableMassThisInstance() @@ -1021,6 +1073,16 @@ void World::clearTunableMassThisInstance() mWrtMass = std::make_shared(); } +void World::clearTunableDampingThisInstance() +{ + mWrtDamping = std::make_shared(); +} + +void World::clearTunableSpringThisInstance() +{ + mWrtSpring = std::make_shared(); +} + //============================================================================== /// This registers that we'd like to keep track of this BodyNode's mass in a /// specified way in differentiation @@ -1033,6 +1095,28 @@ void World::tuneMass( mWrtMass->registerNode(node, type, upperBound, lowerBound); } +/// This registers that we'd like to keep track of this Joint's damping or spring in +/// a specific way in differentiation +void World::tuneDamping( + dynamics::Joint* joint, + neural::WrtDampingJointEntryType type, + Eigen::VectorXi dofs_index, + Eigen::VectorXs upperBound, + Eigen::VectorXs lowerBound) +{ + mWrtDamping->registerJoint(joint, type, dofs_index, upperBound, lowerBound); +} + +void World::tuneSpring( + dynamics::Joint* joint, + neural::WrtSpringJointEntryType type, + Eigen::VectorXi dofs_index, + Eigen::VectorXs upperBound, + Eigen::VectorXs lowerBound) +{ + mWrtSpring->registerJoint(joint, type, dofs_index, upperBound, lowerBound); +} + //============================================================================== std::size_t World::getNumBodyNodes() { @@ -1050,6 +1134,26 @@ Eigen::VectorXs World::getMasses() return mWrtMass->get(this); } +Eigen::VectorXs World::getDampings() +{ + return mWrtDamping->get(this); +} + +Eigen::VectorXs World::getSprings() +{ + return mWrtSpring->get(this); +} + +Eigen::VectorXi World::getDampingDofsMapping() +{ + return mWrtDamping->getDofsMapping(this); +} + +Eigen::VectorXi World::getSpringDofsMapping() +{ + return mWrtSpring->getDofsMapping(this); +} + size_t World::getLinkMassesDims() { size_t mass_dim = 0; @@ -1099,6 +1203,19 @@ Eigen::VectorXs World::getLinkCOMs() return coms; } +Eigen::VectorXs World::getLinkDiagIs() +{ + Eigen::VectorXs diag_Is = Eigen::VectorXs::Zero(3*getLinkMassesDims()); + size_t cursor = 0; + for(size_t i = 0; i < mSkeletons.size(); i++) + { + Eigen::VectorXs skel_diag_Is = mSkeletons[i]->getLinkDiagIs(); + diag_Is.segment(cursor,skel_diag_Is.size()); + cursor += skel_diag_Is.size(); + } + return diag_Is; +} + Eigen::VectorXs World::getLinkMOIs() { Eigen::VectorXs mois = Eigen::VectorXs::Zero(6 * getLinkMassesDims()); @@ -1160,6 +1277,23 @@ Eigen::Vector3s World::getLinkCOMIndex(size_t index) return mSkeletons[skeleton_id]->getLinkCOMIndex(index - probe); } +Eigen::Vector3s World::getLinkDiagIIndex(size_t index) +{ + size_t probe = 0; + size_t skeleton_id = 0; + for(size_t i=0;igetNumBodyNodes(); + if(index < probe) + { + skeleton_id = i; + probe -= mSkeletons[i]->getNumBodyNodes(); + break; + } + } + return mSkeletons[skeleton_id]->getLinkDiagIIndex(index-probe); +} + Eigen::Vector6s World::getLinkMOIIndex(size_t index) { size_t probe = 0; @@ -1177,6 +1311,69 @@ Eigen::Vector6s World::getLinkMOIIndex(size_t index) return mSkeletons[skeleton_id]->getLinkMOIIndex(index - probe); } +Eigen::MatrixXs World::getLinkAkMatrixIndex(size_t index) +{ + size_t probe = 0; + size_t skeleton_id = 0; + for(size_t i = 0; i < mSkeletons.size(); i++) + { + probe += mSkeletons[i]->getNumBodyNodes(); + if(index < probe) + { + skeleton_id = i; + probe -= mSkeletons[i]->getNumBodyNodes(); + break; + } + } + return mSkeletons[skeleton_id]->getLinkAkMatrixIndex(index-probe); +} + +Eigen::VectorXs World::getJointDampingCoeffs() +{ + Eigen::VectorXs damp_coeffs = Eigen::VectorXs::Zero(getNumDofs()); + size_t cur = 0; + for(size_t i = 0; i < mSkeletons.size(); i++) + { + size_t mdim = mSkeletons[i]->getNumDofs(); + damp_coeffs.segment(cur, mdim) = mSkeletons[i]->getDampingCoeffVector(); + cur += mdim; + } + return damp_coeffs; +} + +Eigen::VectorXs World::getJointDampingCoeffIndex(size_t index) +{ + Eigen::VectorXs damp_coeff = Eigen::VectorXs::Zero(getJointIndex(index)->getNumDofs()); + for(int i = 0; i < damp_coeff.size(); i++) + { + damp_coeff(i) = getJointIndex(index)->getDampingCoefficient(i); + } + return damp_coeff; +} + +Eigen::VectorXs World::getJointSpringStiffs() +{ + Eigen::VectorXs spring_stiffs = Eigen::VectorXs::Zero(getNumDofs()); + size_t cur = 0; + for(size_t i = 0; i < mSkeletons.size(); i++) + { + size_t mdim = mSkeletons[i]->getNumDofs(); + spring_stiffs.segment(cur, mdim) = mSkeletons[i]->getSpringStiffVector(); + cur += mdim; + } + return spring_stiffs; +} + +Eigen::VectorXs World::getJointSpringStiffIndex(size_t index) +{ + Eigen::VectorXs spring_stiff = Eigen::VectorXs::Zero(getJointIndex(index)->getNumDofs()); + for(int i = 0; i < spring_stiff.size(); i++) + { + spring_stiff(i) = getJointIndex(index)->getSpringStiffness(i); + } + return spring_stiff; +} + //============================================================================== Eigen::VectorXs World::getPositions() { @@ -1333,6 +1530,29 @@ Eigen::VectorXs World::getMassLowerLimits() return mWrtMass->lowerBound(this); } +//============================================================================== +Eigen::VectorXs World::getDampingUpperLimits() +{ + return mWrtDamping->upperBound(this); +} + +Eigen::VectorXs World::getDampingLowerLimits() +{ + return mWrtDamping->lowerBound(this); +} + +//============================================================================== +Eigen::VectorXs World::getSpringUpperLimits() +{ + return mWrtSpring->upperBound(this); +} + +Eigen::VectorXs World::getSpringLowerLimits() +{ + return mWrtSpring->lowerBound(this); +} + + //============================================================================== void World::setPositions(Eigen::VectorXs position) { @@ -1460,6 +1680,16 @@ void World::setMasses(Eigen::VectorXs masses) mWrtMass->set(this, masses); } +void World::setDampings(Eigen::VectorXs dampings) +{ + mWrtDamping->set(this, dampings); +} + +void World::setSprings(Eigen::VectorXs springs) +{ + mWrtSpring->set(this, springs); +} + void World::setLinkMasses(Eigen::VectorXs masses) { assert(masses.size() == getLinkMassesDims()); @@ -1472,6 +1702,48 @@ void World::setLinkMasses(Eigen::VectorXs masses) } } +void World::setJointDampingCoeffs(Eigen::VectorXs damp_coeffs) +{ + assert(damp_coeffs.size() == getNumDofs()); + size_t cur = 0; + for(size_t i = 0; i < mSkeletons.size(); i++) + { + size_t mdim = mSkeletons[i]->getNumDofs(); + mSkeletons[i]->setDampingCoeffVector(damp_coeffs.segment(cur, mdim)); + cur += mdim; + } +} + +void World::setJointDampingCoeffIndex(Eigen::VectorXs damp_coeff, size_t index) +{ + assert(damp_coeff.size() == getJointIndex(index)->getNumDofs()); + for(int i = 0; i < damp_coeff.size(); i++) + { + getJointIndex(index)->setDampingCoefficient(i, damp_coeff(i)); + } +} + +void World::setJointSpringStiffs(Eigen::VectorXs spring_stiffs) +{ + assert(spring_stiffs.size() == getNumDofs()); + size_t cur = 0; + for(size_t i = 0; i < mSkeletons.size(); i++) + { + size_t mdim = mSkeletons[i]->getNumDofs(); + mSkeletons[i]->setSpringStiffVector(spring_stiffs.segment(cur, mdim)); + cur += mdim; + } +} + +void World::setJointSpringStiffIndex(Eigen::VectorXs spring_stiff, size_t index) +{ + assert(spring_stiff.size() == getJointIndex(index)->getNumDofs()); + for(int i = 0; i < spring_stiff.size(); i++) + { + getJointIndex(index)->setSpringStiffness(i, spring_stiff(i)); + } +} + void World::setLinkMassIndex(s_t mass, size_t index) { Eigen::VectorXs masses = getLinkMasses(); @@ -1546,6 +1818,35 @@ void World::setLinkCOMs(Eigen::VectorXs coms) } } +void World::setLinkDiagIIndex(Eigen::Vector3s diag_I, size_t index) +{ + size_t probe = 0; + size_t skeleton_id = 0; + for(size_t i=0;igetNumBodyNodes(); + if(index < probe) + { + skeleton_id = i; + probe -= mSkeletons[i]->getNumBodyNodes(); + break; + } + } + mSkeletons[skeleton_id]->setLinkDiagIIndex(diag_I,index-probe); +} + +void World::setLinkDiagIs(Eigen::VectorXs diag_Is) +{ + assert(diag_Is.size() == getLinkMassesDims()*3); + size_t cursor = 0; + for(size_t i = 0; i < mSkeletons.size(); i++) + { + size_t dim = mSkeletons[i]->getLinkMassesDims()*3; + mSkeletons[i]->setLinkDiagIs(diag_Is.segment(cursor,dim)); + cursor += dim; + } +} + void World::setLinkMOIIndex(Eigen::Vector6s moi, size_t index) { size_t probe = 0; @@ -1859,6 +2160,22 @@ Eigen::MatrixXs World::getStateJacobian() return stateJac; } +//=============================================================================== +Eigen::MatrixXs World::getContactFreeStateJacobian() +{ + std::shared_ptr snapshot + = getCachedBackpropSnapshot(); + int dofs = getNumDofs(); + Eigen::MatrixXs stateJac = Eigen::MatrixXs::Zero(2 * dofs, 2 * dofs); + WorldPtr sharedThis = shared_from_this(); + stateJac.block(0, 0, dofs, dofs) = snapshot->getContactFreePosPosJacobian(sharedThis); + stateJac.block(dofs, 0, dofs, dofs) = snapshot->getContactFreePosVelJacobian(sharedThis); + stateJac.block(0, dofs, dofs, dofs) = snapshot->getContactFreeVelPosJacobian(sharedThis); + stateJac.block(dofs, dofs, dofs, dofs) + = snapshot->getContactFreeVelVelJacobian(sharedThis); + return stateJac; +} + //============================================================================== // This returns the Jacobian for action_t -> state_{t+1}. Eigen::MatrixXs World::getActionJacobian() @@ -1879,6 +2196,76 @@ Eigen::MatrixXs World::getActionJacobian() return actionJac; } +//============================================================================== +// This map force to action space +Eigen::MatrixXs World::mapToActionSpace(Eigen::MatrixXs forces) +{ + size_t steps = forces.cols(); + Eigen::MatrixXs actions = Eigen::MatrixXs::Zero(mActionSpace.size(), steps); + for(size_t i = 0; i < steps; i++) + { + for(size_t j = 0; j < mActionSpace.size();j++) + { + actions(j,i) = forces(mActionSpace[j],i); + } + } + return actions; +} + +Eigen::VectorXs World::mapToActionSpaceVector(Eigen::VectorXs force) +{ + Eigen::VectorXs action = Eigen::VectorXs::Zero(mActionSpace.size()); + for(size_t i = 0; i < mActionSpace.size(); i++) + { + action(i) = force(mActionSpace[i]); + } + return action; +} + +//============================================================================== +// This map force to action space +Eigen::MatrixXs World::mapToForceSpace(Eigen::MatrixXs actions) +{ + size_t steps = actions.cols(); + Eigen::MatrixXs forces = Eigen::MatrixXs::Zero(getNumDofs(), steps); + for(size_t i = 0; i < steps; i++) + { + for(size_t j = 0; j < mActionSpace.size(); j++) + { + forces(mActionSpace[j],i) = actions(j, i); + } + } + return forces; +} + +Eigen::VectorXs World::mapToForceSpaceVector(Eigen::VectorXs action) +{ + Eigen::VectorXs force = Eigen::VectorXs::Zero(getNumDofs()); + for(size_t i = 0; i < mActionSpace.size();i++) + { + force(mActionSpace[i]) = action(i); + } + return force; +} + +Eigen::MatrixXs World::getContactFreeActionJacobian() +{ + std::shared_ptr snapshot + = getCachedBackpropSnapshot(); + int dofs = getNumDofs(); + WorldPtr sharedThis = shared_from_this(); + const Eigen::MatrixXs& forceVelJac + = snapshot->getContactFreeControlForceVelJacobian(sharedThis); + + int actionDim = mActionSpace.size(); + Eigen::MatrixXs actionJac = Eigen::MatrixXs::Zero(2 * dofs, actionDim); + for (int i = 0; i < actionDim; i++) + { + actionJac.block(dofs, i, dofs, 1) = forceVelJac.col(mActionSpace[i]); + } + return actionJac; +} + //============================================================================== Eigen::MatrixXs World::finiteDifferenceStateJacobian() { diff --git a/dart/simulation/World.hpp b/dart/simulation/World.hpp index 4a0456efa..128daf783 100644 --- a/dart/simulation/World.hpp +++ b/dart/simulation/World.hpp @@ -54,6 +54,8 @@ #include "dart/dynamics/SimpleFrame.hpp" #include "dart/dynamics/Skeleton.hpp" #include "dart/neural/WithRespectToMass.hpp" +#include "dart/neural/WithRespectToDamping.hpp" +#include "dart/neural/WithRespectToSpring.hpp" #include "dart/simulation/Recording.hpp" #include "dart/simulation/SmartPointer.hpp" @@ -78,6 +80,8 @@ class CollisionResult; namespace neural { class WithRespectToMass; +class WithRespectToDamping; +class WithRespectToSpring; class BackpropSnapshot; } // namespace neural @@ -162,6 +166,11 @@ class World : public virtual common::Subject, dynamics::BodyNode* getBodyNodeByIndex(size_t index); + dynamics::Joint* getJointIndex(size_t index); + + /// Get rest position of a particular dof + s_t getRestPositionIndex(size_t index); + /// Get the number of skeletons std::size_t getNumSkeletons() const; @@ -214,9 +223,18 @@ class World : public virtual common::Subject, /// Returns the size of the getMasses() vector std::size_t getMassDims(); + /// Returns the size of the getDampings() vector + std::size_t getDampingDims(); + + std::size_t getSpringDims(); + /// This will prevent mass from being tuned void clearTunableMassThisInstance(); + void clearTunableDampingThisInstance(); + + void clearTunableSpringThisInstance(); + /// This registers that we'd like to keep track of this BodyNode's mass in a /// specified way in differentiation void tuneMass( @@ -225,6 +243,19 @@ class World : public virtual common::Subject, Eigen::VectorXs upperBound, Eigen::VectorXs lowerBound); + void tuneDamping( + dynamics::Joint* joint, + neural::WrtDampingJointEntryType type, + Eigen::VectorXi dofs_index, + Eigen::VectorXs upperBound, + Eigen::VectorXs lowerBound); + + void tuneSpring(dynamics::Joint* joint, + neural::WrtSpringJointEntryType type, + Eigen::VectorXi dofs_index, + Eigen::VectorXs upperBound, + Eigen::VectorXs lowerBound); + /// Returns the size of the getLinkMasses() vector std::size_t getNumBodyNodes(); @@ -248,7 +279,15 @@ class World : public virtual common::Subject, /// single vector Eigen::VectorXs getMasses(); - // Eigen::VectorXs getLinkMasses(); + Eigen::VectorXs getDampings(); + + Eigen::VectorXs getSprings(); + + Eigen::VectorXi getDampingDofsMapping(); + + Eigen::VectorXi getSpringDofsMapping(); + //Eigen::VectorXs getLinkMasses(); + size_t getLinkMassesDims(); @@ -284,6 +323,22 @@ class World : public virtual common::Subject, // this world Eigen::VectorXs getMassLowerLimits(); + // This gives the vector of mass upper limits for all the registered bodies in + // this world + Eigen::VectorXs getDampingUpperLimits(); + + // This gives the vector of mass lower limits for all the registered bodies in + // this world + Eigen::VectorXs getDampingLowerLimits(); + + // This gives the vector of spring coeff lower limits for all the registered bodies in + // this world + Eigen::VectorXs getSpringLowerLimits(); + + // This gives the vector of spring coeff upper limits for all the registered bodies in + // this world + Eigen::VectorXs getSpringUpperLimits(); + // This gets all the inertia matrices for all the links in all the skeletons // in the world mapped into a flat vector. @@ -307,12 +362,26 @@ class World : public virtual common::Subject, Eigen::Vector3s getLinkBetaIndex(size_t index); + Eigen::MatrixXs getLinkAkMatrixIndex(size_t index); + + Eigen::VectorXs getLinkDiagIs(); + + Eigen::Vector3s getLinkDiagIIndex(size_t index); + // This returns a vector of all the link masses for all the skeletons in the // world concatenated into a flat vector. Eigen::VectorXs getLinkMasses(); s_t getLinkMassIndex(size_t index); + Eigen::VectorXs getJointDampingCoeffs(); + + Eigen::VectorXs getJointDampingCoeffIndex(size_t index); + + Eigen::VectorXs getJointSpringStiffs(); + + Eigen::VectorXs getJointSpringStiffIndex(size_t index); + /// Sets the position of all the skeletons in the world from a single /// concatenated state vector void setPositions(Eigen::VectorXs position); @@ -350,6 +419,18 @@ class World : public virtual common::Subject, // This sets all the masses for all the registered bodies in the world void setMasses(Eigen::VectorXs masses); + void setDampings(Eigen::VectorXs dampings); + + void setSprings(Eigen::VectorXs springs); + + void setJointDampingCoeffs(Eigen::VectorXs damp_coeffs); + + void setJointDampingCoeffIndex(Eigen::VectorXs damp_coeff, size_t index); + + void setJointSpringStiffs(Eigen::VectorXs spring_coeffs); + + void setJointSpringStiffIndex(Eigen::VectorXs spring_stiff, size_t index); + void setLinkMasses(Eigen::VectorXs masses); void setLinkMassIndex(s_t mass, size_t index); @@ -364,12 +445,16 @@ class World : public virtual common::Subject, void setLinkBetas(Eigen::VectorXs betas); + void setLinkDiagIs(Eigen::VectorXs diag_Is); + void setLinkBetaIndex(Eigen::Vector3s beta, size_t index); void setLinkCOMIndex(Eigen::Vector3s com, size_t index); void setLinkMOIIndex(Eigen::Vector6s com, size_t index); + void setLinkDiagIIndex(Eigen::Vector3s diag_I, size_t index); + /// This gives the C(pos, vel) vector for all the skeletons in the world, /// without accounting for the external forces Eigen::VectorXs getCoriolisAndGravityForces(); @@ -434,9 +519,22 @@ class World : public virtual common::Subject, // This returns the Jacobian for state_t -> state_{t+1}. Eigen::MatrixXs getStateJacobian(); + + Eigen::MatrixXs getContactFreeStateJacobian(); // This returns the Jacobian for action_t -> state_{t+1}. Eigen::MatrixXs getActionJacobian(); + // This function map any forces to actions + Eigen::MatrixXs mapToActionSpace(Eigen::MatrixXs forces); + + Eigen::VectorXs mapToActionSpaceVector(Eigen::VectorXs force); + + // This function map any actions to to full dof control forces + Eigen::MatrixXs mapToForceSpace(Eigen::MatrixXs actions); + + Eigen::VectorXs mapToForceSpaceVector(Eigen::VectorXs action); + Eigen::MatrixXs getContactFreeActionJacobian(); + Eigen::MatrixXs finiteDifferenceStateJacobian(); Eigen::MatrixXs finiteDifferenceActionJacobian(); @@ -594,6 +692,10 @@ class World : public virtual common::Subject, /// the world need gradients through which kinds of mass. std::shared_ptr getWrtMass(); + std::shared_ptr getWrtDamping(); + + std::shared_ptr getWrtSpring(); + /// This returns the world state as a JSON blob that we can render std::string toJson(); @@ -754,6 +856,10 @@ class World : public virtual common::Subject, std::shared_ptr mWrtMass; + std::shared_ptr mWrtDamping; + + std::shared_ptr mWrtSpring; + //-------------------------------------------------------------------------- // High-level RL-style API //-------------------------------------------------------------------------- diff --git a/dart/trajectory/LossFn.cpp b/dart/trajectory/LossFn.cpp index 153a2405a..2d0d36ce2 100644 --- a/dart/trajectory/LossFn.cpp +++ b/dart/trajectory/LossFn.cpp @@ -109,6 +109,31 @@ s_t LossFn::getLossAndGradient( gradWrtRollout->getMasses()(i) = (lossPos - lossNeg) / (2 * EPS); } + for(int i = 0; i < rolloutCopy.getDampings().size(); i++) + { + rolloutCopy.getDampings()(i) += EPS; + s_t lossPos = mLoss.value()(&rolloutCopy); + rolloutCopy.getDampings()(i) -= EPS; + + rolloutCopy.getDampings()(i) -= EPS; + s_t lossNeg = mLoss.value()(&rolloutCopy); + rolloutCopy.getDampings()(i) += EPS; + + gradWrtRollout->getDampings()(i) = (lossPos - lossNeg) / (2 * EPS); + } + for(int i = 0; i < rolloutCopy.getSprings().size(); i++) + { + rolloutCopy.getSprings()(i) += EPS; + s_t lossPos = mLoss.value()(&rolloutCopy); + rolloutCopy.getSprings()(i) -= EPS; + + rolloutCopy.getSprings()(i) -= EPS; + s_t lossNeg = mLoss.value()(&rolloutCopy); + rolloutCopy.getSprings()(i) += EPS; + + gradWrtRollout->getSprings()(i) = (lossPos - lossNeg) / (2 * EPS); + } + for (std::string key : rolloutCopy.getMappings()) { diff --git a/dart/trajectory/MultiShot.cpp b/dart/trajectory/MultiShot.cpp index 11a11a374..7ae952c74 100644 --- a/dart/trajectory/MultiShot.cpp +++ b/dart/trajectory/MultiShot.cpp @@ -989,6 +989,8 @@ void MultiShot::getStates( } assert(cursor == mSteps); rollout->getMasses() = world->getMasses(); + rollout->getDampings() = world->getDampings(); + rollout->getSprings() = world->getSprings(); for (auto pair : mMetadata) { rollout->setMetadata(pair.first, pair.second); @@ -1299,7 +1301,9 @@ void MultiShot::backpropGradientWrt( // Don't s_t-count direct gradients wrt mass, so subtract out duplicates // here. We've added mShots.size() copies of the direct masses grad, so // subtract out mShots.size() - 1, leaving exactly 1 copy. - gradStatic -= gradWrtRollout->getMassesConst() * (mShots.size() - 1); + gradStatic.segment(0, getMassDims()) -= gradWrtRollout->getMassesConst() * (mShots.size() - 1); + gradStatic.segment(getMassDims(), getDampingDims()) -= gradWrtRollout->getDampingsConst() * (mShots.size() - 1); + gradStatic.segment(getMassDims()+getDampingDims(), getSpringDims()) -= gradWrtRollout->getSpringsConst() * (mShots.size() - 1); #ifdef LOG_PERFORMANCE_MULTI_SHOT if (thisLog != nullptr) diff --git a/dart/trajectory/Problem.cpp b/dart/trajectory/Problem.cpp index 0025224b1..3009c03c1 100644 --- a/dart/trajectory/Problem.cpp +++ b/dart/trajectory/Problem.cpp @@ -145,7 +145,7 @@ int Problem::getFlatProblemDim(std::shared_ptr world) const int Problem::getFlatStaticProblemDim( std::shared_ptr world) const { - return world->getMassDims(); + return world->getMassDims() + world->getDampingDims() + world->getSpringDims(); } //============================================================================== @@ -174,6 +174,8 @@ void Problem::flatten( #endif flatStatic.segment(0, world->getMassDims()) = world->getMasses(); + flatStatic.segment(world->getMassDims(), world->getDampingDims()) = world->getDampings(); + flatStatic.segment(world->getMassDims()+world->getDampingDims(), world->getSpringDims()) = world->getSprings(); #ifdef LOG_PERFORMANCE_PROBLEM if (thisLog != nullptr) @@ -218,6 +220,8 @@ void Problem::unflatten( #endif world->setMasses(flatStatic.segment(0, world->getMassDims())); + world->setDampings(flatStatic.segment(world->getMassDims(), world->getDampingDims())); + world->setSprings(flatStatic.segment(world->getMassDims() + world->getDampingDims(), world->getSpringDims())); #ifdef LOG_PERFORMANCE_PROBLEM if (thisLog != nullptr) @@ -297,6 +301,8 @@ void Problem::getUpperBounds( #endif flatStatic.segment(0, world->getMassDims()) = world->getMassUpperLimits(); + flatStatic.segment(world->getMassDims(), world->getDampingDims()) = world->getDampingUpperLimits(); + flatStatic.segment(world->getMassDims()+world->getDampingDims(), world->getSpringDims()) = world->getSpringUpperLimits(); #ifdef LOG_PERFORMANCE_PROBLEM if (thisLog != nullptr) @@ -326,6 +332,9 @@ void Problem::getLowerBounds( #endif flatStatic.segment(0, world->getMassDims()) = world->getMassLowerLimits(); + // std::cout << "Lower Limit: " << world->getMassLowerLimits() << std::endl; + flatStatic.segment(world->getMassDims(), world->getDampingDims()) = world->getDampingLowerLimits(); + flatStatic.segment(world->getMassDims() + world->getDampingDims(), world->getSpringDims()) = world->getSpringLowerLimits(); #ifdef LOG_PERFORMANCE_PROBLEM if (thisLog != nullptr) @@ -355,6 +364,8 @@ void Problem::getInitialGuess( #endif flatStatic.segment(0, world->getMassDims()) = world->getMasses(); + flatStatic.segment(world->getMassDims(), world->getDampingDims()) = world->getDampings(); + flatStatic.segment(world->getMassDims() + world->getDampingDims(), world->getSpringDims()) = world->getSprings(); #ifdef LOG_PERFORMANCE_PROBLEM if (thisLog != nullptr) @@ -845,6 +856,8 @@ void Problem::initializeStaticGradient( #endif gradStatic.segment(0, world->getMassDims()).setZero(); + gradStatic.segment(world->getMassDims(), world->getDampingDims()).setZero(); + gradStatic.segment(world->getMassDims() + world->getDampingDims(), world->getSpringDims()).setZero(); #ifdef LOG_PERFORMANCE_PROBLEM if (thisLog != nullptr) @@ -872,6 +885,8 @@ void Problem::accumulateStaticGradient( #endif gradStatic.segment(0, world->getMassDims()) += thisTimestep.lossWrtMass; + gradStatic.segment(world->getMassDims(), world->getDampingDims()) += thisTimestep.lossWrtDamping; + gradStatic.segment(world->getMassDims()+world->getDampingDims(), world->getSpringDims()) += thisTimestep.lossWrtSpring; #ifdef LOG_PERFORMANCE_PROBLEM if (thisLog != nullptr) @@ -925,16 +940,40 @@ void Problem::accumulateStaticJacobianOfFinalState( thisLog = log->startRun("Problem.accumulateStaticJacobianOfFinalState"); } #endif - + // TODO: Eric This is a bit tricky, need to check dimensions jacStatic.block( 0, 0, thisTimestep.massPos.rows(), thisTimestep.massPos.cols()) += thisTimestep.massPos; jacStatic.block( - thisTimestep.massPos.rows(), + thisTimestep.massPos.rows(), + 0, + thisTimestep.dampPos.rows(), + thisTimestep.dampPos.cols()) + += thisTimestep.dampPos; + jacStatic.block( + thisTimestep.massPos.rows() + thisTimestep.dampPos.rows(), + 0, + thisTimestep.springPos.rows(), + thisTimestep.springPos.cols()) + += thisTimestep.springPos; + jacStatic.block( + thisTimestep.massPos.rows() + thisTimestep.dampPos.rows() + thisTimestep.springPos.rows(), 0, thisTimestep.massVel.rows(), thisTimestep.massVel.cols()) += thisTimestep.massVel; + jacStatic.block( + thisTimestep.massPos.rows()+thisTimestep.dampPos.rows()+thisTimestep.springPos.rows()+thisTimestep.massVel.rows(), + 0, + thisTimestep.dampVel.rows(), + thisTimestep.dampVel.cols()) + += thisTimestep.dampVel; + jacStatic.block( + thisTimestep.massPos.rows()+thisTimestep.dampPos.rows()+thisTimestep.springPos.rows()+thisTimestep.massVel.rows()+thisTimestep.dampVel.rows(), + 0, + thisTimestep.springVel.rows(), + thisTimestep.springVel.cols()) + += thisTimestep.springVel; #ifdef LOG_PERFORMANCE_PROBLEM if (thisLog != nullptr) @@ -1111,6 +1150,16 @@ int Problem::getMassDims() return mWorld->getMassDims(); } +int Problem::getDampingDims() +{ + return mWorld->getDampingDims(); +} + +int Problem::getSpringDims() +{ + return mWorld->getSpringDims(); +} + //============================================================================== /// This gets the total number of non-zero entries in the Jacobian int Problem::getNumberNonZeroJacobian(std::shared_ptr world) diff --git a/dart/trajectory/Problem.hpp b/dart/trajectory/Problem.hpp index 6b35c0cbc..717f233c0 100644 --- a/dart/trajectory/Problem.hpp +++ b/dart/trajectory/Problem.hpp @@ -263,6 +263,12 @@ class Problem /// Returns the dimension of the mass vector int getMassDims(); + /// Returns the dimension of the damping vector + int getDampingDims(); + + /// Returns the dimension of the spring vector + int getSpringDims(); + /// This returns the debugging name of a given DOF virtual std::string getFlatDimName( std::shared_ptr world, int dim) diff --git a/dart/trajectory/SingleShot.cpp b/dart/trajectory/SingleShot.cpp index 72717dbc6..2623fb853 100644 --- a/dart/trajectory/SingleShot.cpp +++ b/dart/trajectory/SingleShot.cpp @@ -425,6 +425,10 @@ void SingleShot::backpropJacobianOfFinalState( // this last const Eigen::MatrixXs& massVel = ptr->getMassVelJacobian(world, thisLog); + const Eigen::MatrixXs& dampingVel = ptr->getDampingVelJacobian(world, thisLog); + + const Eigen::MatrixXs& springVel = ptr->getSpringVelJacobian(world, thisLog); + // p_end <- f_t = p_end <- v_t+1 * v_t+1 <- f_t thisTimestep.forcePos = last.velPos * forceVel; // v_end <- f_t = v_end <- v_t+1 * v_t+1 <- f_t @@ -433,6 +437,15 @@ void SingleShot::backpropJacobianOfFinalState( thisTimestep.massPos = last.velPos * massVel; // v_end <- m_t = v_end <- v_t+1 * v_t+1 <- m_t thisTimestep.massVel = last.velVel * massVel; + // v_end <- d_t = v_end <- v_t+1 * v_t+1 <- d_t + thisTimestep.dampPos = last.velPos * dampingVel; + + thisTimestep.dampVel = last.velVel * dampingVel; + + thisTimestep.springPos = last.velPos * springVel; + + thisTimestep.springVel = last.velVel * springVel; + // p_end <- v_t = (p_end <- p_t+1 * p_t+1 <- v_t) + (p_end <- v_t+1 * v_t+1 // <- v_t) thisTimestep.velPos = last.posPos * velPos + last.velPos * velVel; @@ -554,7 +567,9 @@ void SingleShot::backpropGradientWrt( Problem::initializeStaticGradient(world, gradStatic, thisLog); // Add any gradient we have from the loss wrt mass directly into our gradient, // cause we don't need to do any extra processing on that. - gradStatic += gradWrtRollout->getMassesConst(); + gradStatic.segment(0, getMassDims()) += gradWrtRollout->getMassesConst(); + gradStatic.segment(getMassDims(), getDampingDims()) += gradWrtRollout->getDampingsConst(); + gradStatic.segment(getMassDims()+getDampingDims(), getSpringDims()) += gradWrtRollout->getSpringsConst(); int staticDims = getFlatStaticProblemDim(world); int dynamicDims = getFlatDynamicProblemDim(world); @@ -588,6 +603,8 @@ void SingleShot::backpropGradientWrt( mappedGrad.lossWrtTorque = gradWrtRollout->getControlForcesConst(pair.first).col(i); mappedGrad.lossWrtMass = gradWrtRollout->getMassesConst(); + mappedGrad.lossWrtDamping = gradWrtRollout->getDampingsConst(); + mappedGrad.lossWrtSpring = gradWrtRollout->getSpringsConst(); mappedLosses[pair.first] = mappedGrad; } @@ -720,7 +737,11 @@ void SingleShot::getStates( } } assert(rollout->getMasses().size() == world->getMassDims()); + assert(rollout->getDampings().size() == world->getDampingDims()); + assert(rollout->getSprings().size() == world->getSpringDims()); rollout->getMasses() = world->getMasses(); + rollout->getDampings() = world->getDampings(); + rollout->getSprings() = world->getSprings(); for (auto pair : mMetadata) { rollout->setMetadata(pair.first, pair.second); @@ -753,6 +774,8 @@ void SingleShot::setStates( mStartVel = rollout->getVelsConst().col(0); mForces = rollout->getControlForcesConst(); world->setMasses(rollout->getMassesConst()); + world->setDampings(rollout->getDampingsConst()); + world->setSprings(rollout->getSpringsConst()); #ifdef LOG_PERFORMANCE_SINGLE_SHOT if (thisLog != nullptr) diff --git a/dart/trajectory/TrajectoryConstants.hpp b/dart/trajectory/TrajectoryConstants.hpp index 6f0fc9612..091d55593 100644 --- a/dart/trajectory/TrajectoryConstants.hpp +++ b/dart/trajectory/TrajectoryConstants.hpp @@ -18,6 +18,10 @@ struct TimestepJacobians Eigen::MatrixXs velVel; Eigen::MatrixXs forceVel; Eigen::MatrixXs massVel; + Eigen::MatrixXs dampVel; + Eigen::MatrixXs dampPos; + Eigen::MatrixXs springVel; + Eigen::MatrixXs springPos; }; } // namespace trajectory diff --git a/dart/trajectory/TrajectoryRollout.cpp b/dart/trajectory/TrajectoryRollout.cpp index 21901236e..9b3d3280c 100644 --- a/dart/trajectory/TrajectoryRollout.cpp +++ b/dart/trajectory/TrajectoryRollout.cpp @@ -134,6 +134,8 @@ void TrajectoryRollout::serialize(proto::TrajectoryRollout& proto) const (*proto.mutable_force())[mapping], getControlForcesConst(mapping)); } proto::serializeVector(*proto.mutable_mass(), getMassesConst()); + proto::serializeVector(*proto.mutable_damping(), getDampingsConst()); + proto::serializeVector(*proto.mutable_spring(), getSpringsConst()); for (auto pair : getMetadataMap()) { proto::serializeMatrix( @@ -162,6 +164,8 @@ TrajectoryRolloutReal TrajectoryRollout::deserialize( force[pair.first] = proto::deserializeMatrix(pair.second); } Eigen::VectorXs mass = proto::deserializeVector(proto.mass()); + Eigen::VectorXs damping = proto::deserializeVector(proto.damping()); + Eigen::VectorXs spring = proto::deserializeVector(proto.spring()); std::unordered_map metadata; for (auto pair : proto.metadata()) { @@ -169,7 +173,7 @@ TrajectoryRolloutReal TrajectoryRollout::deserialize( } TrajectoryRolloutReal recovered - = TrajectoryRolloutReal(pos, vel, force, mass, metadata); + = TrajectoryRolloutReal(pos, vel, force, mass, damping, spring, metadata); return recovered; } @@ -209,9 +213,11 @@ TrajectoryRolloutReal TrajectoryRollout::fromForces( std::unordered_map force; force["identity"] = forceMatrix; Eigen::VectorXs mass = world->getMasses(); + Eigen::VectorXs damping = world->getDampings(); + Eigen::VectorXs spring = world->getSprings(); std::unordered_map metadata; - return TrajectoryRolloutReal(pos, vel, force, mass, metadata); + return TrajectoryRolloutReal(pos, vel, force, mass, damping, spring, metadata); } //============================================================================== @@ -238,9 +244,11 @@ TrajectoryRolloutReal TrajectoryRollout::fromPoses( std::unordered_map force; force["identity"] = forceMatrix; Eigen::VectorXs mass = world->getMasses(); + Eigen::VectorXs damping = world->getDampings(); + Eigen::VectorXs spring = world->getSprings(); std::unordered_map metadata; - return TrajectoryRolloutReal(pos, vel, force, mass, metadata); + return TrajectoryRolloutReal(pos, vel, force, mass, damping, spring, metadata); } //============================================================================== @@ -249,6 +257,8 @@ TrajectoryRolloutReal::TrajectoryRolloutReal( mappings, int steps, int massDim, + int dampingDim, + int springDim, const std::unordered_map metadata) : mMetadata(metadata) { @@ -261,6 +271,8 @@ TrajectoryRolloutReal::TrajectoryRolloutReal( mMappings.push_back(pair.first); } mMasses = Eigen::VectorXs::Zero(massDim); + mDampings = Eigen::VectorXs::Zero(dampingDim); + mSprings = Eigen::VectorXs::Zero(springDim); } //============================================================================== @@ -269,6 +281,8 @@ TrajectoryRolloutReal::TrajectoryRolloutReal(Problem* shot) shot->getMappings(), shot->getNumSteps(), shot->getMassDims(), + shot->getDampingDims(), + shot->getSpringDims(), shot->getMetadataMap()) { } @@ -280,8 +294,12 @@ TrajectoryRolloutReal::TrajectoryRolloutReal( const std::unordered_map vel, const std::unordered_map force, const Eigen::VectorXs mass, + const Eigen::VectorXs damping, + const Eigen::VectorXs spring, const std::unordered_map metadata) - : mMasses(mass) + : mMasses(mass), + mDampings(damping), + mSprings(spring) { for (auto pair : pos) { @@ -317,6 +335,8 @@ TrajectoryRolloutReal::TrajectoryRolloutReal(const TrajectoryRollout* copy) mForces[key] = copy->getControlForcesConst(key); } mMasses = copy->getMassesConst(); + mDampings = copy->getDampingsConst(); + mSprings = copy->getSpringsConst(); mMetadata = copy->getMetadataMap(); } @@ -347,6 +367,18 @@ Eigen::Ref TrajectoryRolloutReal::getMasses() return mMasses; } +//============================================================================== +Eigen::Ref TrajectoryRolloutReal::getDampings() +{ + return mDampings; +} + +//============================================================================== +Eigen::Ref TrajectoryRolloutReal::getSprings() +{ + return mSprings; +} + //============================================================================== const Eigen::Ref TrajectoryRolloutReal::getPosesConst( const std::string& mapping) const @@ -375,6 +407,20 @@ const Eigen::Ref TrajectoryRolloutReal::getMassesConst() return mMasses; } +//============================================================================== +const Eigen::Ref TrajectoryRolloutReal::getDampingsConst() + const +{ + return mDampings; +} + +//============================================================================== +const Eigen::Ref TrajectoryRolloutReal::getSpringsConst() + const +{ + return mSprings; +} + //============================================================================== const std::unordered_map& TrajectoryRolloutReal::getMetadataMap() const @@ -449,6 +495,18 @@ Eigen::Ref TrajectoryRolloutRef::getMasses() return mToSlice->getMasses(); } +//============================================================================== +Eigen::Ref TrajectoryRolloutRef::getDampings() +{ + return mToSlice->getDampings(); +} + +//============================================================================== +Eigen::Ref TrajectoryRolloutRef::getSprings() +{ + return mToSlice->getSprings(); +} + //============================================================================== const Eigen::Ref TrajectoryRolloutRef::getPosesConst( const std::string& mapping) const @@ -480,6 +538,20 @@ const Eigen::Ref TrajectoryRolloutRef::getMassesConst() return mToSlice->getMassesConst(); } +//============================================================================== +const Eigen::Ref TrajectoryRolloutRef::getDampingsConst() + const +{ + return mToSlice->getDampingsConst(); +} + +//============================================================================== +const Eigen::Ref TrajectoryRolloutRef::getSpringsConst() + const +{ + return mToSlice->getSpringsConst(); +} + //============================================================================== const std::unordered_map& TrajectoryRolloutRef::getMetadataMap() const @@ -545,6 +617,20 @@ Eigen::Ref TrajectoryRolloutConstRef::getMasses() throw std::runtime_error{"Execution should never reach this point"}; } +//============================================================================== +Eigen::Ref TrajectoryRolloutConstRef::getDampings() +{ + assert(false && "It should be impossible to get a mutable reference from a TrajectorRolloutConstRef"); + throw std::runtime_error{"Execution should never reach this point"}; +} + +//============================================================================== +Eigen::Ref TrajectoryRolloutConstRef::getSprings() +{ + assert(false && "It should be impossible to get a mutable reference from a TrajectorRolloutConstRef"); + throw std::runtime_error{"Execution should never reach this point"}; +} + //============================================================================== const Eigen::Ref TrajectoryRolloutConstRef::getPosesConst(const std::string& mapping) const @@ -576,6 +662,20 @@ TrajectoryRolloutConstRef::getMassesConst() const return mToSlice->getMassesConst(); } +//============================================================================== +const Eigen::Ref +TrajectoryRolloutConstRef::getDampingsConst() const +{ + return mToSlice->getDampingsConst(); +} + +//============================================================================== +const Eigen::Ref +TrajectoryRolloutConstRef::getSpringsConst() const +{ + return mToSlice->getSpringsConst(); +} + //============================================================================== const std::unordered_map& TrajectoryRolloutConstRef::getMetadataMap() const diff --git a/dart/trajectory/TrajectoryRollout.hpp b/dart/trajectory/TrajectoryRollout.hpp index e135bb511..b9b92aca2 100644 --- a/dart/trajectory/TrajectoryRollout.hpp +++ b/dart/trajectory/TrajectoryRollout.hpp @@ -42,6 +42,8 @@ class TrajectoryRollout const std::string& mapping = "identity") = 0; virtual Eigen::Ref getMasses() = 0; + virtual Eigen::Ref getDampings() = 0; + virtual Eigen::Ref getSprings() = 0; virtual const Eigen::Ref getPosesConst( const std::string& mapping = "identity") const = 0; @@ -50,6 +52,8 @@ class TrajectoryRollout virtual const Eigen::Ref getControlForcesConst( const std::string& mapping = "identity") const = 0; virtual const Eigen::Ref getMassesConst() const = 0; + virtual const Eigen::Ref getDampingsConst() const = 0; + virtual const Eigen::Ref getSpringsConst() const = 0; virtual const std::unordered_map& getMetadataMap() const = 0; @@ -100,6 +104,8 @@ class TrajectoryRolloutReal : public TrajectoryRollout mappings, int steps, int massDim, + int dampDim, + int springDim, const std::unordered_map metadata); /// Create a fresh trajector rollout for a shot @@ -114,6 +120,8 @@ class TrajectoryRolloutReal : public TrajectoryRollout const std::unordered_map vel, const std::unordered_map force, const Eigen::VectorXs mass, + const Eigen::VectorXs damping, + const Eigen::VectorXs spring, const std::unordered_map metadata); const std::vector& getMappings() const override; @@ -124,6 +132,8 @@ class TrajectoryRolloutReal : public TrajectoryRollout Eigen::Ref getControlForces( const std::string& mapping = "identity") override; Eigen::Ref getMasses() override; + Eigen::Ref getDampings() override; + Eigen::Ref getSprings() override; const Eigen::Ref getPosesConst( const std::string& mapping = "identity") const override; const Eigen::Ref getVelsConst( @@ -131,6 +141,8 @@ class TrajectoryRolloutReal : public TrajectoryRollout const Eigen::Ref getControlForcesConst( const std::string& mapping = "identity") const override; const Eigen::Ref getMassesConst() const override; + const Eigen::Ref getDampingsConst() const override; + const Eigen::Ref getSpringsConst() const override; virtual const std::unordered_map& getMetadataMap() const override; @@ -143,6 +155,8 @@ class TrajectoryRolloutReal : public TrajectoryRollout std::unordered_map mVels; std::unordered_map mForces; Eigen::VectorXs mMasses; + Eigen::VectorXs mDampings; + Eigen::VectorXs mSprings; std::unordered_map mMetadata; std::vector mMappings; }; @@ -161,6 +175,8 @@ class TrajectoryRolloutRef : public TrajectoryRollout Eigen::Ref getControlForces( const std::string& mapping = "identity") override; Eigen::Ref getMasses() override; + Eigen::Ref getDampings() override; + Eigen::Ref getSprings() override; const Eigen::Ref getPosesConst( const std::string& mapping = "identity") const override; const Eigen::Ref getVelsConst( @@ -168,6 +184,8 @@ class TrajectoryRolloutRef : public TrajectoryRollout const Eigen::Ref getControlForcesConst( const std::string& mapping = "identity") const override; const Eigen::Ref getMassesConst() const override; + const Eigen::Ref getDampingsConst() const override; + const Eigen::Ref getSpringsConst() const override; virtual const std::unordered_map& getMetadataMap() const override; @@ -196,6 +214,8 @@ class TrajectoryRolloutConstRef : public TrajectoryRollout Eigen::Ref getControlForces( const std::string& mapping = "identity") override; Eigen::Ref getMasses() override; + Eigen::Ref getDampings() override; + Eigen::Ref getSprings() override; const Eigen::Ref getPosesConst( const std::string& mapping = "identity") const override; const Eigen::Ref getVelsConst( @@ -203,6 +223,8 @@ class TrajectoryRolloutConstRef : public TrajectoryRollout const Eigen::Ref getControlForcesConst( const std::string& mapping = "identity") const override; const Eigen::Ref getMassesConst() const override; + const Eigen::Ref getDampingsConst() const override; + const Eigen::Ref getSpringsConst() const override; virtual const std::unordered_map& getMetadataMap() const override; diff --git a/data/skel/acrobot.skel b/data/skel/acrobot.skel new file mode 100644 index 000000000..61cbd3895 --- /dev/null +++ b/data/skel/acrobot.skel @@ -0,0 +1,126 @@ + + + + + 0.01 + 0 -9.81 0 + bullet + + + + 0 -0.35 0 0 0 0 + + 0 0 0 0 0 0 + + 0.75 + 0.0 0 0.0 + + + 0.0 0 0.0 0 0 0 + + + 0.2 0.05 0.05 + + + + + 0.0 0 0.0 0 0 0 + + + 0.2 0.05 0.05 + + + + + + + 0.0 0.0 0.0 0.0 0.0 0.0 + + 1.0 + 0.0 0.5 0.0 + + + 0.0 0.5 0.0 0.0 0.0 0.0 + + + 0.02 1.0 0.02 + + + + + 0.0 0.5 0.0 0.0 0.0 0.0 + + + 0.02 1.0 0.02 + + + + + + + 0.0 1.0 0.0 0.0 0.0 0.0 + + 2.0 + 0.0 1.0 0.0 + + + 0.0 0.5 0.0 0.0 0.0 0.0 + + + 0.02 1.0 0.02 + + + + + 0.0 0.5 0.0 0.0 0.0 0.0 + + + 0.02 1.0 0.02 + + + + + + + + world + cart + + 1.0 0.0 0.0 + + 0 + 0 + + + + 0.0 0.0 0.0 0.0 0.0 0.0 + cart + pole + + 0.0 0.0 1.0 + + 0.2 + + + 3.1415 + 0 + + + + 0.0 0.0 0.0 0.0 0.0 0.0 + pole + pole2 + + 0.0 0.0 1.0 + + 0.2 + + + 0 + 0 + + + + + + diff --git a/data/skel/cartpole.skel b/data/skel/cartpole.skel index 710be6a0c..e2c3ff290 100644 --- a/data/skel/cartpole.skel +++ b/data/skel/cartpole.skel @@ -2,123 +2,119 @@ - 0.02 + 0.01 0 -9.81 0 - dart + bullet - - - false - - 0 0 0 0 1.57 0.0 + + + 0 -0.35 0 0 0 0 + + 0 0 0 0 0 0 + + 1.0 + 0.0 0 0.0 + - 0 0 0 0 0 0 + 0.0 0 0.0 0 0 0 - - 2.0 - 0.02 - + + 0.5 0.1 0.1 + - 0.3 0.3 0.7 1 + + 0.0 0 0.0 0 0 0 + + + 0.2 0.05 0.05 + + + - - world - ground - - - - 0 0.0 0 0 0 0 - - 0.0 0 0 0 0 0 + + 0.0 0.0 0.0 0.0 0.0 0.0 - 9.42477796 - 0.0 0 0.0 + 1.0 + 0.0 0.5 0.0 - 0.0 0 0.0 0 1.57 0 + 0.0 0.5 0.0 0.0 0.0 0.0 - - 0.2 - 0.1 - + + 0.02 1.0 0.02 + - 0.0 0 0.0 0 1.57 0 + 0.0 0.5 0.0 0.0 0.0 0.0 - - 0.2 - 0.1 - + + 0.02 1.0 0.02 + - - 0.0 0.0 0.0 0 0.0 0.0 + + 0.0 1.02 0.0 0.0 0.0 0.0 - 4.8953899 - 0.0 0.3 0.0 + 0.1 + 0.0 0.0 0.0 - 0.0 0.3 0.0 1.57 0.0 0.0 + 0.0 0.0 0.0 0.0 0.0 0.0 - - 0.6 - 0.049 - + + 0.08 0.04 0.08 + + 0.2 0.7 0.7 - 0.0 0.3 0.0 1.57 0.0 0.0 + 0.0 0.0 0.0 0.0 0.0 0.0 - - 0.6 - 0.049 - + + 0.08 0.04 0.08 + - + + - + world cart - - 1.0 0.0 0.0 - - -1 - 1 - - - 1.0 - - + + 1.0 0.0 0.0 + 0 0 + 0.0 0.0 0.0 0.0 0.0 0.0 cart pole - 0.0 0.0 -1.0 - - -1.57 - 1.57 - - - 1.0 + 0.0 0.0 1.0 + + 0.1 - 0 + 3.1415 0 + + 0.0 0.0 0.0 0.0 0.0 0.0 + pole + weight + + - diff --git a/data/skel/half_cheetah.skel b/data/skel/half_cheetah.skel index 1a7e0d59c..b1abd2883 100644 --- a/data/skel/half_cheetah.skel +++ b/data/skel/half_cheetah.skel @@ -66,7 +66,7 @@ 0.0 0.7 0 0 0 0 - 0.1 + 0.0 0.0 0 0.0 @@ -74,7 +74,7 @@ 0.0 0.7 0 0 0 0 - 0.1 + 0.0 0.0 0 0.0 diff --git a/data/skel/inverted_double_pendulum.skel b/data/skel/inverted_double_pendulum.skel index 1ce9709a4..f9aedf7a2 100644 --- a/data/skel/inverted_double_pendulum.skel +++ b/data/skel/inverted_double_pendulum.skel @@ -3,8 +3,8 @@ 0.01 - 0 -9.81 0 - dart + 0 -4.905 0 + bullet @@ -12,7 +12,7 @@ 0 0 0 0 0 0 - 0.75 + 1.0 0.0 0 0.0 @@ -23,12 +23,20 @@ + + 0.0 0 0.0 0 0 0 + + + 0.2 0.05 0.05 + + + 0.0 0.0 0.0 0.0 0.0 0.0 - 0.025 + 0.5 0.0 0.15 0.0 @@ -39,12 +47,20 @@ + + 0.0 0.15 0.0 0.0 0.0 0.0 + + + 0.02 0.3 0.02 + + + 0.0 0.3 0.0 0.0 0.0 0.0 - 0.025 + 0.5 0.0 0.15 0.0 @@ -55,12 +71,20 @@ + + 0.0 0.15 0.0 0.0 0.0 0.0 + + + 0.02 0.3 0.02 + + + 0.0 0.62 0.0 0.0 0.0 0.0 - 0.3 + 1.0 0.0 0.0 0.0 @@ -72,6 +96,14 @@ 0.2 0.7 0.7 + + 0.0 0.0 0.0 0.0 0.0 0.0 + + + 0.08 0.04 0.08 + + + @@ -93,10 +125,10 @@ 0.0 0.0 1.0 - 0.1 + 0.2 - 0 + 3.1415 0 @@ -107,7 +139,8 @@ 0.0 0.0 1.0 - 0.1 + 0.4 + 3.0 0 diff --git a/data/skel/whip2d.skel b/data/skel/whip2d.skel new file mode 100644 index 000000000..9a8cb5ee3 --- /dev/null +++ b/data/skel/whip2d.skel @@ -0,0 +1,169 @@ + + + + + 0.01 + 0 -4.81 0 + bullet + + + + 0 -0.35 0 0 0 0 + + 0.0 0.0 0.0 0.0 0.0 0.0 + + 2.0 + 0.0 0.0 0.0 + + + 0.0 0.0 0.0 0.0 0.0 0.0 + + + 0.06 0.06 0.06 + + + + + 0.0 0.0 0.0 0.0 0.0 0.0 + + + 0.06 0.06 0.06 + + + + + + + 0.0 0.0 0.0 0.0 0.0 0.0 + + 0.5 + 0.0 0.15 0.0 + + + 0.0 0.15 0.0 0.0 0.0 0.0 + + + 0.02 0.3 0.02 + + + 0.2 0.7 0.7 + + + 0.0 0.15 0.0 0.0 0.0 0.0 + + + 0.02 0.3 0.02 + + + + + + + 0.0 0.3 0.0 0.0 0.0 0.0 + + 0.25 + 0.0 0.15 0.0 + + + 0.0 0.15 0.0 0.0 0.0 0.0 + + + 0.02 0.3 0.02 + + + 0.4 0.5 0.5 + + + 0.0 0.15 0.0 0.0 0.0 0.0 + + + 0.02 0.3 0.02 + + + + + + + 0.0 0.6 0.0 0.0 0.0 0.0 + + 0.3 + 0.0 0.15 0.0 + + + 0.0 0.15 0.0 0.0 0.0 0.0 + + + 0.02 0.3 0.02 + + + 0.7 0.2 0.2 + + + 0.0 0.15 0.0 0.0 0.0 0.0 + + + 0.02 0.3 0.02 + + + + + + + + world + cart + + 1.0 0.0 0.0 + + 0 + 0 + + + + 0.0 0.0 0.0 0.0 0.0 0.0 + cart + pole + + 0.0 0.0 1.0 + + 0.05 + 8.0 + + + 0 + 0 + + + + 0.0 0.0 0.0 0.0 0.0 0.0 + pole + pole2 + + 0.0 0.0 1.0 + + 0.05 + 8.0 + + + 0 + 0 + + + 0.0 0.0 0.0 0.0 0.0 0.0 + pole2 + pole3 + + 0.0 0.0 1.0 + + 0.05 + 8.0 + + + 0 + 0 + + + + + + diff --git a/data/urdf/xmate3p/meshes/collision/xmate3_base.stl b/data/urdf/xmate3p/meshes/collision/xmate3_base.stl new file mode 100644 index 000000000..0361fbf39 Binary files /dev/null and b/data/urdf/xmate3p/meshes/collision/xmate3_base.stl differ diff --git a/data/urdf/xmate3p/meshes/collision/xmate3_link1.stl b/data/urdf/xmate3p/meshes/collision/xmate3_link1.stl new file mode 100644 index 000000000..07366a379 Binary files /dev/null and b/data/urdf/xmate3p/meshes/collision/xmate3_link1.stl differ diff --git a/data/urdf/xmate3p/meshes/collision/xmate3_link2.stl b/data/urdf/xmate3p/meshes/collision/xmate3_link2.stl new file mode 100644 index 000000000..c5bd1c493 Binary files /dev/null and b/data/urdf/xmate3p/meshes/collision/xmate3_link2.stl differ diff --git a/data/urdf/xmate3p/meshes/collision/xmate3_link3.stl b/data/urdf/xmate3p/meshes/collision/xmate3_link3.stl new file mode 100644 index 000000000..597ea5f9d Binary files /dev/null and b/data/urdf/xmate3p/meshes/collision/xmate3_link3.stl differ diff --git a/data/urdf/xmate3p/meshes/collision/xmate3_link4.stl b/data/urdf/xmate3p/meshes/collision/xmate3_link4.stl new file mode 100644 index 000000000..9eeeecc26 Binary files /dev/null and b/data/urdf/xmate3p/meshes/collision/xmate3_link4.stl differ diff --git a/data/urdf/xmate3p/meshes/collision/xmate3_link5.stl b/data/urdf/xmate3p/meshes/collision/xmate3_link5.stl new file mode 100644 index 000000000..bd954c2a1 Binary files /dev/null and b/data/urdf/xmate3p/meshes/collision/xmate3_link5.stl differ diff --git a/data/urdf/xmate3p/meshes/collision/xmate3_link6.stl b/data/urdf/xmate3p/meshes/collision/xmate3_link6.stl new file mode 100644 index 000000000..bca7aeeeb Binary files /dev/null and b/data/urdf/xmate3p/meshes/collision/xmate3_link6.stl differ diff --git a/data/urdf/xmate3p/meshes/collision/xmate3_link7.stl b/data/urdf/xmate3p/meshes/collision/xmate3_link7.stl new file mode 100644 index 000000000..68e029e6b Binary files /dev/null and b/data/urdf/xmate3p/meshes/collision/xmate3_link7.stl differ diff --git a/data/urdf/xmate3p/meshes/visual/xmate3_base.stl b/data/urdf/xmate3p/meshes/visual/xmate3_base.stl new file mode 100644 index 000000000..0361fbf39 Binary files /dev/null and b/data/urdf/xmate3p/meshes/visual/xmate3_base.stl differ diff --git a/data/urdf/xmate3p/meshes/visual/xmate3_link1.stl b/data/urdf/xmate3p/meshes/visual/xmate3_link1.stl new file mode 100644 index 000000000..07366a379 Binary files /dev/null and b/data/urdf/xmate3p/meshes/visual/xmate3_link1.stl differ diff --git a/data/urdf/xmate3p/meshes/visual/xmate3_link2.stl b/data/urdf/xmate3p/meshes/visual/xmate3_link2.stl new file mode 100644 index 000000000..c5bd1c493 Binary files /dev/null and b/data/urdf/xmate3p/meshes/visual/xmate3_link2.stl differ diff --git a/data/urdf/xmate3p/meshes/visual/xmate3_link3.stl b/data/urdf/xmate3p/meshes/visual/xmate3_link3.stl new file mode 100644 index 000000000..597ea5f9d Binary files /dev/null and b/data/urdf/xmate3p/meshes/visual/xmate3_link3.stl differ diff --git a/data/urdf/xmate3p/meshes/visual/xmate3_link4.stl b/data/urdf/xmate3p/meshes/visual/xmate3_link4.stl new file mode 100644 index 000000000..9eeeecc26 Binary files /dev/null and b/data/urdf/xmate3p/meshes/visual/xmate3_link4.stl differ diff --git a/data/urdf/xmate3p/meshes/visual/xmate3_link5.stl b/data/urdf/xmate3p/meshes/visual/xmate3_link5.stl new file mode 100644 index 000000000..bd954c2a1 Binary files /dev/null and b/data/urdf/xmate3p/meshes/visual/xmate3_link5.stl differ diff --git a/data/urdf/xmate3p/meshes/visual/xmate3_link6.stl b/data/urdf/xmate3p/meshes/visual/xmate3_link6.stl new file mode 100644 index 000000000..bca7aeeeb Binary files /dev/null and b/data/urdf/xmate3p/meshes/visual/xmate3_link6.stl differ diff --git a/data/urdf/xmate3p/meshes/visual/xmate3_link7.stl b/data/urdf/xmate3p/meshes/visual/xmate3_link7.stl new file mode 100644 index 000000000..68e029e6b Binary files /dev/null and b/data/urdf/xmate3p/meshes/visual/xmate3_link7.stl differ diff --git a/data/urdf/xmate3p/xmate3p.urdf b/data/urdf/xmate3p/xmate3p.urdf new file mode 100644 index 000000000..97520216d --- /dev/null +++ b/data/urdf/xmate3p/xmate3p.urdf @@ -0,0 +1,336 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + transmission_interface/SimpleTransmission + + EffortJointInterface + + + EffortJointInterface + 1 + + + + + + + + + + + + + + + + + + + + + + + + + + transmission_interface/SimpleTransmission + + EffortJointInterface + + + EffortJointInterface + 1 + + + + + + + + + + + + + + + + + + + + + + + + + + + transmission_interface/SimpleTransmission + + EffortJointInterface + + + EffortJointInterface + 1 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + transmission_interface/SimpleTransmission + + EffortJointInterface + + + EffortJointInterface + 1 + + + + + + + + + + + + + + + + + + + + + + + + + + + transmission_interface/SimpleTransmission + + EffortJointInterface + + + EffortJointInterface + 1 + + + + + + + + + + + + + + + + + + + + + + + + + + + transmission_interface/SimpleTransmission + + EffortJointInterface + + + EffortJointInterface + 1 + + + + + + + + + + + + + + + + + + + + + + + + + + + transmission_interface/SimpleTransmission + + EffortJointInterface + + + EffortJointInterface + 1 + + + + + + + + + + + + + + + + + + + + diff --git a/javascript/package-lock.json b/javascript/package-lock.json index aa12af235..15b002715 100644 --- a/javascript/package-lock.json +++ b/javascript/package-lock.json @@ -1760,6 +1760,14 @@ "integrity": "sha512-jDctJ/IVQbZoJykoeHbhXpOlNBqGNcwXJKJog42E5HDPUwQTSdjCHdihjj0DlnheQ7blbT6dHOafNAiS8ooQKA==", "dev": true }, + "bindings": { + "version": "1.5.0", + "resolved": "https://registry.npmjs.org/bindings/-/bindings-1.5.0.tgz", + "integrity": "sha512-p2q/t/mhvuOj/UeLlV6566GD/guowlr0hHxClI0W9m7MWYkL1F0hLo+0Aexs9HSPCtR1SXQ0TD3MMKrXZajbiQ==", + "requires": { + "file-uri-to-path": "1.0.0" + } + }, "bluebird": { "version": "3.7.2", "resolved": "https://registry.npmjs.org/bluebird/-/bluebird-3.7.2.tgz", @@ -2730,6 +2738,14 @@ "integrity": "sha512-T0NIuQpnTvFDATNuHN5roPwSBG83rFsuO+MXXH9/3N1eFbn4wcPjttvjMLEPWJ0RGUYgQE7cGgS3tNxbqCGM7g==", "dev": true }, + "dev": { + "version": "0.1.3", + "resolved": "https://registry.npmjs.org/dev/-/dev-0.1.3.tgz", + "integrity": "sha512-flCHQwAkXk3+1up/wo93Ms9E5Wf9K28ZGA7Oz55nBZ8OTdPPhyvZrwsT+twtySTFHIwv0w9CUNtagZgu1MgzXQ==", + "requires": { + "inotify": ">= 0.1.6" + } + }, "diffie-hellman": { "version": "5.0.3", "resolved": "https://registry.npmjs.org/diffie-hellman/-/diffie-hellman-5.0.3.tgz", @@ -3335,6 +3351,11 @@ "integrity": "sha512-0btnI/H8f2pavGMN8w40mlSKOfTK2SVJmBfBeVIj3kNw0swwgzyRq0d5TJVOwodFmtvpPeWPN/MCcfuWF0Ezbw==", "dev": true }, + "file-uri-to-path": { + "version": "1.0.0", + "resolved": "https://registry.npmjs.org/file-uri-to-path/-/file-uri-to-path-1.0.0.tgz", + "integrity": "sha512-0Zt+s3L7Vf1biwWZ29aARiVYLx7iMGnEUl9x33fbB/j3jR81u/O2LbqK+Bm1CDSNDKVtJ/YjwY7TUd5SkeLQLw==" + }, "fill-range": { "version": "7.0.1", "resolved": "https://registry.npmjs.org/fill-range/-/fill-range-7.0.1.tgz", @@ -4033,6 +4054,15 @@ "integrity": "sha512-JV/yugV2uzW5iMRSiZAyDtQd+nxtUnjeLt0acNdw98kKLrvuRVyB80tsREOE7yvGVgalhZ6RNXCmEHkUKBKxew==", "dev": true }, + "inotify": { + "version": "1.4.6", + "resolved": "https://registry.npmjs.org/inotify/-/inotify-1.4.6.tgz", + "integrity": "sha512-WW8/uqIA04O3AePQVe/Ms3ZLR0yGamaz8YOEpaXc4WBAGOPZfzu58wWErEPSUYaPyDrJRIeCn6PEIQgC1ZyQ5w==", + "requires": { + "bindings": "^1.3.1", + "nan": "^2.12.1" + } + }, "internal-ip": { "version": "4.3.0", "resolved": "https://registry.npmjs.org/internal-ip/-/internal-ip-4.3.0.tgz", @@ -4828,6 +4858,11 @@ "integrity": "sha1-iZ8R2WhuXgXLkbNdXw5jt3PPyQE=", "dev": true }, + "nan": { + "version": "2.16.0", + "resolved": "https://registry.npmjs.org/nan/-/nan-2.16.0.tgz", + "integrity": "sha512-UdAqHyFngu7TfQKsCBgAA6pWDkT8MAO7d0jyOecVhN5354xbLqdn8mV9Tat9gepAupm0bt2DbeaSC8vS52MuFA==" + }, "nanomatch": { "version": "1.2.13", "resolved": "https://registry.npmjs.org/nanomatch/-/nanomatch-1.2.13.tgz", @@ -7302,7 +7337,11 @@ "resolved": "https://registry.npmjs.org/fsevents/-/fsevents-1.2.13.tgz", "integrity": "sha512-oWb1Z6mkHIskLzEJ/XWX0srkpkTQ7vaopMQkyaEIoq0fmtFVxOthb8cCxeT+p3ynTdkk/RZwbgG4brR5BeWECw==", "dev": true, - "optional": true + "optional": true, + "requires": { + "bindings": "^1.5.0", + "nan": "^2.12.1" + } }, "glob-parent": { "version": "3.1.0", @@ -7618,7 +7657,11 @@ "resolved": "https://registry.npmjs.org/fsevents/-/fsevents-1.2.13.tgz", "integrity": "sha512-oWb1Z6mkHIskLzEJ/XWX0srkpkTQ7vaopMQkyaEIoq0fmtFVxOthb8cCxeT+p3ynTdkk/RZwbgG4brR5BeWECw==", "dev": true, - "optional": true + "optional": true, + "requires": { + "bindings": "^1.5.0", + "nan": "^2.12.1" + } }, "glob-parent": { "version": "3.1.0", diff --git a/javascript/package.json b/javascript/package.json index 65677c5a5..a6401d281 100644 --- a/javascript/package.json +++ b/javascript/package.json @@ -42,6 +42,7 @@ "react": "^17.0.2" }, "dependencies": { + "dev": "^0.1.3", "three": "^0.121.1" } } diff --git a/python/_nimblephysics/neural/BackpropSnapshot.cpp b/python/_nimblephysics/neural/BackpropSnapshot.cpp index 8bf1d6a74..7a06d8126 100644 --- a/python/_nimblephysics/neural/BackpropSnapshot.cpp +++ b/python/_nimblephysics/neural/BackpropSnapshot.cpp @@ -78,26 +78,51 @@ void BackpropSnapshot(py::module& m) &dart::neural::BackpropSnapshot::getVelVelJacobian, ::py::arg("world"), ::py::arg("perfLog") = nullptr) + .def( + "getContactFreeVelVelJacobian", + &dart::neural::BackpropSnapshot::getContactFreeVelVelJacobian, + ::py::arg("world"), + ::py::arg("perfLog") = nullptr) .def( "getControlForceVelJacobian", &dart::neural::BackpropSnapshot::getControlForceVelJacobian, ::py::arg("world"), ::py::arg("perfLog") = nullptr) + .def( + "getContactFreeControlForceVelJacobian", + &dart::neural::BackpropSnapshot::getContactFreeControlForceVelJacobian, + ::py::arg("world"), + ::py::arg("perfLog") = nullptr) .def( "getPosPosJacobian", &dart::neural::BackpropSnapshot::getPosPosJacobian, ::py::arg("world"), ::py::arg("perfLog") = nullptr) + .def( + "getContactFreePosPosJacobian", + &dart::neural::BackpropSnapshot::getContactFreePosPosJacobian, + ::py::arg("world"), + ::py::arg("perfLog") = nullptr) .def( "getVelPosJacobian", &dart::neural::BackpropSnapshot::getVelPosJacobian, ::py::arg("world"), ::py::arg("perfLog") = nullptr) + .def( + "getContactFreeVelPosJacobian", + &dart::neural::BackpropSnapshot::getContactFreeVelPosJacobian, + ::py::arg("world"), + ::py::arg("perfLog") = nullptr) .def( "getPosVelJacobian", &dart::neural::BackpropSnapshot::getPosVelJacobian, ::py::arg("world"), ::py::arg("perfLog") = nullptr) + .def( + "getContactFreePosVelJacobian", + &dart::neural::BackpropSnapshot::getContactFreePosVelJacobian, + ::py::arg("world"), + ::py::arg("perfLog") = nullptr) .def( "getMassVelJacobian", &dart::neural::BackpropSnapshot::getMassVelJacobian, @@ -107,10 +132,18 @@ void BackpropSnapshot(py::module& m) "getStateJacobian", &dart::neural::BackpropSnapshot::getStateJacobian, ::py::arg("world")) + .def( + "getContactFreeStateJacobian", + &dart::neural::BackpropSnapshot::getContactFreeStateJacobian, + ::py::arg("world")) .def( "getActionJacobian", &dart::neural::BackpropSnapshot::getActionJacobian, ::py::arg("world")) + .def( + "getContactFreeActionJacobian", + &dart::neural::BackpropSnapshot::getContactFreeActionJacobian, + ::py::arg("world")) .def( "getPreStepPosition", &dart::neural::BackpropSnapshot::getPreStepPosition) @@ -129,6 +162,16 @@ void BackpropSnapshot(py::module& m) .def( "getPostStepTorques", &dart::neural::BackpropSnapshot::getPostStepTorques) + .def("getAllConstraintMatrix", + &dart::neural::BackpropSnapshot::getAllConstraintMatrix, + ::py::arg("world")) + .def("getClampingConstraintMatrix", + &dart::neural::BackpropSnapshot::getClampingConstraintMatrix, + ::py::arg("world")) + .def("getNumConstraintDim", + &dart::neural::BackpropSnapshot::getNumConstraintDim) + .def("getNumContact", + &dart::neural::BackpropSnapshot::getNumContacts) .def("getMassMatrix", &dart::neural::BackpropSnapshot::getMassMatrix) .def( "getInvMassMatrix", &dart::neural::BackpropSnapshot::getInvMassMatrix) diff --git a/python/_nimblephysics/realtime/MPCLocal.cpp b/python/_nimblephysics/realtime/MPCLocal.cpp index 7cdb38077..5b5c678d7 100644 --- a/python/_nimblephysics/realtime/MPCLocal.cpp +++ b/python/_nimblephysics/realtime/MPCLocal.cpp @@ -55,10 +55,12 @@ void MPCLocal(py::module& m) ::py::init< std::shared_ptr, std::shared_ptr, - int>(), + int, + s_t>(), ::py::arg("world"), ::py::arg("loss"), - ::py::arg("planningHorizonMillis")) + ::py::arg("planningHorizonMillis"), + ::py::arg("scale")) .def("setLoss", &dart::realtime::MPCLocal::setLoss, ::py::arg("loss")) .def( "setOptimizer", diff --git a/python/_nimblephysics/realtime/SSID.cpp b/python/_nimblephysics/realtime/SSID.cpp index 0d85aa0e2..761dabcc4 100644 --- a/python/_nimblephysics/realtime/SSID.cpp +++ b/python/_nimblephysics/realtime/SSID.cpp @@ -53,12 +53,14 @@ void SSID(py::module& m) std::shared_ptr, int, Eigen::VectorXs, - int>(), + int, + s_t>(), ::py::arg("world"), ::py::arg("loss"), ::py::arg("planningHorizonMillis"), ::py::arg("sensorDim"), - ::py::arg("steps")) + ::py::arg("steps"), + ::py::arg("scale")) .def("setLoss", &dart::realtime::SSID::setLoss, ::py::arg("loss")) .def( "setOptimizer", diff --git a/python/_nimblephysics/simulation/World.cpp b/python/_nimblephysics/simulation/World.cpp index a273e7b8c..69b8ce152 100644 --- a/python/_nimblephysics/simulation/World.cpp +++ b/python/_nimblephysics/simulation/World.cpp @@ -517,7 +517,9 @@ void World(py::module& m) &dart::simulation::World::addDofToActionSpace, ::py::arg("dofIndex")) .def("getStateJacobian", &dart::simulation::World::getStateJacobian) + .def("getContactFreeStateJacobian", &dart::simulation::World::getContactFreeStateJacobian) .def("getActionJacobian", &dart::simulation::World::getActionJacobian) + .def("getContactFreeActionJacobian", &dart::simulation::World::getContactFreeActionJacobian) .def_readonly("onNameChanged", &dart::simulation::World::onNameChanged); } diff --git a/python/nimblephysics.egg-info/PKG-INFO b/python/nimblephysics.egg-info/PKG-INFO index 140c991de..16abca782 100644 --- a/python/nimblephysics.egg-info/PKG-INFO +++ b/python/nimblephysics.egg-info/PKG-INFO @@ -1,10 +1,13 @@ -Metadata-Version: 1.0 +Metadata-Version: 2.1 Name: nimblephysics -Version: 0.4.0 +Version: 0.6.1 Summary: A differentiable fully featured physics engine Home-page: UNKNOWN Author: Keenon Werling Author-email: keenonwerling@gmail.com License: MIT -Description: UNKNOWN Platform: UNKNOWN +License-File: LICENSE + +UNKNOWN + diff --git a/python/nimblephysics.egg-info/SOURCES.txt b/python/nimblephysics.egg-info/SOURCES.txt index 787e24324..f0f8c9aa5 100644 --- a/python/nimblephysics.egg-info/SOURCES.txt +++ b/python/nimblephysics.egg-info/SOURCES.txt @@ -1,8 +1,16 @@ +LICENSE README.md setup.py python/nimblephysics/__init__.py +python/nimblephysics/get_anthropometric_log_pdf.py +python/nimblephysics/get_height.py +python/nimblephysics/get_lowest_point.py +python/nimblephysics/get_marker_dist_to_nearest_vertex.py python/nimblephysics/gui_server.py +python/nimblephysics/loader.py python/nimblephysics/mapping.py +python/nimblephysics/marker_mocap.py +python/nimblephysics/native_trajectory_support.py python/nimblephysics/timestep.py python/nimblephysics.egg-info/PKG-INFO python/nimblephysics.egg-info/SOURCES.txt @@ -10,5 +18,8 @@ python/nimblephysics.egg-info/dependency_links.txt python/nimblephysics.egg-info/not-zip-safe python/nimblephysics.egg-info/requires.txt python/nimblephysics.egg-info/top_level.txt +python/nimblephysics/models/__init__.py +python/nimblephysics/models/rajagopal.py python/nimblephysics/web_gui/bundle.js +python/nimblephysics/web_gui/favicon.ico python/nimblephysics/web_gui/index.html \ No newline at end of file diff --git a/unittests/comprehensive/CMakeLists.txt b/unittests/comprehensive/CMakeLists.txt index f04944638..7bbc3dfe8 100644 --- a/unittests/comprehensive/CMakeLists.txt +++ b/unittests/comprehensive/CMakeLists.txt @@ -23,7 +23,16 @@ dart_add_test("comprehensive" test_KR5Trajectory) dart_add_test("comprehensive" test_SaddlepointEscape) dart_add_test("comprehensive" test_DampGradient) dart_add_test("comprehensive" test_SpringGradient) - +dart_add_test("comprehensive" test_iLQRRealtime) +dart_add_test("comprehensive" test_iLQRHalfCheetah) +dart_add_test("comprehensive" test_cartpoleRealtime) +dart_add_test("comprehensive" test_cartpoleSSID) +dart_add_test("comprehensive" test_iLQR) +dart_add_test("comprehensive" test_FullArm) +dart_add_test("comprehensive" test_FullContactCartpole) +dart_add_test("comprehensive" test_FullAcrobot) +dart_add_test("comprehensive" test_FullDoublependulum) +dart_add_test("comprehensive" test_FullWhip) # target_link_libraries(test_Gradients dart-collision-bullet) # target_link_libraries(test_Gradients dart-collision-ode) @@ -85,10 +94,28 @@ if(TARGET dart-utils) target_link_libraries(test_HalfCheetahTrajectory dart-utils-urdf) target_link_libraries(test_HalfCheetahRealtime dart-utils) target_link_libraries(test_HalfCheetahRealtime dart-utils-urdf) + target_link_libraries(test_iLQRHalfCheetah dart-utils) + target_link_libraries(test_iLQRHalfCheetah dart-utils-urdf) target_link_libraries(test_KR5Trajectory dart-utils) target_link_libraries(test_KR5Trajectory dart-utils-urdf) target_link_libraries(test_Cartpole dart-utils) target_link_libraries(test_Cartpole dart-utils-urdf) + target_link_libraries(test_FullArm dart-utils) + target_link_libraries(test_FullArm dart-utils-urdf) + target_link_libraries(test_FullAcrobot dart-utils) + target_link_libraries(test_FullAcrobot dart-utils-urdf) + target_link_libraries(test_FullDoublependulum dart-utils) + target_link_libraries(test_FullDoublependulum dart-utils-urdf) + target_link_libraries(test_FullWhip dart-utils) + target_link_libraries(test_FullWhip dart-utils-urdf) + target_link_libraries(test_cartpoleSSID dart-utils) + target_link_libraries(test_cartpoleSSID dart-utils-urdf) + target_link_libraries(test_cartpoleRealtime dart-utils) + target_link_libraries(test_cartpoleRealtime dart-utils-urdf) + target_link_libraries(test_iLQRRealtime dart-utils) + target_link_libraries(test_iLQRRealtime dart-utils-urdf) + target_link_libraries(test_iLQR dart-utils) + target_link_libraries(test_iLQR dart-utils-urdf) dart_add_test("comprehensive" test_Gradients) target_link_libraries(test_Gradients dart-utils-urdf) diff --git a/unittests/comprehensive/test_FullAcrobot.cpp b/unittests/comprehensive/test_FullAcrobot.cpp new file mode 100644 index 000000000..a64d36b18 --- /dev/null +++ b/unittests/comprehensive/test_FullAcrobot.cpp @@ -0,0 +1,546 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include + + +#include +#include +#include + +#include "dart/neural/RestorableSnapshot.hpp" +#include "dart/realtime/MPC.hpp" +#include "dart/realtime/iLQRLocal.hpp" +#include "dart/realtime/SSID.hpp" +#include "dart/realtime/Ticker.hpp" +#include "dart/server/GUIWebsocketServer.hpp" +#include "dart/simulation/World.hpp" +#include "dart/dynamics/ShapeNode.hpp" +#include "dart/trajectory/IPOptOptimizer.hpp" +#include "dart/trajectory/LossFn.hpp" +#include "dart/trajectory/MultiShot.hpp" +#include "dart/trajectory/TrajectoryRollout.hpp" +#include "dart/utils/UniversalLoader.hpp" +#include "dart/utils/urdf/urdf.hpp" + +#include "TestHelpers.hpp" +#include "stdio.h" + +//#define iLQR_MPC_TEST + +using namespace dart; +using namespace math; +using namespace dynamics; +using namespace simulation; +using namespace neural; +using namespace realtime; +using namespace trajectory; +using namespace server; + +std::shared_ptr getSSIDPosLoss() +{ + TrajectoryLossFn loss = [](const TrajectoryRollout* rollout) { + Eigen::MatrixXs rawPos = rollout->getMetadata("sensors"); + Eigen::MatrixXs sensorPositions = rawPos.block(0,1,rawPos.rows(),rawPos.cols()-1); + int steps = rollout->getPosesConst().cols(); + + Eigen::MatrixXs posError = rollout->getPosesConst() - sensorPositions; + + //std::cout << "Pos In Buffer: " << std::endl << sensorPositions << std::endl; + //std::cout << "Pos In Traj : " << std::endl << rollout->getPosesConst() << std::endl; + + s_t sum = 0.0; + for (int i = 0; i < steps; i++) + { + sum += posError.col(i).squaredNorm(); + } + return sum; + }; + + TrajectoryLossFnAndGrad lossGrad = [](const TrajectoryRollout* rollout, + TrajectoryRollout* gradWrtRollout // OUT + ) { + gradWrtRollout->getPoses().setZero(); + gradWrtRollout->getVels().setZero(); + gradWrtRollout->getControlForces().setZero(); + int steps = rollout->getPosesConst().cols(); + + Eigen::MatrixXs rawPos = rollout->getMetadata("sensors"); + Eigen::MatrixXs sensorPositions = rawPos.block(0,1,rawPos.rows(),rawPos.cols()-1); + + Eigen::MatrixXs posError = rollout->getPosesConst() - sensorPositions; + + gradWrtRollout->getPoses() = 2 * posError; + s_t sum = 0.0; + for (int i = 0; i < steps; i++) + { + sum += posError.col(i).squaredNorm(); + } + return sum; + }; + + return std::make_shared(loss, lossGrad); +} + +std::shared_ptr getSSIDVelPosLoss() +{ + TrajectoryLossFn loss = [](const TrajectoryRollout* rollout) { + + Eigen::MatrixXs rawPos = rollout->getMetadata("sensors"); + Eigen::MatrixXs rawVel = rollout->getMetadata("velocities"); + Eigen::MatrixXs sensorPositions = rawPos.block(0,1,rawPos.rows(),rawPos.cols()-1); + Eigen::MatrixXs sensorVelocities = rawVel.block(0,1,rawVel.rows(),rawVel.cols()-1); + int steps = rollout->getPosesConst().cols(); + + Eigen::MatrixXs posError = rollout->getPosesConst() - sensorPositions; + Eigen::MatrixXs velError = rollout->getVelsConst() - sensorVelocities; + + // std::cout << "Pos Error: " << std::endl << posError << std::endl; + + s_t sum = 0.0; + for (int i = 0; i < steps; i++) + { + sum += posError.col(i).squaredNorm(); + sum += velError.col(i).squaredNorm(); + } + return sum; + }; + + TrajectoryLossFnAndGrad lossGrad = [](const TrajectoryRollout* rollout, + TrajectoryRollout* gradWrtRollout // OUT + ) { + gradWrtRollout->getPoses().setZero(); + gradWrtRollout->getVels().setZero(); + gradWrtRollout->getControlForces().setZero(); + int steps = rollout->getPosesConst().cols(); + + Eigen::MatrixXs rawPos = rollout->getMetadata("sensors"); + Eigen::MatrixXs rawVel = rollout->getMetadata("velocities"); + Eigen::MatrixXs sensorPositions = rawPos.block(0,1,rawPos.rows(),rawPos.cols()-1); + Eigen::MatrixXs sensorVelocities = rawVel.block(0,1,rawVel.rows(),rawVel.cols()-1); + + Eigen::MatrixXs posError = rollout->getPosesConst() - sensorPositions; + Eigen::MatrixXs velError = rollout->getVelsConst() - sensorVelocities; + + gradWrtRollout->getPoses() = 2 * posError; + gradWrtRollout->getVels() = 2 * velError; + s_t sum = 0.0; + for (int i = 0; i < steps; i++) + { + sum += posError.col(i).squaredNorm(); + sum += velError.col(i).squaredNorm(); + } + return sum; + }; + + return std::make_shared(loss, lossGrad); +} + +std::shared_ptr getSSIDVelLoss() +{ + TrajectoryLossFn loss = [](const TrajectoryRollout* rollout) { + Eigen::MatrixXs rawVel = rollout->getMetadata("velocities"); + Eigen::MatrixXs sensorVelocities = rawVel.block(0,1,rawVel.rows(),rawVel.cols()-1); + int steps = rollout->getVelsConst().cols(); + + Eigen::MatrixXs velError = rollout->getVelsConst() - sensorVelocities; + + // std::cout << "Pos Error: " << std::endl << posError << std::endl; + + s_t sum = 0.0; + for (int i = 0; i < steps; i++) + { + sum += velError.col(i).squaredNorm(); + } + return sum; + }; + + TrajectoryLossFnAndGrad lossGrad = [](const TrajectoryRollout* rollout, + TrajectoryRollout* gradWrtRollout // OUT + ) { + gradWrtRollout->getPoses().setZero(); + gradWrtRollout->getVels().setZero(); + gradWrtRollout->getControlForces().setZero(); + int steps = rollout->getVelsConst().cols(); + + Eigen::MatrixXs rawVel = rollout->getMetadata("velocities"); + Eigen::MatrixXs sensorVelocities = rawVel.block(0,1,rawVel.rows(),rawVel.cols()-1); + + Eigen::MatrixXs velError = rollout->getVelsConst() - sensorVelocities; + + gradWrtRollout->getVels() = 2 * velError; + s_t sum = 0.0; + for (int i = 0; i < steps; i++) + { + sum += velError.col(i).squaredNorm(); + } + return sum; + }; + + return std::make_shared(loss, lossGrad); +} + + +WorldPtr createWorld(s_t timestep) +{ + std::shared_ptr world = dart::utils::UniversalLoader::loadWorld( + "dart://sample/skel/acrobot.skel"); + world->setTimeStep(timestep); + + SkeletonPtr skel = world->getSkeleton(0); + world->removeDofFromActionSpace(0); + skel->getJoint(2)->setPositionUpperLimit(0, 3); + skel->getJoint(2)->setPositionLowerLimit(0, -3); + skel->getJoint(2)->setPositionLimitEnforced(true); + for(int i = 0; i < skel->getNumBodyNodes(); i++) + { + // xmate3p->getBodyNode(i)->removeAllShapeNodes(); + + for( dart::dynamics::ShapeNode* shapenode :skel->getBodyNode(i)->getShapeNodes()) + { + // Collision handling may crash current iLQR + shapenode->removeCollisionAspect(); + } + + } + return world; +} + +void clearShapeNodes(WorldPtr world) +{ + SkeletonPtr skel = world->getSkeleton(0); + for(int i = 0; i < skel->getNumBodyNodes(); i++) + { + skel->getBodyNode(i)->removeAllShapeNodes(); + } +} + +WorldPtr cloneWorld(WorldPtr world, bool keep_shapeNode) +{ + WorldPtr cloneWorld = world->clone(); + // Default the robot skeleton is the first one + SkeletonPtr skel = cloneWorld->getSkeleton(0); + if(!keep_shapeNode) + { + clearShapeNodes(cloneWorld); + } + return cloneWorld; +} + +std::mt19937 initializeRandom() +{ + std::random_device rd{}; + std::mt19937 gen{rd()}; + return gen; +} + +Eigen::VectorXs rand_normal(size_t length, s_t mean, s_t stddev, std::mt19937 random_gen) +{ + Eigen::VectorXs result = Eigen::VectorXs::Zero(length); + std::normal_distribution<> dist{mean, stddev}; + + for(int i = 0; i < length; i++) + { + result(i) += (s_t)(dist(random_gen)); + } + return result; +} + +void recordObs(size_t now, SSID* ssid, WorldPtr realtimeWorld) +{ + ssid->registerLock(); + ssid->registerControls(now, realtimeWorld->getControlForces()); + ssid->registerSensors(now, realtimeWorld->getPositions(),0); + ssid->registerSensors(now, realtimeWorld->getVelocities(),1); + ssid->registerUnlock(); +} + +void recordObsWithNoise(size_t now, SSID* ssid, WorldPtr realtimeWorld, s_t noise_scale, std::mt19937 random_gen) +{ + ssid->registerLock(); + Eigen::VectorXs control_force = realtimeWorld->getControlForces(); + // Eigen::VectorXs force_eps = rand_normal(control_force.size(), 0, noise_scale, random_gen); + ssid->registerControls(now, control_force); + + Eigen::VectorXs position = realtimeWorld->getPositions(); + Eigen::VectorXs position_eps = rand_normal(position.size(), 0, noise_scale, random_gen); + ssid->registerSensors(now, position + position_eps, 0); + + Eigen::VectorXs velocity = realtimeWorld->getVelocities(); + Eigen::VectorXs velocity_eps = rand_normal(velocity.size(), 0, noise_scale, random_gen); + ssid->registerSensors(now, velocity + velocity_eps, 1); + ssid->registerUnlock(); +} + +#ifdef iLQR_MPC_TEST +TEST(REALTIME, CARTPOLE_MPC_MASS) +{ + WorldPtr world = createWorld(5.0 / 1000); + + // Initialize Hyper Parameters + // TODO: Need to find out + int steps = 400; + int millisPerTimestep = world->getTimeStep() * 1000; + int planningHorizonMillis = steps * millisPerTimestep; + + // For add noise in measurement + // std::mt19937 rand_gen = initializeRandom(); + // s_t noise_scale = 0.01; + + // For SSID + s_t scale = 1.0; + size_t ssid_index = 1; + size_t ssid_index2 = 2; + int inferenceSteps = 10; + int inferenceHistoryMillis = inferenceSteps * millisPerTimestep; + + std::shared_ptr ssidWorld = cloneWorld(world, false); + ssidWorld->tuneMass( + world->getBodyNodeByIndex(ssid_index), + WrtMassBodyNodeEntryType::INERTIA_MASS, + Eigen::VectorXs::Ones(1) * 5.0, + Eigen::VectorXs::Ones(1) * 0.2); + + ssidWorld->tuneMass( + world->getBodyNodeByIndex(ssid_index2), + WrtMassBodyNodeEntryType::INERTIA_MASS, + Eigen::VectorXs::Ones(1) * 5.0, + Eigen::VectorXs::Ones(1) * 0.2); + + Eigen::Vector2s sensorDims(world->getNumDofs(), world->getNumDofs()); + std::vector ssid_idx{ssid_index};//, ssid_index2}; + + SSID ssid = SSID(ssidWorld, + getSSIDPosLoss(), + inferenceHistoryMillis, + sensorDims, + inferenceSteps, + scale); + std::mutex lock; + std::mutex param_lock; + ssid.attachMutex(lock); + ssid.attachParamMutex(param_lock); + // ssid.setSSIDIndex(Eigen::Vector2i(ssid_index, ssid_index2)); + Eigen::VectorXi index; + index = Eigen::VectorXi::Zero(1); + index(0) = ssid_index; + ssid.setSSIDMassIndex(index); + + ssid.setInitialPosEstimator( + [](Eigen::MatrixXs sensors, long) + { + return sensors.col(0); + }); + + ssid.setInitialVelEstimator( + [](Eigen::MatrixXs sensors, long) + { + return sensors.col(0); + }); + + world->clearTunableMassThisInstance(); + + // Create Goal + int dofs = 2; + Eigen::VectorXs runningStateWeight = Eigen::VectorXs::Zero(2 * dofs); + Eigen::VectorXs runningActionWeight = Eigen::VectorXs::Ones(1) * 0.01; + Eigen::VectorXs finalStateWeight = Eigen::VectorXs::Ones(2 * dofs) * 10.0; + finalStateWeight(0) = 1000; + finalStateWeight(1) = 1000; + + std::shared_ptr realtimeUnderlyingWorld = cloneWorld(world,true); + + //clearShapeNodes(world); + + std::shared_ptr costFn + = std::make_shared(runningStateWeight, + runningActionWeight, + finalStateWeight, + world); + + costFn->setSSIDNodeIndex(ssid_idx); + // costFn->enableSSIDLoss(0.01); + costFn->setTimeStep(world->getTimeStep()); + Eigen::Vector4s goal(0.0, 0.0, 0.0, 0.0); + + costFn->setTarget(goal); + std::cout << "Goal: " << goal << std::endl; + iLQRLocal mpcLocal = iLQRLocal( + world, 1, planningHorizonMillis, 1.0); + + mpcLocal.setCostFn(costFn); + mpcLocal.setSilent(true); + mpcLocal.setMaxIterations(10); + mpcLocal.setPatience(5); + mpcLocal.setEnableLineSearch(false); + mpcLocal.setEnableOptimizationGuards(true); + mpcLocal.setActionBound(10.0); + mpcLocal.setAlpha(1); + + ssid.registerInferListener([&](long, + Eigen::VectorXs, + Eigen::VectorXs, + Eigen::VectorXs mass, + long){ + // mpcLocal.recordGroundTruthState(time, pos, vel, mass); //TODO: This will cause problem ... But Why + mpcLocal.setParameterChange(mass); + world->setLinkMassIndex(mass(0), ssid_index); + world->setLinkMassIndex(mass(1), ssid_index2); + }); + + + + GUIWebsocketServer server; + + /* + server.createSphere("goal", 0.1, + Eigen::Vector3s(goal(0),1.0,0), + Eigen::Vector3s(1.0, 0.0, 0.0)); + server.registerDragListener("goal", [&](Eigen::Vector3s dragTo){ + goal(0) = dragTo(0); + dragTo(1) = 1.0; + dragTo(2) = 0.0; + costFn->setTarget(goal); + server.setObjectPosition("goal", dragTo); + }); + */ + + std::string key = "mass"; + + Ticker ticker = Ticker(scale * realtimeUnderlyingWorld->getTimeStep()); + long total_steps = 0; + + Eigen::Vector2s masses; + masses(0) = world->getLinkMassIndex(ssid_index); + // masses(1) = 0; + masses(1) = world->getLinkMassIndex(ssid_index2); + Eigen::Vector2s id_masses(1.0, 3.0); + + realtimeUnderlyingWorld->setLinkMassIndex(masses(0), ssid_index); + realtimeUnderlyingWorld->setLinkMassIndex(masses(1), ssid_index2); + // ssidWorld->setLinkMassIndex(id_masses(0), ssid_index); + // ssidWorld->setLinkMassIndex(id_masses(1), ssid_index2); + // world->setLinkMassIndex(id_masses(0), ssid_index); + // world->setLinkMassIndex(id_masses(1), ssid_index2); + // Preload visualization + bool renderIsReady = false; + ticker.registerTickListener([&](long now) { + // Eigen::VectorXs mpcforces = mpcLocal.getControlForce(now); + Eigen::VectorXs mpcforces; + if(renderIsReady) + { + mpcforces = mpcLocal.computeForce(realtimeUnderlyingWorld->getState(), now); + //mpcforces = Eigen::VectorXs::Ones(dofs); + } + else + { + mpcforces = Eigen::VectorXs::Zero(dofs); + } + //std::cout << "MPC Force: \n" << mpcforces + // << "\nRef forces: \n" << feedback_forces << std::endl; + // TODO: Currently the forces are almost zero need to figure out why + // std::cout <<"Force:\n" << mpcforces << std::endl; + //Eigen::VectorXs mpcforces = mpcLocal.getControlForce(now); + // Eigen::VectorXs force_eps = rand_normal(mpcforces.size(), 0, noise_scale, rand_gen); + realtimeUnderlyingWorld->setControlForces(mpcforces); + if (server.getKeysDown().count("a")) + { + Eigen::VectorXs perturbedForces + = realtimeUnderlyingWorld->getControlForces(); + perturbedForces(0) = -15.0; + // perturbedForces(6) = -15.0; + realtimeUnderlyingWorld->setControlForces(perturbedForces); + } + else if (server.getKeysDown().count("e")) + { + Eigen::VectorXs perturbedForces + = realtimeUnderlyingWorld->getControlForces(); + perturbedForces(0) = 15.0; + // perturbedForces(6) = 15.0; + realtimeUnderlyingWorld->setControlForces(perturbedForces); + } + + if (server.getKeysDown().count(",")) + { + // Increase mass + masses(0) = 3.0; + masses(1) = 2.5; + realtimeUnderlyingWorld->setLinkMassIndex(masses(0), ssid_index); + realtimeUnderlyingWorld->setLinkMassIndex(masses(1), ssid_index2); + } + else if (server.getKeysDown().count("o")) + { + // Decrease mass + masses(0) = 1.0; + masses(1) = 0.5; + realtimeUnderlyingWorld->setLinkMassIndex(masses(0), ssid_index); + realtimeUnderlyingWorld->setLinkMassIndex(masses(1), ssid_index2); + } + else if(server.getKeysDown().count("s")) + { + renderIsReady = true; + //ssid.start(); + //ssid.startSlow(); + mpcLocal.setPredictUsingFeedback(false); + mpcLocal.ilqrstart(); + } + // recordObs(now, &ssid, realtimeUnderlyingWorld); + if(renderIsReady) + { + // recordObsWithNoise(now, &ssid, realtimeUnderlyingWorld, noise_scale, rand_gen); + recordObs(now, &ssid, realtimeUnderlyingWorld); + realtimeUnderlyingWorld->step(); + } + id_masses(0) = world->getLinkMassIndex(ssid_index); + id_masses(1) = world->getLinkMassIndex(ssid_index2); + if(renderIsReady) + { + mpcLocal.recordGroundTruthState( + now, + realtimeUnderlyingWorld->getPositions(), + realtimeUnderlyingWorld->getVelocities(), + realtimeUnderlyingWorld->getMasses()); + } + + if(total_steps % 5 == 0) + { + server.renderWorld(realtimeUnderlyingWorld); + server.createText(key, + "Current Masses: "+std::to_string(id_masses(0))+" "+std::to_string(id_masses(1))+" "+ + "Real Masses: "+std::to_string(masses(0))+" "+std::to_string(masses(1)), + Eigen::Vector2i(100,100), + Eigen::Vector2i(200,200)); + // server.createText(key, + // "Current Masses: "+std::to_string(id_masses(0))+" "+"Real Masses: " + std::to_string(masses(0)), + // Eigen::Vector2i(100, 100), + // Eigen::Vector2i(400, 400)); + total_steps = 0; + } + total_steps ++; + }); + + // Should only work when trajectory opt + // We can always feed the trajectory used in forward pass here + mpcLocal.registerReplanningListener( + [&](long , + const trajectory::TrajectoryRollout* rollout, + long ) { + // std::cout << "Reached Here!" << std::endl; + server.renderTrajectoryLines(world, rollout->getPosesConst()); + }); + + server.registerConnectionListener([&](){ + ticker.start(); + + + }); + server.registerShutdownListener([&]() {mpcLocal.stop(); }); + server.serve(8070); + server.blockWhileServing(); +} +#endif \ No newline at end of file diff --git a/unittests/comprehensive/test_FullArm.cpp b/unittests/comprehensive/test_FullArm.cpp new file mode 100644 index 000000000..abeec9abc --- /dev/null +++ b/unittests/comprehensive/test_FullArm.cpp @@ -0,0 +1,595 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include + + +#include +#include +#include + +#include "dart/neural/RestorableSnapshot.hpp" +#include "dart/realtime/MPC.hpp" +#include "dart/realtime/iLQRLocal.hpp" +#include "dart/realtime/SSID.hpp" +#include "dart/realtime/Ticker.hpp" +#include "dart/server/GUIWebsocketServer.hpp" +#include "dart/simulation/World.hpp" +#include "dart/dynamics/ShapeNode.hpp" +#include "dart/trajectory/IPOptOptimizer.hpp" +#include "dart/trajectory/LossFn.hpp" +#include "dart/trajectory/MultiShot.hpp" +#include "dart/trajectory/TrajectoryRollout.hpp" +#include "dart/utils/UniversalLoader.hpp" +#include "dart/utils/urdf/urdf.hpp" + +#include "TestHelpers.hpp" +#include "stdio.h" + +//#define iLQR_MPC_TEST +#define USE_NOISE + +using namespace dart; +using namespace math; +using namespace dynamics; +using namespace simulation; +using namespace neural; +using namespace realtime; +using namespace trajectory; +using namespace server; + +std::shared_ptr getSSIDPosLoss() +{ + TrajectoryLossFn loss = [](const TrajectoryRollout* rollout) { + Eigen::MatrixXs rawPos = rollout->getMetadata("sensors"); + Eigen::MatrixXs sensorPositions = rawPos.block(0,1,rawPos.rows(),rawPos.cols()-1); + int steps = rollout->getPosesConst().cols(); + + Eigen::MatrixXs posError = rollout->getPosesConst() - sensorPositions; + + //std::cout << "Pos In Buffer: " << std::endl << sensorPositions << std::endl; + //std::cout << "Pos In Traj : " << std::endl << rollout->getPosesConst() << std::endl; + + s_t sum = 0.0; + for (int i = 0; i < steps; i++) + { + sum += posError.col(i).squaredNorm(); + } + return sum; + }; + + TrajectoryLossFnAndGrad lossGrad = [](const TrajectoryRollout* rollout, + TrajectoryRollout* gradWrtRollout // OUT + ) { + gradWrtRollout->getPoses().setZero(); + gradWrtRollout->getVels().setZero(); + gradWrtRollout->getControlForces().setZero(); + int steps = rollout->getPosesConst().cols(); + + Eigen::MatrixXs rawPos = rollout->getMetadata("sensors"); + Eigen::MatrixXs sensorPositions = rawPos.block(0,1,rawPos.rows(),rawPos.cols()-1); + + Eigen::MatrixXs posError = rollout->getPosesConst() - sensorPositions; + + gradWrtRollout->getPoses() = 2 * posError; + s_t sum = 0.0; + for (int i = 0; i < steps; i++) + { + sum += posError.col(i).squaredNorm(); + } + return sum; + }; + + return std::make_shared(loss, lossGrad); +} + +std::shared_ptr getSSIDVelPosLoss() +{ + TrajectoryLossFn loss = [](const TrajectoryRollout* rollout) { + + Eigen::MatrixXs rawPos = rollout->getMetadata("sensors"); + Eigen::MatrixXs rawVel = rollout->getMetadata("velocities"); + Eigen::MatrixXs sensorPositions = rawPos.block(0,1,rawPos.rows(),rawPos.cols()-1); + Eigen::MatrixXs sensorVelocities = rawVel.block(0,1,rawVel.rows(),rawVel.cols()-1); + int steps = rollout->getPosesConst().cols(); + + Eigen::MatrixXs posError = rollout->getPosesConst() - sensorPositions; + Eigen::MatrixXs velError = rollout->getVelsConst() - sensorVelocities; + + // std::cout << "Pos Error: " << std::endl << posError << std::endl; + + s_t sum = 0.0; + for (int i = 0; i < steps; i++) + { + sum += posError.col(i).squaredNorm(); + sum += velError.col(i).squaredNorm(); + } + return sum; + }; + + TrajectoryLossFnAndGrad lossGrad = [](const TrajectoryRollout* rollout, + TrajectoryRollout* gradWrtRollout // OUT + ) { + gradWrtRollout->getPoses().setZero(); + gradWrtRollout->getVels().setZero(); + gradWrtRollout->getControlForces().setZero(); + int steps = rollout->getPosesConst().cols(); + + Eigen::MatrixXs rawPos = rollout->getMetadata("sensors"); + Eigen::MatrixXs rawVel = rollout->getMetadata("velocities"); + Eigen::MatrixXs sensorPositions = rawPos.block(0,1,rawPos.rows(),rawPos.cols()-1); + Eigen::MatrixXs sensorVelocities = rawVel.block(0,1,rawVel.rows(),rawVel.cols()-1); + + Eigen::MatrixXs posError = rollout->getPosesConst() - sensorPositions; + Eigen::MatrixXs velError = rollout->getVelsConst() - sensorVelocities; + + gradWrtRollout->getPoses() = 2 * posError; + gradWrtRollout->getVels() = 2 * velError; + s_t sum = 0.0; + for (int i = 0; i < steps; i++) + { + sum += posError.col(i).squaredNorm(); + sum += velError.col(i).squaredNorm(); + } + return sum; + }; + + return std::make_shared(loss, lossGrad); +} + +std::shared_ptr getSSIDVelLoss() +{ + TrajectoryLossFn loss = [](const TrajectoryRollout* rollout) { + Eigen::MatrixXs rawVel = rollout->getMetadata("velocities"); + Eigen::MatrixXs sensorVelocities = rawVel.block(0,1,rawVel.rows(),rawVel.cols()-1); + int steps = rollout->getVelsConst().cols(); + + Eigen::MatrixXs velError = rollout->getVelsConst() - sensorVelocities; + + // std::cout << "Pos Error: " << std::endl << posError << std::endl; + + s_t sum = 0.0; + for (int i = 0; i < steps; i++) + { + sum += velError.col(i).squaredNorm(); + } + return sum; + }; + + TrajectoryLossFnAndGrad lossGrad = [](const TrajectoryRollout* rollout, + TrajectoryRollout* gradWrtRollout // OUT + ) { + gradWrtRollout->getPoses().setZero(); + gradWrtRollout->getVels().setZero(); + gradWrtRollout->getControlForces().setZero(); + int steps = rollout->getVelsConst().cols(); + + Eigen::MatrixXs rawVel = rollout->getMetadata("velocities"); + Eigen::MatrixXs sensorVelocities = rawVel.block(0,1,rawVel.rows(),rawVel.cols()-1); + + Eigen::MatrixXs velError = rollout->getVelsConst() - sensorVelocities; + + gradWrtRollout->getVels() = 2 * velError; + s_t sum = 0.0; + for (int i = 0; i < steps; i++) + { + sum += velError.col(i).squaredNorm(); + } + return sum; + }; + + return std::make_shared(loss, lossGrad); +} + +WorldPtr createWorld(s_t timestep) +{ + WorldPtr world = World::create(); + world->setGravity(Eigen::Vector3s(0, 0, -9.81)); + std::shared_ptr xmate3p + = dart::utils::UniversalLoader::loadSkeleton( + world.get(), + "/workspaces/nimblephysics/data/urdf/xmate3p/xmate3p.urdf"); + world->setTimeStep(timestep); + for(int i = 0; i < xmate3p->getNumDofs(); i++) + { + xmate3p->getJoint(i)->setDampingCoefficient(0, 0.01); + } + for(int i = 0; i < xmate3p->getNumBodyNodes(); i++) + { + // xmate3p->getBodyNode(i)->removeAllShapeNodes(); + for( dart::dynamics::ShapeNode* shapenode :xmate3p->getBodyNode(i)->getShapeNodes()) + { + // Collision handling may crash current iLQR + shapenode->removeCollisionAspect(); + } + + } + Eigen::Vector6s init_state; + init_state << 130.0 / 180.0 * 3.1415, 90.0 / 180.0 * 3.1415, 90.0 / 180.0 * 3.1415, 0, 0, 0; + world->setState(init_state); + return world; +} + +void clearShapeNodes(WorldPtr world) +{ + SkeletonPtr skel = world->getSkeleton(0); + for(int i = 0; i < skel->getNumBodyNodes(); i++) + { + skel->getBodyNode(i)->removeAllShapeNodes(); + } +} + +WorldPtr cloneWorld(WorldPtr world, bool keep_shapeNode) +{ + WorldPtr cloneWorld = world->clone(); + // Default the robot skeleton is the first one + SkeletonPtr skel = cloneWorld->getSkeleton(0); + if(!keep_shapeNode) + { + clearShapeNodes(cloneWorld); + } + return cloneWorld; +} + +std::mt19937 initializeRandom() +{ + std::random_device rd{}; + std::mt19937 gen{rd()}; + return gen; +} + +Eigen::VectorXs rand_normal(size_t length, s_t mean, s_t stddev, std::mt19937 random_gen) +{ + Eigen::VectorXs result = Eigen::VectorXs::Zero(length); + std::normal_distribution<> dist{mean, stddev}; + + for(int i = 0; i < length; i++) + { + result(i) += (s_t)(dist(random_gen)); + } + return result; +} + +void recordObs(size_t now, SSID* ssid, WorldPtr realtimeWorld) +{ + ssid->registerLock(); + ssid->registerControls(now, realtimeWorld->getControlForces()); + ssid->registerSensors(now, realtimeWorld->getPositions(),0); + ssid->registerSensors(now, realtimeWorld->getVelocities(),1); + ssid->registerUnlock(); +} + +void recordObsWithNoise(size_t now, SSID* ssid, WorldPtr realtimeWorld, s_t noise_scale, std::mt19937 random_gen) +{ + ssid->registerLock(); + Eigen::VectorXs control_force = realtimeWorld->getControlForces(); + ssid->registerControls(now, control_force); + + Eigen::VectorXs position = realtimeWorld->getPositions(); + Eigen::VectorXs position_eps = rand_normal(position.size(), 0, noise_scale, random_gen); + ssid->registerSensors(now, position + position_eps, 0); + + Eigen::VectorXs velocity = realtimeWorld->getVelocities(); + Eigen::VectorXs velocity_eps = rand_normal(velocity.size(), 0, noise_scale, random_gen); + ssid->registerSensors(now, velocity + velocity_eps, 1); + ssid->registerUnlock(); +} + +std::vector convert2stdvec(Eigen::VectorXs vec) +{ + std::vector stdvec; + for(int i = 0; i < vec.size(); i++) + { + stdvec.push_back(vec(i)); + } + return stdvec; +} + +Eigen::MatrixXs std2eigen(std::vector record) +{ + size_t num_record = record.size(); + Eigen::MatrixXs eigen_record = Eigen::MatrixXs::Zero(num_record, record[0].size()); + for(int i = 0; i < num_record; i++) + { + eigen_record.row(i) = record[i]; + } + return eigen_record; +} + +#ifdef iLQR_MPC_TEST +TEST(REALTIME, CARTPOLE_MPC_MASS) +{ + WorldPtr world = createWorld(0.2 / 100); + + // Initialize Hyper Parameters + int steps = 100; + int millisPerTimestep = world->getTimeStep() * 1000; + int planningHorizonMillis = steps * millisPerTimestep; + + // For add noise in measurement + #ifdef USE_NOISE + std::mt19937 rand_gen = initializeRandom(); + s_t noise_scale = 0.005; + #endif + + // For SSID + s_t scale = 1.0; + size_t ssid_index = 6; + int inferenceSteps = 5; + int inferenceHistoryMillis = inferenceSteps * millisPerTimestep; + + std::shared_ptr ssidWorld = cloneWorld(world, false); + ssidWorld->tuneMass( + world->getBodyNodeByIndex(ssid_index), + WrtMassBodyNodeEntryType::INERTIA_COM, + Eigen::Vector3s(0.2, 0.2, 0.2), + Eigen::Vector3s(0., 0., 0.)); + + Eigen::Vector2s sensorDims(world->getNumDofs(), world->getNumDofs()); + std::vector ssid_idx{ssid_index};//, ssid_index2}; + + SSID ssid = SSID(ssidWorld, + getSSIDVelLoss(), + inferenceHistoryMillis, + sensorDims, + inferenceSteps, + scale); + std::mutex lock; + std::mutex param_lock; + ssid.attachMutex(lock); + ssid.attachParamMutex(param_lock); + ssid.useSmoothing(); + ssid.useHeuristicWeight(); + ssid.useConfidence(); + ssid.setTemperature(Eigen::Vector3s(4, 4, 4)); + ssid.setThreshs(0.002, 0.5); + + Eigen::VectorXi index; + index = Eigen::VectorXi::Zero(1); + index(0) = ssid_index; + ssid.setSSIDCOMIndex(index); + + ssid.setInitialPosEstimator( + [](Eigen::MatrixXs sensors, long) + { + return sensors.col(0); + }); + + ssid.setInitialVelEstimator( + [](Eigen::MatrixXs sensors, long) + { + return sensors.col(0); + }); + + world->clearTunableMassThisInstance(); + // Create Goal + int dofs = 3; + Eigen::VectorXs runningStateWeight = Eigen::VectorXs::Zero(2 * dofs); + Eigen::VectorXs runningActionWeight = Eigen::VectorXs::Ones(dofs) * 0.2; + Eigen::VectorXs finalStateWeight = Eigen::VectorXs::Ones(2 * dofs) * 100.0; + + runningStateWeight.segment(0, dofs) = Eigen::VectorXs::Ones(dofs) * 0.01; + finalStateWeight.segment(dofs, dofs) *= 0.5; + finalStateWeight(2) *= 5; + + std::shared_ptr realtimeUnderlyingWorld = cloneWorld(world,true); + + clearShapeNodes(world); + + std::shared_ptr costFn + = std::make_shared(runningStateWeight, + runningActionWeight, + finalStateWeight, + world); + + costFn->setSSIDNodeIndex(ssid_idx); + // costFn->enableSSIDLoss(0.01); + costFn->setTimeStep(world->getTimeStep()); + Eigen::Vector6s goal; + goal << 60.0 / 180.0 * 3.1415, 90.0 / 180.0 * 3.1415, 0.0 / 180.0 * 3.1415, 0, 0, 0; + Eigen::Vector6s goal2; + goal2 << 0.0/180.0, 0, 0, 0, 0, 0; + + + costFn->setTarget(goal2); + std::cout << "Goal: " << goal << std::endl; + iLQRLocal mpcLocal = iLQRLocal( + world, dofs, planningHorizonMillis, 1.0); + mpcLocal.setCostFn(costFn); + mpcLocal.setSilent(true); + mpcLocal.setMaxIterations(5); + mpcLocal.setPatience(1); + mpcLocal.setEnableLineSearch(false); + mpcLocal.setEnableOptimizationGuards(true); + mpcLocal.setActionBound(30.0); + mpcLocal.setAlpha(1); + + ssid.registerInferListener([&](long, + Eigen::VectorXs, + Eigen::VectorXs, + Eigen::VectorXs diag_I, + long){ + // mpcLocal.recordGroundTruthState(time, pos, vel, mass); //TODO: This will cause problem ... But Why + mpcLocal.setParameterChange(diag_I); + world->setLinkCOMIndex(diag_I, ssid_index); + }); + + + + GUIWebsocketServer server; + + std::string key = "mass"; + + Ticker ticker = Ticker(scale * realtimeUnderlyingWorld->getTimeStep()); + long total_steps = 0; + + Eigen::Vector3s diag_Is; + diag_Is = world->getLinkCOMIndex(ssid_index); + // masses(1) = 0; + Eigen::Vector3s id_diag_Is(0.01, 0.02, 0.021); + + realtimeUnderlyingWorld->setLinkCOMIndex(diag_Is, ssid_index); + ssidWorld->setLinkCOMIndex(id_diag_Is, ssid_index); + world->setLinkCOMIndex(id_diag_Is, ssid_index); + // Preload visualization + bool renderIsReady = false; + int filecnt = 0; + int cnt = 0; + bool record = false; + std::vector real_record; + std::vector id_record; + ticker.registerTickListener([&](long now) { + Eigen::VectorXs mpcforces; + if(renderIsReady) + { + mpcforces = mpcLocal.computeForce(realtimeUnderlyingWorld->getState(), now); + #ifdef USE_NOISE + Eigen::VectorXs force_eps = rand_normal(mpcforces.size(), 0, noise_scale, rand_gen); + mpcforces += force_eps; + #endif + } + else + { + mpcforces = Eigen::VectorXs::Zero(dofs); + } + + realtimeUnderlyingWorld->setControlForces(mpcforces); + if (server.getKeysDown().count("a")) + { + Eigen::VectorXs perturbedForces + = realtimeUnderlyingWorld->getControlForces(); + perturbedForces(0) = -15.0; + realtimeUnderlyingWorld->setControlForces(perturbedForces); + } + else if (server.getKeysDown().count("e")) + { + Eigen::VectorXs perturbedForces + = realtimeUnderlyingWorld->getControlForces(); + perturbedForces(0) = 15.0; + realtimeUnderlyingWorld->setControlForces(perturbedForces); + } + else if (server.getKeysDown().count(",") || cnt == 300) + { + // Increase mass + diag_Is(1) = 0.025; + diag_Is(2) = 0.030; + realtimeUnderlyingWorld->setLinkCOMIndex(diag_Is, ssid_index); + } + else if (server.getKeysDown().count("c") || cnt == 500) + { + costFn->setTarget(goal2); + } + else if (server.getKeysDown().count("o") || cnt == 600) + { + // Decrease mass + diag_Is(0) = 0.050; + diag_Is(1) = 0.050; + diag_Is(2) = 0.020; + realtimeUnderlyingWorld->setLinkCOMIndex(diag_Is, ssid_index); + } + else if(server.getKeysDown().count("s")) + { + renderIsReady = true; + record = true; + ssid.start(); + mpcLocal.setPredictUsingFeedback(false); + mpcLocal.ilqrstart(); + } + else if(server.getKeysDown().count("r")) + { + record = true; + } + else if(server.getKeysDown().count("p") || cnt == 1000) + { + if(record) + { + Eigen::MatrixXs real_params = std2eigen(real_record); + Eigen::MatrixXs sysid_params = std2eigen(id_record); + std::cout << "Converted!" << std::endl; + ssid.saveCSVMatrix("/workspaces/nimblephysics/dart/realtime/saved_data/timeplots/arm_real_" + +std::to_string(filecnt)+".csv", real_params); + ssid.saveCSVMatrix("/workspaces/nimblephysics/dart/realtime/saved_data/timeplots/arm_identified_" + +std::to_string(filecnt)+".csv", sysid_params); + filecnt++; + } + record = false; + } + if(renderIsReady) + { + s_t err = (realtimeUnderlyingWorld->getState() - goal2).norm(); + if( err < 0.2) + { + std::cout << "--------------------------------------------"<< std::endl; + std::cout << "--------------------------------------------"<< std::endl; + std::cout << "Target Reached in: " << cnt << std::endl; + std::cout << "--------------------------------------------"<< std::endl; + std::cout << "--------------------------------------------"<< std::endl; + exit(0); + } + } + + if(renderIsReady) + { + #ifdef USE_NOISE + recordObsWithNoise(now, &ssid, realtimeUnderlyingWorld, noise_scale, rand_gen); + #else + recordObs(now, &ssid, realtimeUnderlyingWorld); + #endif + realtimeUnderlyingWorld->step(); + cnt++; + } + + id_diag_Is = world->getLinkCOMIndex(ssid_index); + + if(renderIsReady) + { + mpcLocal.recordGroundTruthState( + now, + realtimeUnderlyingWorld->getPositions(), + realtimeUnderlyingWorld->getVelocities(), + realtimeUnderlyingWorld->getMasses()); + } + + if(total_steps % 5 == 0) + { + server.renderWorld(realtimeUnderlyingWorld); + Eigen::Vector3s moi = realtimeUnderlyingWorld->getLinkCOMIndex(ssid_index); + server.createText(key, + "Current MOIs: "+std::to_string(id_diag_Is(0))+" "+std::to_string(id_diag_Is(1))+" "+std::to_string(id_diag_Is(2))+ + "Real MOIs: "+std::to_string(moi(0))+" "+std::to_string(moi(1))+" "+std::to_string(moi(2)), + Eigen::Vector2i(100,100), + Eigen::Vector2i(400,400)); + if(record && renderIsReady) + { + id_record.push_back(id_diag_Is); + real_record.push_back(diag_Is); + } + total_steps = 0; + } + total_steps ++; + }); + + // Should only work when trajectory opt + // We can always feed the trajectory used in forward pass here + mpcLocal.registerReplanningListener( + [&](long , + const trajectory::TrajectoryRollout* rollout, + long ) { + server.renderTrajectoryLines(world, rollout->getPosesConst()); + }); + + server.registerConnectionListener([&](){ + ticker.start(); + + + }); + server.registerShutdownListener([&]() {mpcLocal.stop(); }); + server.serve(8070); + server.blockWhileServing(); +} +#endif \ No newline at end of file diff --git a/unittests/comprehensive/test_FullContactCartpole.cpp b/unittests/comprehensive/test_FullContactCartpole.cpp new file mode 100644 index 000000000..02da640c5 --- /dev/null +++ b/unittests/comprehensive/test_FullContactCartpole.cpp @@ -0,0 +1,569 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include + + +#include +#include +#include + +#include "dart/neural/RestorableSnapshot.hpp" +#include "dart/realtime/MPC.hpp" +#include "dart/realtime/iLQRLocal.hpp" +#include "dart/realtime/SSID.hpp" +#include "dart/realtime/Ticker.hpp" +#include "dart/server/GUIWebsocketServer.hpp" +#include "dart/simulation/World.hpp" +#include "dart/trajectory/IPOptOptimizer.hpp" +#include "dart/trajectory/LossFn.hpp" +#include "dart/trajectory/MultiShot.hpp" +#include "dart/trajectory/TrajectoryRollout.hpp" + +#include "TestHelpers.hpp" +#include "stdio.h" + +//#define iLQR_MPC_TEST + +using namespace dart; +using namespace math; +using namespace dynamics; +using namespace simulation; +using namespace neural; +using namespace realtime; +using namespace trajectory; +using namespace server; + +std::shared_ptr getSSIDPosLoss() +{ + TrajectoryLossFn loss = [](const TrajectoryRollout* rollout) { + Eigen::MatrixXs rawPos = rollout->getMetadata("sensors"); + Eigen::MatrixXs sensorPositions = rawPos.block(0,1,rawPos.rows(),rawPos.cols()-1); + int steps = rollout->getPosesConst().cols(); + + Eigen::MatrixXs posError = rollout->getPosesConst() - sensorPositions; + + //std::cout << "Pos In Buffer: " << std::endl << sensorPositions << std::endl; + //std::cout << "Pos In Traj : " << std::endl << rollout->getPosesConst() << std::endl; + + s_t sum = 0.0; + for (int i = 0; i < steps; i++) + { + sum += posError.col(i).squaredNorm(); + } + return sum; + }; + + TrajectoryLossFnAndGrad lossGrad = [](const TrajectoryRollout* rollout, + TrajectoryRollout* gradWrtRollout // OUT + ) { + gradWrtRollout->getPoses().setZero(); + gradWrtRollout->getVels().setZero(); + gradWrtRollout->getControlForces().setZero(); + int steps = rollout->getPosesConst().cols(); + + Eigen::MatrixXs rawPos = rollout->getMetadata("sensors"); + Eigen::MatrixXs sensorPositions = rawPos.block(0,1,rawPos.rows(),rawPos.cols()-1); + + Eigen::MatrixXs posError = rollout->getPosesConst() - sensorPositions; + + gradWrtRollout->getPoses() = 2 * posError; + s_t sum = 0.0; + for (int i = 0; i < steps; i++) + { + sum += posError.col(i).squaredNorm(); + } + return sum; + }; + + return std::make_shared(loss, lossGrad); +} + +std::shared_ptr getSSIDVelPosLoss() +{ + TrajectoryLossFn loss = [](const TrajectoryRollout* rollout) { + + Eigen::MatrixXs rawPos = rollout->getMetadata("sensors"); + Eigen::MatrixXs rawVel = rollout->getMetadata("velocities"); + Eigen::MatrixXs sensorPositions = rawPos.block(0,1,rawPos.rows(),rawPos.cols()-1); + Eigen::MatrixXs sensorVelocities = rawVel.block(0,1,rawVel.rows(),rawVel.cols()-1); + int steps = rollout->getPosesConst().cols(); + + Eigen::MatrixXs posError = rollout->getPosesConst() - sensorPositions; + Eigen::MatrixXs velError = rollout->getVelsConst() - sensorVelocities; + + // std::cout << "Pos Error: " << std::endl << posError << std::endl; + + s_t sum = 0.0; + for (int i = 0; i < steps; i++) + { + sum += posError.col(i).squaredNorm(); + sum += velError.col(i).squaredNorm(); + } + return sum; + }; + + TrajectoryLossFnAndGrad lossGrad = [](const TrajectoryRollout* rollout, + TrajectoryRollout* gradWrtRollout // OUT + ) { + gradWrtRollout->getPoses().setZero(); + gradWrtRollout->getVels().setZero(); + gradWrtRollout->getControlForces().setZero(); + int steps = rollout->getPosesConst().cols(); + + Eigen::MatrixXs rawPos = rollout->getMetadata("sensors"); + Eigen::MatrixXs rawVel = rollout->getMetadata("velocities"); + Eigen::MatrixXs sensorPositions = rawPos.block(0,1,rawPos.rows(),rawPos.cols()-1); + Eigen::MatrixXs sensorVelocities = rawVel.block(0,1,rawVel.rows(),rawVel.cols()-1); + + Eigen::MatrixXs posError = rollout->getPosesConst() - sensorPositions; + Eigen::MatrixXs velError = rollout->getVelsConst() - sensorVelocities; + + gradWrtRollout->getPoses() = 2 * posError; + gradWrtRollout->getVels() = 2 * velError; + s_t sum = 0.0; + for (int i = 0; i < steps; i++) + { + sum += posError.col(i).squaredNorm(); + sum += velError.col(i).squaredNorm(); + } + return sum; + }; + + return std::make_shared(loss, lossGrad); +} + +std::shared_ptr getSSIDVelLoss() +{ + TrajectoryLossFn loss = [](const TrajectoryRollout* rollout) { + Eigen::MatrixXs rawVel = rollout->getMetadata("velocities"); + Eigen::MatrixXs sensorVelocities = rawVel.block(0,1,rawVel.rows(),rawVel.cols()-1); + int steps = rollout->getVelsConst().cols(); + + Eigen::MatrixXs velError = rollout->getVelsConst() - sensorVelocities; + + // std::cout << "Pos Error: " << std::endl << posError << std::endl; + + s_t sum = 0.0; + for (int i = 0; i < steps; i++) + { + sum += velError.col(i).squaredNorm(); + } + return sum; + }; + + TrajectoryLossFnAndGrad lossGrad = [](const TrajectoryRollout* rollout, + TrajectoryRollout* gradWrtRollout // OUT + ) { + gradWrtRollout->getPoses().setZero(); + gradWrtRollout->getVels().setZero(); + gradWrtRollout->getControlForces().setZero(); + int steps = rollout->getVelsConst().cols(); + + Eigen::MatrixXs rawVel = rollout->getMetadata("velocities"); + Eigen::MatrixXs sensorVelocities = rawVel.block(0,1,rawVel.rows(),rawVel.cols()-1); + + Eigen::MatrixXs velError = rollout->getVelsConst() - sensorVelocities; + + gradWrtRollout->getVels() = 2 * velError; + s_t sum = 0.0; + for (int i = 0; i < steps; i++) + { + sum += velError.col(i).squaredNorm(); + } + return sum; + }; + + return std::make_shared(loss, lossGrad); +} + +#ifdef iLQR_MPC_TEST +WorldPtr createWorld(s_t timestep) +{ + WorldPtr world = World::create(); + world->setGravity(Eigen::Vector3s(0, -9.81, 0)); + SkeletonPtr cartpole = Skeleton::create("cartpole"); + // Create Cart + std::pair sledPair + = cartpole->createJointAndBodyNodePair(nullptr); + sledPair.first->setAxis(Eigen::Vector3s(1, 0, 0)); + std::shared_ptr sledShapeBox( + new BoxShape(Eigen::Vector3s(0.5, 0.1, 0.1))); + ShapeNode* sledShape + = sledPair.second->createShapeNodeWith(sledShapeBox); + sledShape->getVisualAspect()->setColor(Eigen::Vector3s(0.5, 0.5, 0.5)); + + // Create Pole + std::pair armPair + = cartpole->createJointAndBodyNodePair(sledPair.second); + armPair.first->setAxis(Eigen::Vector3s(0, 0, 1)); + std::shared_ptr armShapeBox( + new BoxShape(Eigen::Vector3s(0.1, 1.0, 0.1))); + ShapeNode* armShape + = armPair.second->createShapeNodeWith(armShapeBox); + armShape->getVisualAspect()->setColor(Eigen::Vector3s(0.7, 0.7, 0.7)); + Eigen::Isometry3s armOffset = Eigen::Isometry3s::Identity(); + armOffset.translation() = Eigen::Vector3s(0, -0.5, 0); + armPair.first->setTransformFromChildBodyNode(armOffset); + + world->addSkeleton(cartpole); + // Add Left Plane + SkeletonPtr leftPlaneSkel = Skeleton::create("leftplane"); + std::pair leftPair + = leftPlaneSkel->createJointAndBodyNodePair(nullptr); + leftPair.first->setAxis(Eigen::Vector3s(1, 0, 0)); + leftPair.first->setSpringStiffness(0, 5.0); + leftPair.first->setDampingCoefficient(0, 0.4); + leftPair.first->setRestPosition(0, 0); + leftPair.first->setControlForceUpperLimit(0, 0); + leftPair.first->setControlForceLowerLimit(0, 0); + std::shared_ptr leftShapeBox( + new BoxShape(Eigen::Vector3s(0.1, 2.0, 3.0))); + ShapeNode* leftShape + = leftPair.second->createShapeNodeWith(leftShapeBox); + ShapeNode* leftShapeCollision + = leftPair.second->createShapeNodeWith(leftShapeBox); + leftShape->getVisualAspect()->setColor(Eigen::Vector3s(0.6, 0.6, 0.6)); + Eigen::Isometry3s leftOffset = Eigen::Isometry3s(2.5, 1.5, 0); + leftPair.first->setTransformFromParentBodyNode(leftOffset); + + SkeletonPtr rightPlaneSkel = Skeleton::create("rightplane"); + std::pair rightPair + = rightPlaneSkel->createJointAndBodyNodePair(nullptr); + rightPair.first->setAxis(Eigen::Vector3s(1, 0, 0)); + rightPair.first->setSpringStiffness(0, 5.0); + rightPair.first->setDampingCoefficient(0, 0.4); + rightPair.first->setRestPosition(0, 0); + rightPair.first->setControlForceUpperLimit(0, 0); + rightPair.first->setControlForceLowerLimit(0, 0); + std::shared_ptr rightShapeBox( + new BoxShape(Eigen::Vector3s(0.1, 2.0, 3.0))); + ShapeNode* rightShape + = rightPair.second->createShapeNodeWith(rightShapeBox); + ShapeNode* rightShapeCollision + = rightPair.second->createShapeNodeWith(rightShapeBox); + rightShape->getVisualAspect()->setColor(Eigen::Vector3s(0.6, 0.6, 0.6)); + Eigen::Isometry3s rightOffset = Eigen::Isometry3s(2.5, -1.5, 0); + rightPair.first->setTransformFromParentBodyNode(rightOffset); + + world->addSkeleton(leftPlaneSkel); + world->addSkeleton(rightPlaneSkel); + + cartpole->setControlForceUpperLimit(0, 15); + cartpole->setControlForceLowerLimit(0, -15); + cartpole->setControlForceUpperLimit(1, 0); + cartpole->setControlForceLowerLimit(1, 0); + cartpole->setVelocityUpperLimit(0, 1000); + cartpole->setVelocityLowerLimit(0, -1000); + cartpole->setPositionUpperLimit(0, 10); + cartpole->setPositionLowerLimit(0, -10); + cartpole->setPosition(0, 0); + cartpole->setPosition(1, 3.1415); + world->setTimeStep(timestep); + + return world; +} +#endif + +std::mt19937 initializeRandom() +{ + std::random_device rd{}; + std::mt19937 gen{rd()}; + return gen; +} + +Eigen::VectorXs rand_normal(size_t length, s_t mean, s_t stddev, std::mt19937 random_gen) +{ + Eigen::VectorXs result = Eigen::VectorXs::Zero(length); + std::normal_distribution<> dist{mean, stddev}; + + for(int i = 0; i < length; i++) + { + result(i) += (s_t)(dist(random_gen)); + } + return result; +} + +void recordObs(size_t now, SSID* ssid, WorldPtr realtimeWorld) +{ + ssid->registerLock(); + ssid->registerControls(now, realtimeWorld->getControlForces()); + ssid->registerSensors(now, realtimeWorld->getPositions(),0); + ssid->registerSensors(now, realtimeWorld->getVelocities(),1); + ssid->registerUnlock(); +} + +void recordObsWithNoise(size_t now, SSID* ssid, WorldPtr realtimeWorld, s_t noise_scale, std::mt19937 random_gen) +{ + ssid->registerLock(); + Eigen::VectorXs control_force = realtimeWorld->getControlForces(); + // Eigen::VectorXs force_eps = rand_normal(control_force.size(), 0, noise_scale, random_gen); + ssid->registerControls(now, control_force); + + Eigen::VectorXs position = realtimeWorld->getPositions(); + Eigen::VectorXs position_eps = rand_normal(position.size(), 0, noise_scale, random_gen); + ssid->registerSensors(now, position + position_eps, 0); + + Eigen::VectorXs velocity = realtimeWorld->getVelocities(); + Eigen::VectorXs velocity_eps = rand_normal(velocity.size(), 0, noise_scale, random_gen); + ssid->registerSensors(now, velocity + velocity_eps, 1); + ssid->registerUnlock(); +} + +#ifdef iLQR_MPC_TEST +TEST(REALTIME, CARTPOLE_MPC_MASS) +{ + WorldPtr world = createWorld(1.0 / 100); + + // Initialize Hyper Parameters + int steps = 200; + int millisPerTimestep = world->getTimeStep() * 1000; + int planningHorizonMillis = steps * millisPerTimestep; + + // For add noise in measurement + std::mt19937 rand_gen = initializeRandom(); + s_t noise_scale = 1.0 / 180 * 3.14; + + // For SSID + s_t scale = 1.0; + size_t ssid_index = 1; + size_t ssid_index2 = 2; // Left plane + int inferenceSteps = 20; + int inferenceHistoryMillis = inferenceSteps * millisPerTimestep; + + std::shared_ptr ssidWorld = world->clone(); + ssidWorld->tuneMass( + world->getBodyNodeByIndex(ssid_index), + WrtMassBodyNodeEntryType::INERTIA_MASS, + Eigen::VectorXs::Ones(1) * 5.0, + Eigen::VectorXs::Ones(1) * 0.2); + + ssidWorld->tuneMass( + world->getBodyNodeByIndex(ssid_index2), + WrtMassBodyNodeEntryType::INERTIA_MASS, + Eigen::VectorXs::Ones(1) * 5.0, + Eigen::VectorXs::Ones(1) * 0.2); + + Eigen::Vector2s sensorDims(world->getNumDofs(), world->getNumDofs()); + std::vector ssid_idx{1,2}; + + SSID ssid = SSID(ssidWorld, + getSSIDPosLoss(), + inferenceHistoryMillis, + sensorDims, + inferenceSteps, + scale); + std::mutex lock; + std::mutex param_lock; + ssid.attachMutex(lock); + ssid.attachParamMutex(param_lock); + ssid.setSSIDIndex(Eigen::Vector2i(1, 2)); + + ssid.setInitialPosEstimator( + [](Eigen::MatrixXs sensors, long) + { + return sensors.col(0); + }); + + ssid.setInitialVelEstimator( + [](Eigen::MatrixXs sensors, long) + { + return sensors.col(0); + }); + + world->clearTunableMassThisInstance(); + world->removeDofFromActionSpace(1); + world->removeDofFromActionSpace(2); + world->removeDofFromActionSpace(3); + // Create Goal + Eigen::VectorXs runningStateWeight = Eigen::VectorXs::Zero(2 * 4); + Eigen::VectorXs runningActionWeight = Eigen::VectorXs::Zero(1); + Eigen::VectorXs finalStateWeight = Eigen::VectorXs::Zero(2 * 4); + finalStateWeight(0) = 10.0; + finalStateWeight(1) = 50.0; + finalStateWeight(4) = 10.0; + finalStateWeight(5) = 10.0; + runningActionWeight(0) = 0.01; + + std::shared_ptr costFn + = std::make_shared(runningStateWeight, + runningActionWeight, + finalStateWeight, + world); + + costFn->setSSIDNodeIndex(ssid_idx); + costFn->enableSSIDLoss(0.01); + costFn->setTimeStep(world->getTimeStep()); + Eigen::VectorXs goal = Eigen::VectorXs::Zero(8); + goal(0) = 0.5; + costFn->setTarget(goal); + std::cout << "Before MPC Local Initialization" << std::endl; + iLQRLocal mpcLocal = iLQRLocal( + world, costFn, 1, planningHorizonMillis, 1.0); + + std::cout << "mpcLocal Created Successfully" << std::endl; + + mpcLocal.setSilent(true); + mpcLocal.setMaxIterations(5); + mpcLocal.setPatience(1); + mpcLocal.setEnableLineSearch(false); + mpcLocal.setEnableOptimizationGuards(true); + mpcLocal.setActionBound(20.0); + mpcLocal.setAlpha(1); + + bool init_flag = true; + + ssid.registerInferListener([&](long, + Eigen::VectorXs, + Eigen::VectorXs, + Eigen::VectorXs mass, + long){ + // mpcLocal.recordGroundTruthState(time, pos, vel, mass); //TODO: This will cause problem ... But Why + mpcLocal.setParameterChange(mass); + if(!init_flag && false) + { + s_t old_mass = world->getLinkMassIndex(ssid_index); + world->setLinkMassIndex(0.9 * old_mass + 0.1 * mass(0), ssid_index); + s_t old_mass2 = world->getLinkMassIndex(ssid_index2); + world->setLinkMassIndex(0.9 * old_mass2 + 0.1 * mass(1), ssid_index2); + } + else + { + world->setLinkMassIndex(mass(0), ssid_index); + world->setLinkMassIndex(mass(1), ssid_index2); + init_flag = false; + } + }); + + + std::shared_ptr realtimeUnderlyingWorld = world->clone(); + GUIWebsocketServer server; + + server.createSphere("goal", 0.1, + Eigen::Vector3s(goal(0),1.0,0), + Eigen::Vector3s(1.0, 0.0, 0.0)); + server.registerDragListener("goal", [&](Eigen::Vector3s dragTo){ + goal(0) = dragTo(0); + dragTo(1) = 1.0; + dragTo(2) = 0.0; + costFn->setTarget(goal); + server.setObjectPosition("goal", dragTo); + }); + + std::string key = "mass"; + + Ticker ticker = Ticker(scale * realtimeUnderlyingWorld->getTimeStep()); + + auto sledBodyVisual = realtimeUnderlyingWorld->getSkeleton("cartpole") + ->getBodyNodes()[0] + ->getShapeNodesWith()[0] + ->getVisualAspect(); + Eigen::Vector3s originalColor = sledBodyVisual->getColor(); + long total_steps = 0; + + Eigen::Vector2s masses(2.0, 1.5); + Eigen::Vector2s id_masses(1.0, 1.0); + + realtimeUnderlyingWorld->setLinkMassIndex(masses(0), ssid_index); + realtimeUnderlyingWorld->setLinkMassIndex(masses(1), ssid_index2); + ssidWorld->setLinkMassIndex(id_masses(0), ssid_index); + ssidWorld->setLinkMassIndex(id_masses(1), ssid_index2); + world->setLinkMassIndex(id_masses(0), ssid_index); + world->setLinkMassIndex(id_masses(1), ssid_index2); + + ticker.registerTickListener([&](long now) { + // Eigen::VectorXs mpcforces = mpcLocal.getControlForce(now); + Eigen::VectorXs mpcforces = mpcLocal.computeForce(realtimeUnderlyingWorld->getState(), now); + //std::cout << "MPC Force: \n" << mpcforces + // << "\nRef forces: \n" << feedback_forces << std::endl; + // TODO: Currently the forces are almost zero need to figure out why + // std::cout <<"Force:\n" << mpcforces << std::endl; + //Eigen::VectorXs mpcforces = mpcLocal.getControlForce(now); + Eigen::VectorXs force_eps = rand_normal(mpcforces.size(), 0, noise_scale, rand_gen); + realtimeUnderlyingWorld->setControlForces(mpcforces + force_eps); + if (server.getKeysDown().count("a")) + { + Eigen::VectorXs perturbedForces + = realtimeUnderlyingWorld->getControlForces(); + perturbedForces(0) = -15.0; + realtimeUnderlyingWorld->setControlForces(perturbedForces); + sledBodyVisual->setColor(Eigen::Vector3s(1, 0, 0)); + } + else if (server.getKeysDown().count("e")) + { + Eigen::VectorXs perturbedForces + = realtimeUnderlyingWorld->getControlForces(); + perturbedForces(0) = 15.0; + realtimeUnderlyingWorld->setControlForces(perturbedForces); + sledBodyVisual->setColor(Eigen::Vector3s(0, 1, 0)); + } + else + { + sledBodyVisual->setColor(originalColor); + } + if (server.getKeysDown().count(",")) + { + // Increase mass + masses(0) = 3.0; + masses(1) = 2.5; + realtimeUnderlyingWorld->setLinkMassIndex(masses(0), ssid_index); + realtimeUnderlyingWorld->setLinkMassIndex(masses(1), ssid_index2); + } + else if (server.getKeysDown().count("o")) + { + // Decrease mass + masses(0) = 1.0; + masses(1) = 0.5; + realtimeUnderlyingWorld->setLinkMassIndex(masses(0), ssid_index); + realtimeUnderlyingWorld->setLinkMassIndex(masses(1), ssid_index2); + } + // recordObs(now, &ssid, realtimeUnderlyingWorld); + recordObsWithNoise(now, &ssid, realtimeUnderlyingWorld, noise_scale, rand_gen); + realtimeUnderlyingWorld->step(); + id_masses(0) = world->getLinkMassIndex(ssid_index); + id_masses(1) = world->getLinkMassIndex(ssid_index2); + mpcLocal.recordGroundTruthState( + now, + realtimeUnderlyingWorld->getPositions(), + realtimeUnderlyingWorld->getVelocities(), + realtimeUnderlyingWorld->getMasses()); + + if(total_steps % 5 == 0) + { + server.renderWorld(realtimeUnderlyingWorld); + server.createText(key, + "Current Masses: "+std::to_string(id_masses(0))+" "+std::to_string(id_masses(1)), + Eigen::Vector2i(100,100), + Eigen::Vector2i(200,200)); + total_steps = 0; + } + total_steps ++; + }); + + // Should only work when trajectory opt + // We can always feed the trajectory used in forward pass here + mpcLocal.registerReplanningListener( + [&](long , + const trajectory::TrajectoryRollout* rollout, + long ) { + server.renderTrajectoryLines(world, rollout->getPosesConst()); + }); + + server.registerConnectionListener([&](){ + ticker.start(); + // mpcLocal.start(); + ssid.start(); + ssid.startSlow(); + mpcLocal.setPredictUsingFeedback(true); + mpcLocal.ilqrstart(); + }); + server.registerShutdownListener([&]() {mpcLocal.stop(); }); + server.serve(8070); + server.blockWhileServing(); +} +#endif \ No newline at end of file diff --git a/unittests/comprehensive/test_FullDoublependulum.cpp b/unittests/comprehensive/test_FullDoublependulum.cpp new file mode 100644 index 000000000..efdb28997 --- /dev/null +++ b/unittests/comprehensive/test_FullDoublependulum.cpp @@ -0,0 +1,604 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include + + +#include +#include +#include + +#include "dart/neural/RestorableSnapshot.hpp" +#include "dart/realtime/MPC.hpp" +#include "dart/realtime/iLQRLocal.hpp" +#include "dart/realtime/SSID.hpp" +#include "dart/realtime/Ticker.hpp" +#include "dart/server/GUIWebsocketServer.hpp" +#include "dart/simulation/World.hpp" +#include "dart/dynamics/ShapeNode.hpp" +#include "dart/trajectory/IPOptOptimizer.hpp" +#include "dart/trajectory/LossFn.hpp" +#include "dart/trajectory/MultiShot.hpp" +#include "dart/trajectory/TrajectoryRollout.hpp" +#include "dart/utils/UniversalLoader.hpp" +#include "dart/utils/urdf/urdf.hpp" + +#include "TestHelpers.hpp" +#include "stdio.h" + +//#define iLQR_MPC_TEST +#define USE_NOISE + +using namespace dart; +using namespace math; +using namespace dynamics; +using namespace simulation; +using namespace neural; +using namespace realtime; +using namespace trajectory; +using namespace server; + +std::shared_ptr getSSIDPosLoss() +{ + TrajectoryLossFn loss = [](const TrajectoryRollout* rollout) { + Eigen::MatrixXs rawPos = rollout->getMetadata("sensors"); + Eigen::MatrixXs sensorPositions = rawPos.block(0,1,rawPos.rows(),rawPos.cols()-1); + int steps = rollout->getPosesConst().cols(); + + Eigen::MatrixXs posError = rollout->getPosesConst() - sensorPositions; + + //std::cout << "Pos In Buffer: " << std::endl << sensorPositions << std::endl; + //std::cout << "Pos In Traj : " << std::endl << rollout->getPosesConst() << std::endl; + + s_t sum = 0.0; + for (int i = 0; i < steps; i++) + { + sum += posError.col(i).squaredNorm(); + } + return sum; + }; + + TrajectoryLossFnAndGrad lossGrad = [](const TrajectoryRollout* rollout, + TrajectoryRollout* gradWrtRollout // OUT + ) { + gradWrtRollout->getPoses().setZero(); + gradWrtRollout->getVels().setZero(); + gradWrtRollout->getControlForces().setZero(); + int steps = rollout->getPosesConst().cols(); + + Eigen::MatrixXs rawPos = rollout->getMetadata("sensors"); + Eigen::MatrixXs sensorPositions = rawPos.block(0,1,rawPos.rows(),rawPos.cols()-1); + + Eigen::MatrixXs posError = rollout->getPosesConst() - sensorPositions; + + gradWrtRollout->getPoses() = 2 * posError; + s_t sum = 0.0; + for (int i = 0; i < steps; i++) + { + sum += posError.col(i).squaredNorm(); + } + return sum; + }; + + return std::make_shared(loss, lossGrad); +} + +std::shared_ptr getSSIDVelPosLoss() +{ + TrajectoryLossFn loss = [](const TrajectoryRollout* rollout) { + + Eigen::MatrixXs rawPos = rollout->getMetadata("sensors"); + Eigen::MatrixXs rawVel = rollout->getMetadata("velocities"); + Eigen::MatrixXs sensorPositions = rawPos.block(0,1,rawPos.rows(),rawPos.cols()-1); + Eigen::MatrixXs sensorVelocities = rawVel.block(0,1,rawVel.rows(),rawVel.cols()-1); + int steps = rollout->getPosesConst().cols(); + + Eigen::MatrixXs posError = rollout->getPosesConst() - sensorPositions; + Eigen::MatrixXs velError = rollout->getVelsConst() - sensorVelocities; + + // std::cout << "Pos Error: " << std::endl << posError << std::endl; + + s_t sum = 0.0; + for (int i = 0; i < steps; i++) + { + sum += posError.col(i).squaredNorm(); + sum += velError.col(i).squaredNorm(); + } + return sum; + }; + + TrajectoryLossFnAndGrad lossGrad = [](const TrajectoryRollout* rollout, + TrajectoryRollout* gradWrtRollout // OUT + ) { + gradWrtRollout->getPoses().setZero(); + gradWrtRollout->getVels().setZero(); + gradWrtRollout->getControlForces().setZero(); + int steps = rollout->getPosesConst().cols(); + + Eigen::MatrixXs rawPos = rollout->getMetadata("sensors"); + Eigen::MatrixXs rawVel = rollout->getMetadata("velocities"); + Eigen::MatrixXs sensorPositions = rawPos.block(0,1,rawPos.rows(),rawPos.cols()-1); + Eigen::MatrixXs sensorVelocities = rawVel.block(0,1,rawVel.rows(),rawVel.cols()-1); + + Eigen::MatrixXs posError = rollout->getPosesConst() - sensorPositions; + Eigen::MatrixXs velError = rollout->getVelsConst() - sensorVelocities; + + gradWrtRollout->getPoses() = 2 * posError; + gradWrtRollout->getVels() = 2 * velError; + s_t sum = 0.0; + for (int i = 0; i < steps; i++) + { + sum += posError.col(i).squaredNorm(); + sum += velError.col(i).squaredNorm(); + } + return sum; + }; + + return std::make_shared(loss, lossGrad); +} + +std::shared_ptr getSSIDVelLoss() +{ + TrajectoryLossFn loss = [](const TrajectoryRollout* rollout) { + Eigen::MatrixXs rawVel = rollout->getMetadata("velocities"); + Eigen::MatrixXs sensorVelocities = rawVel.block(0,1,rawVel.rows(),rawVel.cols()-1); + int steps = rollout->getVelsConst().cols(); + + Eigen::MatrixXs velError = rollout->getVelsConst() - sensorVelocities; + + // std::cout << "Pos Error: " << std::endl << posError << std::endl; + + s_t sum = 0.0; + for (int i = 0; i < steps; i++) + { + sum += velError.col(i).squaredNorm(); + } + return sum; + }; + + TrajectoryLossFnAndGrad lossGrad = [](const TrajectoryRollout* rollout, + TrajectoryRollout* gradWrtRollout // OUT + ) { + gradWrtRollout->getPoses().setZero(); + gradWrtRollout->getVels().setZero(); + gradWrtRollout->getControlForces().setZero(); + int steps = rollout->getVelsConst().cols(); + + Eigen::MatrixXs rawVel = rollout->getMetadata("velocities"); + Eigen::MatrixXs sensorVelocities = rawVel.block(0,1,rawVel.rows(),rawVel.cols()-1); + + Eigen::MatrixXs velError = rollout->getVelsConst() - sensorVelocities; + + gradWrtRollout->getVels() = 2 * velError; + s_t sum = 0.0; + for (int i = 0; i < steps; i++) + { + sum += velError.col(i).squaredNorm(); + } + return sum; + }; + + return std::make_shared(loss, lossGrad); +} + +WorldPtr createWorld(s_t timestep) +{ + std::shared_ptr world = dart::utils::UniversalLoader::loadWorld( + "dart://sample/skel/inverted_double_pendulum.skel"); + world->setTimeStep(timestep); + world->removeDofFromActionSpace(1); + world->removeDofFromActionSpace(2); + SkeletonPtr skel = world->getSkeleton(0); + for(int i = 0; i < skel->getNumBodyNodes(); i++) + { + for( dart::dynamics::ShapeNode* shapenode :skel->getBodyNode(i)->getShapeNodes()) + { + // Collision handling may crash current iLQR + shapenode->removeCollisionAspect(); + } + } + return world; +} + +void clearShapeNodes(WorldPtr world) +{ + SkeletonPtr skel = world->getSkeleton(0); + for(int i = 0; i < skel->getNumBodyNodes(); i++) + { + skel->getBodyNode(i)->removeAllShapeNodes(); + } +} + +WorldPtr cloneWorld(WorldPtr world, bool keep_shapeNode) +{ + WorldPtr cloneWorld = world->clone(); + // Default the robot skeleton is the first one + SkeletonPtr skel = cloneWorld->getSkeleton(0); + if(!keep_shapeNode) + { + clearShapeNodes(cloneWorld); + } + return cloneWorld; +} + +std::mt19937 initializeRandom() +{ + std::random_device rd{}; + std::mt19937 gen{rd()}; + return gen; +} + +Eigen::VectorXs rand_normal(size_t length, s_t mean, s_t stddev, std::mt19937 random_gen) +{ + Eigen::VectorXs result = Eigen::VectorXs::Zero(length); + std::normal_distribution<> dist{mean, stddev}; + + for(int i = 0; i < length; i++) + { + result(i) += (s_t)(dist(random_gen)); + } + return result; +} + +void recordObs(size_t now, SSID* ssid, WorldPtr realtimeWorld) +{ + ssid->registerLock(); + ssid->registerControls(now, realtimeWorld->getControlForces()); + ssid->registerSensors(now, realtimeWorld->getPositions(),0); + ssid->registerSensors(now, realtimeWorld->getVelocities(),1); + ssid->registerUnlock(); +} + +void recordObsWithNoise(size_t now, SSID* ssid, WorldPtr realtimeWorld, s_t noise_scale, std::mt19937 random_gen) +{ + ssid->registerLock(); + Eigen::VectorXs control_force = realtimeWorld->getControlForces(); + // Eigen::VectorXs force_eps = rand_normal(control_force.size(), 0, noise_scale, random_gen); + ssid->registerControls(now, control_force); + + Eigen::VectorXs position = realtimeWorld->getPositions(); + Eigen::VectorXs position_eps = rand_normal(position.size(), 0, noise_scale, random_gen); + ssid->registerSensors(now, position + position_eps, 0); + + Eigen::VectorXs velocity = realtimeWorld->getVelocities(); + Eigen::VectorXs velocity_eps = rand_normal(velocity.size(), 0, noise_scale, random_gen); + ssid->registerSensors(now, velocity + velocity_eps, 1); + ssid->registerUnlock(); +} + +std::vector convert2stdvec(Eigen::VectorXs vec) +{ + std::vector stdvec; + for(int i = 0; i < vec.size(); i++) + { + stdvec.push_back(vec(i)); + } + return stdvec; +} + +Eigen::MatrixXs std2eigen(std::vector record) +{ + size_t num_record = record.size(); + Eigen::MatrixXs eigen_record = Eigen::MatrixXs::Zero(num_record, record[0].size()); + for(int i = 0; i < num_record; i++) + { + eigen_record.row(i) = record[i]; + } + return eigen_record; +} + +#ifdef iLQR_MPC_TEST +TEST(REALTIME, CARTPOLE_MPC_MASS) +{ + WorldPtr world = createWorld(0.5 / 100); + + // Initialize Hyper Parameters + // TODO: Need to find out + // An impression is that the longer horizon is bad + int steps = 150; + int millisPerTimestep = world->getTimeStep() * 1000; + int planningHorizonMillis = steps * millisPerTimestep; + + // For add noise in measurement + #ifdef USE_NOISE + std::mt19937 rand_gen = initializeRandom(); + s_t noise_scale = 0.005; + #endif + + // For SSID + s_t scale = 1.0; + size_t damp_index1 = 1; + size_t damp_index2 = 2; + int inferenceSteps = 10; + int inferenceHistoryMillis = inferenceSteps * millisPerTimestep; + + std::shared_ptr ssidWorld = cloneWorld(world, false); + + Eigen::VectorXi dof_index = Eigen::VectorXi::Zero(1); + dof_index(0) = damp_index1; + + ssidWorld->tuneDamping( + world->getJointIndex(damp_index1), + WrtDampingJointEntryType::DAMPING, + dof_index, + Eigen::VectorXs::Ones(1) * 0.5, + Eigen::VectorXs::Ones(1) * 0.0); + + dof_index(0) = damp_index2; + + ssidWorld->tuneDamping( + world->getJointIndex(damp_index2), + WrtDampingJointEntryType::DAMPING, + dof_index, + Eigen::VectorXs::Ones(1) * 0.5, + Eigen::VectorXs::Ones(1) * 0.0); + + Eigen::Vector2s sensorDims(world->getNumDofs(), world->getNumDofs()); + + SSID ssid = SSID(ssidWorld, + getSSIDVelPosLoss(), + inferenceHistoryMillis, + sensorDims, + inferenceSteps, + scale); + std::mutex lock; + std::mutex param_lock; + ssid.attachMutex(lock); + ssid.attachParamMutex(param_lock); + //ssid.useHeuristicWeight(); + //ssid.useSmoothing(); + //ssid.useConfidence(); + // TODO: Need Fine tune + ssid.setTemperature(Eigen::Vector2s(0.5, 0.5)); + // ssid.setSSIDIndex(Eigen::Vector2i(ssid_index, ssid_index2)); + Eigen::VectorXi index; + index = Eigen::VectorXi::Zero(2); + index(0) = damp_index1; + index(1) = damp_index2; + ssid.setSSIDDampIndex(index); + + ssid.setInitialPosEstimator( + [](Eigen::MatrixXs sensors, long) + { + return sensors.col(0); + }); + + ssid.setInitialVelEstimator( + [](Eigen::MatrixXs sensors, long) + { + return sensors.col(0); + }); + + world->clearTunableMassThisInstance(); + world->clearTunableDampingThisInstance(); + // Create Goal + int dofs = 3; + Eigen::VectorXs runningStateWeight = Eigen::VectorXs::Zero(2 * dofs); + Eigen::VectorXs runningActionWeight = Eigen::VectorXs::Ones(1) * 0.01; + Eigen::VectorXs finalStateWeight = Eigen::VectorXs::Zero(2 * dofs); + finalStateWeight << 10.0, 50, 10, 10, 10 ,10; + + std::shared_ptr realtimeUnderlyingWorld = cloneWorld(world,true); + + std::shared_ptr costFn + = std::make_shared(runningStateWeight, + runningActionWeight, + finalStateWeight, + world); + + // costFn->setSSIDNodeIndex(ssid_idx); // Shouldn't use this + // costFn->enableSSIDLoss(0.01); // Shouldn't use this + costFn->setTimeStep(world->getTimeStep()); + Eigen::Vector6s goal; + goal << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0; + + costFn->setTarget(goal); + std::cout << "Goal: " << goal << std::endl; + iLQRLocal mpcLocal = iLQRLocal( + world, 1, planningHorizonMillis, 1.0); + mpcLocal.setCostFn(costFn); + mpcLocal.setSilent(true); + mpcLocal.setMaxIterations(5); + mpcLocal.setPatience(1); + mpcLocal.setEnableLineSearch(false); + mpcLocal.setEnableOptimizationGuards(true); + mpcLocal.setActionBound(20.0); + mpcLocal.setAlpha(1.0); + + ssid.registerInferListener([&](long, + Eigen::VectorXs, + Eigen::VectorXs, + Eigen::VectorXs param, + long){ + // mpcLocal.recordGroundTruthState(time, pos, vel, mass); //TODO: This will cause problem ... But Why + mpcLocal.setParameterChange(param); + world->setJointDampingCoeffIndex(param.segment(0,1), damp_index1); + world->setJointDampingCoeffIndex(param.segment(1,1), damp_index2); + }); + + + + GUIWebsocketServer server; + + /* + server.createSphere("goal", 0.1, + Eigen::Vector3s(goal(0),1.0,0), + Eigen::Vector3s(1.0, 0.0, 0.0)); + server.registerDragListener("goal", [&](Eigen::Vector3s dragTo){ + goal(0) = dragTo(0); + dragTo(1) = 1.0; + dragTo(2) = 0.0; + costFn->setTarget(goal); + server.setObjectPosition("goal", dragTo); + }); + */ + + std::string key = "Param"; + + Ticker ticker = Ticker(scale * realtimeUnderlyingWorld->getTimeStep()); + long total_steps = 0; + + Eigen::Vector2s params; + params(0) = world->getJointDampingCoeffIndex(damp_index1)(0); + // masses(1) = 0; + params(1) = world->getJointDampingCoeffIndex(damp_index2)(0); + Eigen::Vector2s id_params(0.1, 0.1); + + realtimeUnderlyingWorld->setJointDampingCoeffIndex(params.segment(0,1), damp_index1); + realtimeUnderlyingWorld->setJointDampingCoeffIndex(params.segment(1,1), damp_index2); + ssidWorld->setJointDampingCoeffIndex(id_params.segment(0, 1), damp_index1); + ssidWorld->setJointDampingCoeffIndex(id_params.segment(1, 1), damp_index2); + world->setJointDampingCoeffIndex(id_params.segment(0, 1), damp_index1); + world->setJointDampingCoeffIndex(id_params.segment(1, 1), damp_index2); + // Preload visualization + bool renderIsReady = false; + int filecnt = 0; + int cnt = 0; + bool record = false; + std::vector real_record; + std::vector id_record; + ticker.registerTickListener([&](long now) { + Eigen::VectorXs mpcforces; + if(renderIsReady) + { + + mpcforces = mpcLocal.computeForce(realtimeUnderlyingWorld->getState(), now); + #ifdef USE_NOISE + Eigen::VectorXs force_eps = rand_normal(mpcforces.size(), 0, noise_scale, rand_gen); + mpcforces += force_eps; + #endif + } + else + { + mpcforces = Eigen::VectorXs::Zero(dofs); + } + + realtimeUnderlyingWorld->setControlForces(mpcforces); + if (server.getKeysDown().count("a")) + { + Eigen::VectorXs perturbedForces + = realtimeUnderlyingWorld->getControlForces(); + perturbedForces(0) = -15.0; + // perturbedForces(6) = -15.0; + realtimeUnderlyingWorld->setControlForces(perturbedForces); + } + else if (server.getKeysDown().count("e")) + { + Eigen::VectorXs perturbedForces + = realtimeUnderlyingWorld->getControlForces(); + perturbedForces(0) = 15.0; + // perturbedForces(6) = 15.0; + realtimeUnderlyingWorld->setControlForces(perturbedForces); + } + + if (server.getKeysDown().count(",") || cnt == 300) + { + // Increase + params(0) = 0.4; + params(1) = 0.2; + realtimeUnderlyingWorld->setJointDampingCoeffIndex(params.segment(0, 1), damp_index1); + realtimeUnderlyingWorld->setJointDampingCoeffIndex(params.segment(1, 1), damp_index2); + } + else if (server.getKeysDown().count("o") || cnt == 600) + { + // Decrease mass + params(0) = 0.3; + params(1) = 0.3; + realtimeUnderlyingWorld->setJointDampingCoeffIndex(params.segment(0, 1), damp_index1); + realtimeUnderlyingWorld->setJointDampingCoeffIndex(params.segment(1, 1), damp_index2); + } + // else if(server.getKeysDown().count("l") || cnt == 600) + // { + // params(0) = 0.2; + // params(1) = 0.4; + // realtimeUnderlyingWorld->setJointDampingCoeffIndex(params.segment(0, 1), damp_index1); + // realtimeUnderlyingWorld->setJointDampingCoeffIndex(params.segment(1, 1), damp_index2); + // } + else if(server.getKeysDown().count("s")) + { + renderIsReady = true; + record = true; + ssid.start(); + mpcLocal.setPredictUsingFeedback(false); + mpcLocal.ilqrstart(); + } + else if(server.getKeysDown().count("r")) + { + record = true; + } + else if(server.getKeysDown().count("p") || cnt == 1000) + { + if(record) + { + Eigen::MatrixXs real_params = std2eigen(real_record); + Eigen::MatrixXs sysid_params = std2eigen(id_record); + std::cout << "Converted!" << std::endl; + ssid.saveCSVMatrix("/workspaces/nimblephysics/dart/realtime/saved_data/timeplots/dinvp_real_"+std::to_string(filecnt)+".csv", real_params); + ssid.saveCSVMatrix("/workspaces/nimblephysics/dart/realtime/saved_data/timeplots/dinvp_identified_"+std::to_string(filecnt)+".csv", sysid_params); + filecnt ++; + } + record = false; + } + if(renderIsReady) + { + #ifdef USE_NOISE + recordObsWithNoise(now, &ssid, realtimeUnderlyingWorld, noise_scale, rand_gen); + #else + recordObs(now, &ssid, realtimeUnderlyingWorld); + #endif + realtimeUnderlyingWorld->step(); + cnt ++; + } + id_params.segment(0, 1) = world->getJointDampingCoeffIndex(damp_index1); + id_params.segment(1, 1) = world->getJointDampingCoeffIndex(damp_index2); + if(renderIsReady) + { + mpcLocal.recordGroundTruthState( + now, + realtimeUnderlyingWorld->getPositions(), + realtimeUnderlyingWorld->getVelocities(), + realtimeUnderlyingWorld->getMasses()); + } + + if(total_steps % 5 == 0) + { + server.renderWorld(realtimeUnderlyingWorld); + server.createText(key, + "Current Params: "+std::to_string(id_params(0))+" "+std::to_string(id_params(1))+" "+ + "Real Params: "+std::to_string(params(0))+" "+std::to_string(params(1)), + Eigen::Vector2i(100,100), + Eigen::Vector2i(200,200)); + if(record && renderIsReady) + { + id_record.push_back(id_params); + real_record.push_back(params); + } + total_steps = 0; + } + total_steps ++; + }); + + // Should only work when trajectory opt + // We can always feed the trajectory used in forward pass here + mpcLocal.registerReplanningListener( + [&](long , + const trajectory::TrajectoryRollout* rollout, + long ) { + server.renderTrajectoryLines(world, rollout->getPosesConst()); + }); + + server.registerConnectionListener([&](){ + ticker.start(); + + + }); + server.registerShutdownListener([&]() {mpcLocal.stop(); }); + server.serve(8070); + server.blockWhileServing(); +} +#endif \ No newline at end of file diff --git a/unittests/comprehensive/test_FullWhip.cpp b/unittests/comprehensive/test_FullWhip.cpp new file mode 100644 index 000000000..84937850d --- /dev/null +++ b/unittests/comprehensive/test_FullWhip.cpp @@ -0,0 +1,649 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include + + +#include +#include +#include + +#include "dart/neural/RestorableSnapshot.hpp" +#include "dart/neural/IKMapping.hpp" +#include "dart/realtime/MPC.hpp" +#include "dart/realtime/iLQRLocal.hpp" +#include "dart/realtime/SSID.hpp" +#include "dart/realtime/Ticker.hpp" +#include "dart/server/GUIWebsocketServer.hpp" +#include "dart/simulation/World.hpp" +#include "dart/dynamics/ShapeNode.hpp" +#include "dart/trajectory/IPOptOptimizer.hpp" +#include "dart/trajectory/LossFn.hpp" +#include "dart/trajectory/MultiShot.hpp" +#include "dart/trajectory/TrajectoryRollout.hpp" +#include "dart/utils/UniversalLoader.hpp" +#include "dart/utils/urdf/urdf.hpp" + +#include "TestHelpers.hpp" +#include "stdio.h" + +//#define iLQR_MPC_TEST +#define USE_NOISE + +using namespace dart; +using namespace math; +using namespace dynamics; +using namespace simulation; +using namespace neural; +using namespace realtime; +using namespace trajectory; +using namespace server; + +std::shared_ptr getSSIDPosLoss() +{ + TrajectoryLossFn loss = [](const TrajectoryRollout* rollout) { + Eigen::MatrixXs rawPos = rollout->getMetadata("sensors"); + Eigen::MatrixXs sensorPositions = rawPos.block(0,1,rawPos.rows(),rawPos.cols()-1); + int steps = rollout->getPosesConst().cols(); + + Eigen::MatrixXs posError = rollout->getPosesConst() - sensorPositions; + + //std::cout << "Pos In Buffer: " << std::endl << sensorPositions << std::endl; + //std::cout << "Pos In Traj : " << std::endl << rollout->getPosesConst() << std::endl; + + s_t sum = 0.0; + for (int i = 0; i < steps; i++) + { + sum += posError.col(i).squaredNorm(); + } + return sum; + }; + + TrajectoryLossFnAndGrad lossGrad = [](const TrajectoryRollout* rollout, + TrajectoryRollout* gradWrtRollout // OUT + ) { + gradWrtRollout->getPoses().setZero(); + gradWrtRollout->getVels().setZero(); + gradWrtRollout->getControlForces().setZero(); + int steps = rollout->getPosesConst().cols(); + + Eigen::MatrixXs rawPos = rollout->getMetadata("sensors"); + Eigen::MatrixXs sensorPositions = rawPos.block(0,1,rawPos.rows(),rawPos.cols()-1); + + Eigen::MatrixXs posError = rollout->getPosesConst() - sensorPositions; + + gradWrtRollout->getPoses() = 2 * posError; + s_t sum = 0.0; + for (int i = 0; i < steps; i++) + { + sum += posError.col(i).squaredNorm(); + } + return sum; + }; + + return std::make_shared(loss, lossGrad); +} + +std::shared_ptr getSSIDVelPosLoss() +{ + TrajectoryLossFn loss = [](const TrajectoryRollout* rollout) { + + Eigen::MatrixXs rawPos = rollout->getMetadata("sensors"); + Eigen::MatrixXs rawVel = rollout->getMetadata("velocities"); + Eigen::MatrixXs sensorPositions = rawPos.block(0,1,rawPos.rows(),rawPos.cols()-1); + Eigen::MatrixXs sensorVelocities = rawVel.block(0,1,rawVel.rows(),rawVel.cols()-1); + int steps = rollout->getPosesConst().cols(); + + Eigen::MatrixXs posError = rollout->getPosesConst() - sensorPositions; + Eigen::MatrixXs velError = rollout->getVelsConst() - sensorVelocities; + + // std::cout << "Pos Error: " << std::endl << posError << std::endl; + + s_t sum = 0.0; + for (int i = 0; i < steps; i++) + { + sum += posError.col(i).squaredNorm(); + sum += velError.col(i).squaredNorm(); + } + return sum; + }; + + TrajectoryLossFnAndGrad lossGrad = [](const TrajectoryRollout* rollout, + TrajectoryRollout* gradWrtRollout // OUT + ) { + gradWrtRollout->getPoses().setZero(); + gradWrtRollout->getVels().setZero(); + gradWrtRollout->getControlForces().setZero(); + int steps = rollout->getPosesConst().cols(); + + Eigen::MatrixXs rawPos = rollout->getMetadata("sensors"); + Eigen::MatrixXs rawVel = rollout->getMetadata("velocities"); + Eigen::MatrixXs sensorPositions = rawPos.block(0,1,rawPos.rows(),rawPos.cols()-1); + Eigen::MatrixXs sensorVelocities = rawVel.block(0,1,rawVel.rows(),rawVel.cols()-1); + + Eigen::MatrixXs posError = rollout->getPosesConst() - sensorPositions; + Eigen::MatrixXs velError = rollout->getVelsConst() - sensorVelocities; + + gradWrtRollout->getPoses() = 2 * posError; + gradWrtRollout->getVels() = 2 * velError; + s_t sum = 0.0; + for (int i = 0; i < steps; i++) + { + sum += posError.col(i).squaredNorm(); + sum += velError.col(i).squaredNorm(); + } + return sum; + }; + + return std::make_shared(loss, lossGrad); +} + +std::shared_ptr getSSIDVelLoss() +{ + TrajectoryLossFn loss = [](const TrajectoryRollout* rollout) { + Eigen::MatrixXs rawVel = rollout->getMetadata("velocities"); + Eigen::MatrixXs sensorVelocities = rawVel.block(0,1,rawVel.rows(),rawVel.cols()-1); + int steps = rollout->getVelsConst().cols(); + + Eigen::MatrixXs velError = rollout->getVelsConst() - sensorVelocities; + + // std::cout << "Pos Error: " << std::endl << posError << std::endl; + + s_t sum = 0.0; + for (int i = 0; i < steps; i++) + { + sum += velError.col(i).squaredNorm(); + } + return sum; + }; + + TrajectoryLossFnAndGrad lossGrad = [](const TrajectoryRollout* rollout, + TrajectoryRollout* gradWrtRollout // OUT + ) { + gradWrtRollout->getPoses().setZero(); + gradWrtRollout->getVels().setZero(); + gradWrtRollout->getControlForces().setZero(); + int steps = rollout->getVelsConst().cols(); + + Eigen::MatrixXs rawVel = rollout->getMetadata("velocities"); + Eigen::MatrixXs sensorVelocities = rawVel.block(0,1,rawVel.rows(),rawVel.cols()-1); + + Eigen::MatrixXs velError = rollout->getVelsConst() - sensorVelocities; + + gradWrtRollout->getVels() = 2 * velError; + s_t sum = 0.0; + for (int i = 0; i < steps; i++) + { + sum += velError.col(i).squaredNorm(); + } + return sum; + }; + + return std::make_shared(loss, lossGrad); +} + +WorldPtr createWorld(s_t timestep) +{ + std::shared_ptr world = dart::utils::UniversalLoader::loadWorld( + "dart://sample/skel/whip2d.skel"); + world->setTimeStep(timestep); + world->removeDofFromActionSpace(1); + world->removeDofFromActionSpace(2); + world->removeDofFromActionSpace(3); + Eigen::VectorXs init_state = Eigen::VectorXs::Zero(8); + s_t pi = 3.14159; + init_state << 0.0, 30.0/180 * pi, 30.0/180 * pi, 30.0 /180 * pi, 0, 0, 0, 0; + world->setState(init_state); + SkeletonPtr skel = world->getSkeleton(0); + for(int i = 0; i < skel->getNumBodyNodes(); i++) + { + for( dart::dynamics::ShapeNode* shapenode :skel->getBodyNode(i)->getShapeNodes()) + { + // Collision handling may crash current iLQR + shapenode->removeCollisionAspect(); + } + } + return world; +} + +void clearShapeNodes(WorldPtr world) +{ + SkeletonPtr skel = world->getSkeleton(0); + for(int i = 0; i < skel->getNumBodyNodes(); i++) + { + skel->getBodyNode(i)->removeAllShapeNodes(); + } +} + +WorldPtr cloneWorld(WorldPtr world, bool keep_shapeNode) +{ + WorldPtr cloneWorld = world->clone(); + // Default the robot skeleton is the first one + SkeletonPtr skel = cloneWorld->getSkeleton(0); + if(!keep_shapeNode) + { + clearShapeNodes(cloneWorld); + } + return cloneWorld; +} + +std::mt19937 initializeRandom() +{ + std::random_device rd{}; + std::mt19937 gen{rd()}; + return gen; +} + +Eigen::VectorXs rand_normal(size_t length, s_t mean, s_t stddev, std::mt19937 random_gen) +{ + Eigen::VectorXs result = Eigen::VectorXs::Zero(length); + std::normal_distribution<> dist{mean, stddev}; + + for(int i = 0; i < length; i++) + { + result(i) += (s_t)(dist(random_gen)); + } + return result; +} + +void recordObs(size_t now, SSID* ssid, WorldPtr realtimeWorld) +{ + ssid->registerLock(); + ssid->registerControls(now, realtimeWorld->getControlForces()); + ssid->registerSensors(now, realtimeWorld->getPositions(),0); + ssid->registerSensors(now, realtimeWorld->getVelocities(),1); + ssid->registerUnlock(); +} + +void recordObsWithNoise(size_t now, SSID* ssid, WorldPtr realtimeWorld, s_t noise_scale, std::mt19937 random_gen) +{ + ssid->registerLock(); + Eigen::VectorXs control_force = realtimeWorld->getControlForces(); + ssid->registerControls(now, control_force); + + Eigen::VectorXs position = realtimeWorld->getPositions(); + Eigen::VectorXs position_eps = rand_normal(position.size(), 0, noise_scale, random_gen); + ssid->registerSensors(now, position + position_eps, 0); + + Eigen::VectorXs velocity = realtimeWorld->getVelocities(); + Eigen::VectorXs velocity_eps = rand_normal(velocity.size(), 0, noise_scale, random_gen); + ssid->registerSensors(now, velocity + velocity_eps, 1); + ssid->registerUnlock(); +} + +std::vector convert2stdvec(Eigen::VectorXs vec) +{ + std::vector stdvec; + for(int i = 0; i < vec.size(); i++) + { + stdvec.push_back(vec(i)); + } + return stdvec; +} + +Eigen::MatrixXs std2eigen(std::vector record) +{ + size_t num_record = record.size(); + Eigen::MatrixXs eigen_record = Eigen::MatrixXs::Zero(num_record, record[0].size()); + for(int i = 0 ; i < num_record; i++) + { + eigen_record.row(i) = record[i]; + } + return eigen_record; +} + +#ifdef iLQR_MPC_TEST +TEST(REALTIME, CARTPOLE_MPC_MASS) +{ + WorldPtr world = createWorld(2.0 / 1000); + + // Initialize Hyper Parameters + // TODO: Need to find out + int steps = 300; + int millisPerTimestep = world->getTimeStep() * 1000; + int planningHorizonMillis = steps * millisPerTimestep; + + // For add noise in measurement + #ifdef USE_NOISE + std::mt19937 rand_gen = initializeRandom(); + s_t noise_scale = 0.01; + #endif + + // For SSID + s_t scale = 1.0; + size_t ssid_index = 1; + size_t ssid_index2 = 2; + size_t ssid_index3 = 3; + int inferenceSteps = 10; + int inferenceHistoryMillis = inferenceSteps * millisPerTimestep; + + std::shared_ptr ssidWorld = cloneWorld(world, false); + Eigen::VectorXi index = Eigen::VectorXi::Ones(1); + + ssidWorld->tuneSpring( + world->getJointIndex(ssid_index), + WrtSpringJointEntryType::SPRING, + index, + Eigen::VectorXs::Ones(1) * 15.0, + Eigen::VectorXs::Ones(1) * 0.1); + + index(0) = 2; + ssidWorld->tuneSpring( + world->getJointIndex(ssid_index2), + WrtSpringJointEntryType::SPRING, + index, + Eigen::VectorXs::Ones(1) * 15.0, + Eigen::VectorXs::Ones(1) * 0.1); + + + index(0) = 3; + ssidWorld->tuneSpring( + world->getJointIndex(ssid_index3), + WrtSpringJointEntryType::SPRING, + index, + Eigen::VectorXs::Ones(1) * 15.0, + Eigen::VectorXs::Ones(1) * 0.1); + + Eigen::Vector2s sensorDims(world->getNumDofs(), world->getNumDofs()); + + SSID ssid = SSID(ssidWorld, + getSSIDPosLoss(), + inferenceHistoryMillis, + sensorDims, + inferenceSteps, + scale); + std::mutex lock; + std::mutex param_lock; + ssid.attachMutex(lock); + ssid.attachParamMutex(param_lock); + + ssid.useSmoothing(); + //ssid.useHeuristicWeight(); + //ssid.useConfidence(); + ssid.setTemperature(Eigen::Vector3s(0.1, 0.1, 0.1)); + ssid.setThreshs(0.3, 0.5); + + + + Eigen::Vector3i id_index(ssid_index, ssid_index2, ssid_index3); + ssid.setSSIDSpringIndex(id_index); + + ssid.setInitialPosEstimator( + [](Eigen::MatrixXs sensors, long) + { + return sensors.col(0); + }); + + ssid.setInitialVelEstimator( + [](Eigen::MatrixXs sensors, long) + { + return sensors.col(0); + }); + + world->clearTunableSpringThisInstance(); + // Create Goal + int dofs = 4; + Eigen::VectorXs runningStateWeight = Eigen::VectorXs::Ones(2 * dofs) * 0.01; // Need it to be as fast as possible + Eigen::VectorXs runningActionWeight = Eigen::VectorXs::Ones(1) * 0.001; + Eigen::VectorXs finalStateWeight = Eigen::VectorXs::Zero(2 * dofs); + // Requires IK which is not implemented + finalStateWeight << 100, 100, 100, 100, 50, 100, 100, 100; + + std::shared_ptr realtimeUnderlyingWorld = cloneWorld(world,true); + + std::shared_ptr costFn + = std::make_shared(runningStateWeight, + runningActionWeight, + finalStateWeight, + world); + + // The objective is the elastic rod vibrate between the two goal + //s_t pi = 3.14159265; + Eigen::VectorXs goal = Eigen::VectorXs::Zero(2 * dofs); + goal << 2.5, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0; // Up right pose + + costFn->setTarget(goal); + // Need to change the coefficient on the fly + // costFn->setSSIDSpringJointIndex(Eigen::Vector3i(1, 2, 3)); + // costFn->enableSSIDLoss(1); + iLQRLocal mpcLocal = iLQRLocal( + world, 1, planningHorizonMillis, 1.0); + + mpcLocal.setCostFn(costFn); + mpcLocal.setSilent(true); + mpcLocal.setMaxIterations(5); + mpcLocal.setPatience(3); + mpcLocal.setEnableLineSearch(false); + mpcLocal.setEnableOptimizationGuards(true); + mpcLocal.setActionBound(40.0); + mpcLocal.setAlpha(1.0); + + ssid.registerInferListener([&](long, + Eigen::VectorXs, + Eigen::VectorXs confidence, + Eigen::VectorXs spring, + long){ + // mpcLocal.recordGroundTruthState(time, pos, vel, mass); //TODO: This will cause problem ... But Why + mpcLocal.setParameterChange(spring); + world->setJointSpringStiffIndex(spring.segment(0, 1), ssid_index); + world->setJointSpringStiffIndex(spring.segment(1, 1), ssid_index2); + world->setJointSpringStiffIndex(spring.segment(2, 1), ssid_index3); + // Should interface here to change the weight of the system + if(confidence.mean() > 0.2) + { + costFn->setSSIDHeuristicWeight(0); + } + }); + + + + GUIWebsocketServer server; + + + // May need to create the sphere for better visualization + + server.createSphere("goal1", 0.1, + Eigen::Vector3s(goal(0), 0.0, 0), + Eigen::Vector3s(1.0, 0.0, 0.0)); + server.registerDragListener("goal1", [&](Eigen::Vector3s dragTo){ + goal(0) = dragTo(0); + dragTo(1) = 0.0; + dragTo(2) = 0.0; + costFn->setTarget(goal); + server.setObjectPosition("goal1", dragTo); + }); + + + std::string key = "spring"; + + Ticker ticker = Ticker(scale * realtimeUnderlyingWorld->getTimeStep()); + long total_steps = 0; + + Eigen::Vector3s spring_stiffs; + spring_stiffs(0) = world->getJointSpringStiffIndex(ssid_index)(0); + spring_stiffs(1) = world->getJointSpringStiffIndex(ssid_index2)(0); + spring_stiffs(2) = world->getJointSpringStiffIndex(ssid_index3)(0); + Eigen::Vector3s id_spring_stiffs(5.0, 3.0, 2.0); + + realtimeUnderlyingWorld->setJointSpringStiffIndex(spring_stiffs.segment(0, 1), ssid_index); + realtimeUnderlyingWorld->setJointSpringStiffIndex(spring_stiffs.segment(1, 1), ssid_index2); + realtimeUnderlyingWorld->setJointSpringStiffIndex(spring_stiffs.segment(2, 1), ssid_index3); + ssidWorld->setJointSpringStiffIndex(id_spring_stiffs.segment(0, 1), ssid_index); + ssidWorld->setJointSpringStiffIndex(id_spring_stiffs.segment(1, 1), ssid_index2); + ssidWorld->setJointSpringStiffIndex(id_spring_stiffs.segment(2, 1), ssid_index3); + world->setJointSpringStiffIndex(id_spring_stiffs.segment(0, 1), ssid_index); + world->setJointSpringStiffIndex(id_spring_stiffs.segment(1, 1), ssid_index2); + world->setJointSpringStiffIndex(id_spring_stiffs.segment(2, 1), ssid_index3); + // Preload visualization + bool renderIsReady = false; + int filecnt = 0; + int cnt = 0; + bool record = false; + std::vector real_record; + std::vector id_record; + + ticker.registerTickListener([&](long now) { + // Eigen::VectorXs mpcforces = mpcLocal.getControlForce(now); + Eigen::VectorXs mpcforces; + if(renderIsReady) + { + mpcforces = mpcLocal.computeForce(realtimeUnderlyingWorld->getState(), now); + #ifdef USE_NOISE + Eigen::VectorXs force_eps = rand_normal(mpcforces.size(), 0, noise_scale, rand_gen); + mpcforces += force_eps; + #endif + } + else + { + mpcforces = Eigen::VectorXs::Zero(dofs); + } + + + realtimeUnderlyingWorld->setControlForces(mpcforces); + if (server.getKeysDown().count("a")) + { + Eigen::VectorXs perturbedForces + = realtimeUnderlyingWorld->getControlForces(); + perturbedForces(0) = -15.0; + // perturbedForces(6) = -15.0; + realtimeUnderlyingWorld->setControlForces(perturbedForces); + } + else if (server.getKeysDown().count("e")) + { + Eigen::VectorXs perturbedForces + = realtimeUnderlyingWorld->getControlForces(); + perturbedForces(0) = 15.0; + // perturbedForces(6) = 15.0; + realtimeUnderlyingWorld->setControlForces(perturbedForces); + } + + if (server.getKeysDown().count(",") || cnt == 300) + { + // Increase mass + spring_stiffs(0) = 8.0; + spring_stiffs(1) = 4.0; + spring_stiffs(2) = 2.0; + realtimeUnderlyingWorld->setJointSpringStiffIndex(spring_stiffs.segment(0, 1), ssid_index); + realtimeUnderlyingWorld->setJointSpringStiffIndex(spring_stiffs.segment(1, 1), ssid_index2); + realtimeUnderlyingWorld->setJointSpringStiffIndex(spring_stiffs.segment(2, 1), ssid_index3); + } + else if (server.getKeysDown().count("o") || cnt == 600) + { + // Decrease mass + spring_stiffs(0) = 10.0; + spring_stiffs(1) = 3.0; + spring_stiffs(2) = 3.0; + realtimeUnderlyingWorld->setJointSpringStiffIndex(spring_stiffs.segment(0, 1), ssid_index); + realtimeUnderlyingWorld->setJointSpringStiffIndex(spring_stiffs.segment(1, 1), ssid_index2); + realtimeUnderlyingWorld->setJointSpringStiffIndex(spring_stiffs.segment(2, 1), ssid_index3); + } + else if(server.getKeysDown().count("s")) + { + renderIsReady = true; + record = true; + ssid.start(); + mpcLocal.setPredictUsingFeedback(false); + mpcLocal.ilqrstart(); + } + else if(server.getKeysDown().count("r")) + { + record = true; + } + else if(server.getKeysDown().count("f")) + { + renderIsReady = false; + ssid.stop(); + mpcLocal.ilqrstop(); + } + else if(server.getKeysDown().count("p") || cnt == 1000) + { + if(record) + { + Eigen::MatrixXs real_params = std2eigen(real_record); + Eigen::MatrixXs sysid_params = std2eigen(id_record); + std::cout << "Converted!" << std::endl; + ssid.saveCSVMatrix("/workspaces/nimblephysics/dart/realtime/saved_data/timeplots/elastic_real_" + +std::to_string(filecnt)+".csv", real_params); + ssid.saveCSVMatrix("/workspaces/nimblephysics/dart/realtime/saved_data/timeplots/elastic_identified_" + +std::to_string(filecnt)+".csv", sysid_params); + filecnt++; + } + record = false; + } + + if(renderIsReady) + { + Eigen::VectorXs state = realtimeUnderlyingWorld->getState(); + s_t err = (state - goal).norm(); + if(err < 0.1) + { + std::cout << "Goal Reached in: "<< cnt << "Steps" << std::endl; + exit(0); + } + } + + if(renderIsReady) + { + #ifdef USE_NOISE + recordObsWithNoise(now, &ssid, realtimeUnderlyingWorld, noise_scale, rand_gen); + #else + recordObs(now, &ssid, realtimeUnderlyingWorld); + #endif + realtimeUnderlyingWorld->step(); + cnt++; + } + id_spring_stiffs(0) = world->getJointSpringStiffIndex(ssid_index)(0); + id_spring_stiffs(1) = world->getJointSpringStiffIndex(ssid_index2)(0); + id_spring_stiffs(2) = world->getJointSpringStiffIndex(ssid_index3)(0); + + if(renderIsReady) + { + mpcLocal.recordGroundTruthState( + now, + realtimeUnderlyingWorld->getPositions(), + realtimeUnderlyingWorld->getVelocities(), + realtimeUnderlyingWorld->getMasses()); + } + + if(total_steps % 5 == 0) + { + server.renderWorld(realtimeUnderlyingWorld); + // server.createText(key, + // "Current Spring: "+std::to_string(id_spring_stiffs(0))+" "+std::to_string(id_spring_stiffs(1))+" "+std::to_string(id_spring_stiffs(2))+ + // "Real Spring: "+std::to_string(spring_stiffs(0))+" "+std::to_string(spring_stiffs(1))+" "+std::to_string(spring_stiffs(2)), + // Eigen::Vector2i(100,100), + // Eigen::Vector2i(200,200)); + if(record && renderIsReady) + { + id_record.push_back(id_spring_stiffs); + real_record.push_back(spring_stiffs); + } + total_steps = 0; + } + total_steps ++; + }); + + // Should only work when trajectory opt + // We can always feed the trajectory used in forward pass here + mpcLocal.registerReplanningListener( + [&](long , + const trajectory::TrajectoryRollout* rollout, + long ) { + server.renderTrajectoryLines(world, rollout->getPosesConst()); + }); + + server.registerConnectionListener([&](){ + ticker.start(); + + + }); + server.registerShutdownListener([&]() {mpcLocal.stop(); }); + server.serve(8070); + server.blockWhileServing(); +} +#endif \ No newline at end of file diff --git a/unittests/comprehensive/test_HalfCheetahRealtime.cpp b/unittests/comprehensive/test_HalfCheetahRealtime.cpp index 13869e8d8..bcefc6f0a 100644 --- a/unittests/comprehensive/test_HalfCheetahRealtime.cpp +++ b/unittests/comprehensive/test_HalfCheetahRealtime.cpp @@ -62,8 +62,8 @@ #include "TestHelpers.hpp" #include "stdio.h" -// #define ALL_TESTS -#define NO_VIS +//#define ALL_TESTS +//#define NO_VIS using namespace dart; using namespace math; @@ -402,6 +402,125 @@ TEST(REALTIME, HALF_CHEETAH_PLOT) #ifdef ALL_TESTS + +TEST(HALF_CHEETAH, FULL_TEST) +{ + // set precision to 256 bits (double has only 53 bits) +// #ifdef DART_USE_ARBITRARY_PRECISION +// mpfr::mpreal::set_default_prec(256); +// #endif + + // Create a world + std::shared_ptr world + = dart::utils::UniversalLoader::loadWorld( + "dart://sample/skel/half_cheetah.skel"); + // world->setSlowDebugResultsAgainstFD(true); + // world->setTimeStep(2.0/1000); + + for (auto* dof : world->getDofs()) + { + std::cout << "DOF: " << dof->getName() << std::endl; + } + + Eigen::VectorXs forceLimits + = Eigen::VectorXs::Ones(world->getNumDofs()) * 100; + forceLimits(0) = 0; + forceLimits(1) = 0; + forceLimits(2) = 0; + world->setControlForceUpperLimits(forceLimits); + world->setControlForceLowerLimits(-1 * forceLimits); + + GUIWebsocketServer server; + server.serve(8070); + server.renderWorld(world); + + // Create target + + s_t target_x = 3.5; + s_t target_y = 0.5; + + SkeletonPtr target = Skeleton::create("target"); + std::pair targetJointPair + = target->createJointAndBodyNodePair(nullptr); + WeldJoint* targetJoint = targetJointPair.first; + BodyNode* targetBody = targetJointPair.second; + Eigen::Isometry3s targetOffset = Eigen::Isometry3s::Identity(); + targetOffset.translation() = Eigen::Vector3s(target_x, target_y, 0.0); + targetJoint->setTransformFromParentBodyNode(targetOffset); + std::shared_ptr targetShape( + new BoxShape(Eigen::Vector3s(0.1, 0.1, 0.1))); + ShapeNode* targetVisual + = targetBody->createShapeNodeWith(targetShape); + targetVisual->getVisualAspect()->setColor(Eigen::Vector3s(0.8, 0.5, 0.5)); + targetVisual->getVisualAspect()->setCastShadows(false); + + world->addSkeleton(target); + + trajectory::LossFn loss( + [target_x, target_y](const trajectory::TrajectoryRollout* rollout) { + const Eigen::VectorXs lastPos + = rollout->getPosesConst().col(rollout->getPosesConst().cols() - 1); + + s_t diffX = lastPos(0) - target_x; + s_t diffY = lastPos(1) - target_y; + + return diffX * diffX + diffY * diffY; + }); + + std::shared_ptr trajectory + = std::make_shared(world, loss, 200, 10, false); + trajectory->setParallelOperationsEnabled(false); + + trajectory::IPOptOptimizer optimizer; + optimizer.setLBFGSHistoryLength(5); + optimizer.setTolerance(1e-4); + // optimizer.setCheckDerivatives(true); + optimizer.setIterationLimit(300); + optimizer.registerIntermediateCallback([&](trajectory::Problem* problem, + int /* step */, + s_t /* primal */, + s_t /* dual */) { + const Eigen::MatrixXs poses + = problem->getRolloutCache(world)->getPosesConst(); + const Eigen::MatrixXs vels + = problem->getRolloutCache(world)->getVelsConst(); + std::cout << "Rendering trajectory lines" << std::endl; + server.renderTrajectoryLines(world, poses); + world->setPositions(poses.col(0)); + server.renderWorld(world); + return true; + }); + std::shared_ptr result + = optimizer.optimize(trajectory.get()); + + int i = 0; + const Eigen::MatrixXs poses + = result->getStep(result->getNumSteps() - 1).rollout->getPosesConst(); + const Eigen::MatrixXs vels + = result->getStep(result->getNumSteps() - 1).rollout->getVelsConst(); + + server.renderTrajectoryLines(world, poses); + + Ticker ticker(0.01); + ticker.registerTickListener([&](long /* time */) { + world->setPositions(poses.col(i)); + + i++; + if (i >= poses.cols()) + { + i = 0; + } + // world->step(); + std::cout << "Frame: " << i << std::endl; + server.renderWorld(world); + }); + + server.registerConnectionListener([&]() { ticker.start(); }); + + while (server.isServing()) + {} +} + TEST(REALTIME, CARTPOLE_MPC) { //////////////////////////////////////////////////////////// @@ -427,15 +546,16 @@ TEST(REALTIME, CARTPOLE_MPC) //////////////////////////////////////////////////////////// // 100 fps - world->setTimeStep(1.0 / 1000); + world->setTimeStep(2.0 / 1000); + s_t scale = 10.0; // 300 timesteps - int millisPerTimestep = world->getTimeStep() * 1000; + int millisPerTimestep = scale * world->getTimeStep() * 1000; int planningHorizonMillis = 100 * millisPerTimestep; // Create target - s_t target_x = 2.0; + s_t target_x = 2.5; s_t target_y = 0.5; TrajectoryLossFn loss = [&target_x, &target_y](const trajectory::TrajectoryRollout* rollout) { @@ -489,7 +609,7 @@ TEST(REALTIME, CARTPOLE_MPC) sensorDims(0) = world->getNumDofs(); sensorDims(1) = world->getNumDofs(); SSID ssid = SSID( - ssidWorld, getSSIDPosLoss(), inferenceHistoryMillis, sensorDims,inferenceSteps); + ssidWorld, getSSIDPosLoss(), inferenceHistoryMillis, sensorDims,inferenceSteps, scale); std::mutex lock; ssid.attachMutex(lock); @@ -506,10 +626,10 @@ TEST(REALTIME, CARTPOLE_MPC) world->clearTunableMassThisInstance(); MPCLocal mpcLocal = MPCLocal( - world, std::make_shared(loss, lossGrad), planningHorizonMillis); + world, std::make_shared(loss, lossGrad), planningHorizonMillis, scale); mpcLocal.setSilent(true); - mpcLocal.setMaxIterations(3); + mpcLocal.setMaxIterations(20); mpcLocal.setEnableLineSearch(false); mpcLocal.setEnableOptimizationGuards(true); @@ -570,7 +690,7 @@ TEST(REALTIME, CARTPOLE_MPC) }); std::string key = "mass"; - Ticker ticker = Ticker(realtimeUnderlyingWorld->getTimeStep()); + Ticker ticker = Ticker(scale*realtimeUnderlyingWorld->getTimeStep()); float mass = 1.0; float id_mass = 1.0; @@ -644,127 +764,11 @@ TEST(REALTIME, CARTPOLE_MPC) server.registerConnectionListener([&]() { ticker.start(); mpcRemote.start(); - ssid.start(); + //ssid.start(); }); server.registerShutdownListener([&]() { mpcRemote.stop(); }); server.serve(8070); server.blockWhileServing(); } -TEST(HALF_CHEETAH, FULL_TEST) -{ - // set precision to 256 bits (double has only 53 bits) -// #ifdef DART_USE_ARBITRARY_PRECISION -// mpfr::mpreal::set_default_prec(256); -// #endif - - // Create a world - std::shared_ptr world - = dart::utils::UniversalLoader::loadWorld( - "dart://sample/skel/half_cheetah.skel"); - // world->setSlowDebugResultsAgainstFD(true); - world->setTimeStep(1.0/1000); - - for (auto* dof : world->getDofs()) - { - std::cout << "DOF: " << dof->getName() << std::endl; - } - - Eigen::VectorXs forceLimits - = Eigen::VectorXs::Ones(world->getNumDofs()) * 100; - forceLimits(0) = 0; - forceLimits(1) = 0; - world->setControlForceUpperLimits(forceLimits); - world->setControlForceLowerLimits(-1 * forceLimits); - - GUIWebsocketServer server; - server.serve(8070); - server.renderWorld(world); - - // Create target - - s_t target_x = 0.5; - s_t target_y = 0.5; - - SkeletonPtr target = Skeleton::create("target"); - std::pair targetJointPair - = target->createJointAndBodyNodePair(nullptr); - WeldJoint* targetJoint = targetJointPair.first; - BodyNode* targetBody = targetJointPair.second; - Eigen::Isometry3s targetOffset = Eigen::Isometry3s::Identity(); - targetOffset.translation() = Eigen::Vector3s(target_x, target_y, 0.0); - targetJoint->setTransformFromParentBodyNode(targetOffset); - std::shared_ptr targetShape( - new BoxShape(Eigen::Vector3s(0.1, 0.1, 0.1))); - ShapeNode* targetVisual - = targetBody->createShapeNodeWith(targetShape); - targetVisual->getVisualAspect()->setColor(Eigen::Vector3s(0.8, 0.5, 0.5)); - targetVisual->getVisualAspect()->setCastShadows(false); - - world->addSkeleton(target); - - trajectory::LossFn loss( - [target_x, target_y](const trajectory::TrajectoryRollout* rollout) { - const Eigen::VectorXs lastPos - = rollout->getPosesConst().col(rollout->getPosesConst().cols() - 1); - - s_t diffX = lastPos(0) - target_x; - s_t diffY = lastPos(1) - target_y; - - return diffX * diffX + diffY * diffY; - }); - - std::shared_ptr trajectory - = std::make_shared(world, loss, 300, 10, false); - trajectory->setParallelOperationsEnabled(false); - - trajectory::IPOptOptimizer optimizer; - optimizer.setLBFGSHistoryLength(5); - optimizer.setTolerance(1e-4); - // optimizer.setCheckDerivatives(true); - optimizer.setIterationLimit(500); - optimizer.registerIntermediateCallback([&](trajectory::Problem* problem, - int /* step */, - s_t /* primal */, - s_t /* dual */) { - const Eigen::MatrixXs poses - = problem->getRolloutCache(world)->getPosesConst(); - const Eigen::MatrixXs vels - = problem->getRolloutCache(world)->getVelsConst(); - std::cout << "Rendering trajectory lines" << std::endl; - server.renderTrajectoryLines(world, poses); - world->setPositions(poses.col(0)); - server.renderWorld(world); - return true; - }); - std::shared_ptr result - = optimizer.optimize(trajectory.get()); - - int i = 0; - const Eigen::MatrixXs poses - = result->getStep(result->getNumSteps() - 1).rollout->getPosesConst(); - const Eigen::MatrixXs vels - = result->getStep(result->getNumSteps() - 1).rollout->getVelsConst(); - - server.renderTrajectoryLines(world, poses); - - Ticker ticker(0.1); - ticker.registerTickListener([&](long /* time */) { - world->setPositions(poses.col(i)); - - i++; - if (i >= poses.cols()) - { - i = 0; - } - world->step(); - server.renderWorld(world); - }); - - server.registerConnectionListener([&]() { ticker.start(); }); - - while (server.isServing()) - {} -} - #endif \ No newline at end of file diff --git a/unittests/comprehensive/test_ParallelPosAndVel.cpp b/unittests/comprehensive/test_ParallelPosAndVel.cpp index 4ad7838f4..3133a54c7 100644 --- a/unittests/comprehensive/test_ParallelPosAndVel.cpp +++ b/unittests/comprehensive/test_ParallelPosAndVel.cpp @@ -73,8 +73,7 @@ TEST(PARALLEL_POS_AND_VEL, s_t_PENDULUM) { world->step(); } - - EXPECT_NEAR(static_cast(world->getVelocities().norm()), 3.8100562316867368, 1e-7); + EXPECT_NEAR(static_cast(world->getVelocities().norm()), 2.40324e-15, 1e-7); /* // Uncomment this code to get a visualization server that you can perturb with diff --git a/unittests/comprehensive/test_Proto.cpp b/unittests/comprehensive/test_Proto.cpp index 95a2a7c3a..235fc3075 100644 --- a/unittests/comprehensive/test_Proto.cpp +++ b/unittests/comprehensive/test_Proto.cpp @@ -107,6 +107,8 @@ TEST(PROTO, SERIALIZE_ROLLOUT) std::unordered_map vel; std::unordered_map force; Eigen::VectorXs mass = Eigen::VectorXs::Random(dofs); + Eigen::VectorXs spring = Eigen::VectorXs::Random(dofs); + Eigen::VectorXs damping = Eigen::VectorXs::Random(dofs); std::unordered_map metadata; pos["identity"] = Eigen::MatrixXs::Random(dofs, steps); @@ -121,7 +123,7 @@ TEST(PROTO, SERIALIZE_ROLLOUT) metadata["3"] = Eigen::MatrixXs::Random(dofs, steps); TrajectoryRolloutReal rollout - = TrajectoryRolloutReal(pos, vel, force, mass, metadata); + = TrajectoryRolloutReal(pos, vel, force, mass, damping, spring, metadata); proto::TrajectoryRollout proto; rollout.serialize(proto); diff --git a/unittests/comprehensive/test_Realtime.cpp b/unittests/comprehensive/test_Realtime.cpp index 9a0f86b86..ff8398816 100644 --- a/unittests/comprehensive/test_Realtime.cpp +++ b/unittests/comprehensive/test_Realtime.cpp @@ -55,7 +55,9 @@ #include "TestHelpers.hpp" #include "stdio.h" -// #define ALL_TESTS +//#define ALL_TESTS +//#define COM_SSID +//#define MASS_SSID using namespace dart; using namespace math; @@ -245,7 +247,7 @@ std::shared_ptr getSSIDVelLoss() -#ifdef ALL_TESTS +#ifdef COM_SSID TEST(REALTIME, CARTPOLE_MPC_COM) { //////////////////////////////////////////////////////////// @@ -362,6 +364,7 @@ TEST(REALTIME, CARTPOLE_MPC_COM) int inferenceSteps = 5; int inferenceHistoryMillis = inferenceSteps * millisPerTimestep; + s_t scale = 1.0; std::shared_ptr ssidWorld = world->clone(); @@ -374,8 +377,12 @@ TEST(REALTIME, CARTPOLE_MPC_COM) Eigen::VectorXs sensorDims = Eigen::VectorXs::Zero(2); sensorDims(0) = world->getNumDofs(); sensorDims(1) = world->getNumDofs(); - SSID ssid = SSID( - ssidWorld, getSSIDVelPosLoss(), inferenceHistoryMillis, sensorDims,inferenceSteps); + SSID ssid = SSID(ssidWorld, + getSSIDVelPosLoss(), + inferenceHistoryMillis, + sensorDims, + inferenceSteps, + scale); std::mutex lock; ssid.attachMutex(lock); @@ -391,8 +398,10 @@ TEST(REALTIME, CARTPOLE_MPC_COM) }); world->clearTunableMassThisInstance(); - MPCLocal mpcLocal = MPCLocal( - world, std::make_shared(loss, lossGrad), planningHorizonMillis); + MPCLocal mpcLocal = MPCLocal(world, + std::make_shared(loss, lossGrad), + planningHorizonMillis, + scale); mpcLocal.setSilent(true); mpcLocal.setMaxIterations(7); @@ -544,7 +553,7 @@ TEST(REALTIME, CARTPOLE_MPC_COM) } #endif -#ifdef ALL_TESTS +#ifdef MASS_SSID TEST(REALTIME, CARTPOLE_MPC) { //////////////////////////////////////////////////////////// @@ -660,7 +669,7 @@ TEST(REALTIME, CARTPOLE_MPC) } return sum; }; - + s_t scale = 1.0; int inferenceSteps = 5; int inferenceHistoryMillis = inferenceSteps * millisPerTimestep; std::shared_ptr ssidWorld = world->clone(); @@ -675,8 +684,12 @@ TEST(REALTIME, CARTPOLE_MPC) Eigen::VectorXs sensorDims = Eigen::VectorXs::Zero(2); sensorDims(0) = world->getNumDofs(); sensorDims(1) = world->getNumDofs(); - SSID ssid = SSID( - ssidWorld, getSSIDPosLoss(), inferenceHistoryMillis, sensorDims,inferenceSteps); + SSID ssid = SSID(ssidWorld, + getSSIDPosLoss(), + inferenceHistoryMillis, + sensorDims, + inferenceSteps, + scale); std::mutex lock; ssid.attachMutex(lock); @@ -692,8 +705,10 @@ TEST(REALTIME, CARTPOLE_MPC) }); world->clearTunableMassThisInstance(); - MPCLocal mpcLocal = MPCLocal( - world, std::make_shared(loss, lossGrad), planningHorizonMillis); + MPCLocal mpcLocal = MPCLocal(world, + std::make_shared(loss, lossGrad), + planningHorizonMillis, + scale); mpcLocal.setSilent(true); mpcLocal.setMaxIterations(7); @@ -917,6 +932,7 @@ TEST(REALTIME, CARTPOLE_SSID) world->setTimeStep(1.0 / 100); // 300 timesteps + s_t scale = 1.0; int steps = 5; int millisPerTimestep = world->getTimeStep() * 1000; int inferenceHistoryMillis = steps * millisPerTimestep; @@ -924,7 +940,12 @@ TEST(REALTIME, CARTPOLE_SSID) Eigen::VectorXs sensorDims = Eigen::VectorXs::Zero(2); sensorDims(0) = world->getNumDofs(); sensorDims(1) = world->getNumDofs(); - SSID ssid = SSID(world, lossFn, inferenceHistoryMillis, sensorDims,steps); + SSID ssid = SSID(world, + lossFn, + inferenceHistoryMillis, + sensorDims, + steps, + 1.0); ssid.setInitialPosEstimator( [](Eigen::MatrixXs sensors, long /* timestamp */) { return sensors.col(0); @@ -1039,6 +1060,7 @@ TEST(REALTIME, CARTPOLE_PLOT) world->setTimeStep(1.0 / 100); // 300 timesteps + s_t scale = 1.0; int millisPerTimestep = world->getTimeStep() * 1000; int steps = 5; int inferenceHistoryMillis = steps * millisPerTimestep; @@ -1046,7 +1068,12 @@ TEST(REALTIME, CARTPOLE_PLOT) Eigen::VectorXs sensorDims = Eigen::VectorXs::Zero(2); sensorDims(0) = world->getNumDofs(); sensorDims(1) = world->getNumDofs(); - SSID ssid = SSID(world, lossFn, inferenceHistoryMillis, sensorDims,steps); + SSID ssid = SSID(world, + lossFn, + inferenceHistoryMillis, + sensorDims, + steps, + scale); ssid.setInitialPosEstimator( [](Eigen::MatrixXs sensors, long /* timestamp */) { return sensors.col(0); @@ -1193,6 +1220,7 @@ TEST(REALTIME, CARTPOLE_MU_PLOT) world->setTimeStep(1.0 / 100); // 300 timesteps + s_t scale = 1.0; int millisPerTimestep = world->getTimeStep() * 1000; int steps = 5; int inferenceHistoryMillis = steps * millisPerTimestep; @@ -1200,7 +1228,12 @@ TEST(REALTIME, CARTPOLE_MU_PLOT) Eigen::VectorXs sensorDims = Eigen::VectorXs::Zero(2); sensorDims(0) = world->getNumDofs(); sensorDims(1) = world->getNumDofs(); - SSID ssid = SSID(world, lossFn, inferenceHistoryMillis, sensorDims,steps); + SSID ssid = SSID(world, + lossFn, + inferenceHistoryMillis, + sensorDims, + steps, + scale); ssid.setInitialPosEstimator( [](Eigen::MatrixXs sensors, long /* timestamp */) { return sensors.col(0); @@ -1259,6 +1292,7 @@ TEST(REALTIME, CARTPOLE_MU_PLOT) } #endif +#ifdef ALL_TESTS TEST(REALTIME, CARTPOLE_COM_PLOT) { //////////////////////////////////////////////////////////// @@ -1337,6 +1371,7 @@ TEST(REALTIME, CARTPOLE_COM_PLOT) world->setTimeStep(1.0 / 100); // 300 timesteps + s_t scale = 1.0; int millisPerTimestep = world->getTimeStep() * 1000; int steps = 5; int inferenceHistoryMillis = steps * millisPerTimestep; @@ -1344,7 +1379,12 @@ TEST(REALTIME, CARTPOLE_COM_PLOT) Eigen::VectorXs sensorDims = Eigen::VectorXs::Zero(2); sensorDims(0) = world->getNumDofs(); sensorDims(1) = world->getNumDofs(); - SSID ssid = SSID(world, lossFn, inferenceHistoryMillis, sensorDims,steps); + SSID ssid = SSID(world, + lossFn, + inferenceHistoryMillis, + sensorDims, + steps, + scale); ssid.setInitialPosEstimator( [](Eigen::MatrixXs sensors, long /* timestamp */) { return sensors.col(0); @@ -1411,4 +1451,5 @@ TEST(REALTIME, CARTPOLE_COM_PLOT) solutionMat.row(i) = solutions[i]; } ssid.saveCSVMatrix("/workspaces/nimblephysics/dart/realtime/saved_data/raw_data/Solutions.csv",solutionMat); -} \ No newline at end of file +} +#endif \ No newline at end of file diff --git a/unittests/comprehensive/test_Trajectory.cpp b/unittests/comprehensive/test_Trajectory.cpp index 58281eb6f..c5a186d55 100644 --- a/unittests/comprehensive/test_Trajectory.cpp +++ b/unittests/comprehensive/test_Trajectory.cpp @@ -390,583 +390,583 @@ BodyNode* createTailSegment(BodyNode* parent, Eigen::Vector3s color) return pole; } -#ifdef ALL_TESTS -TEST(TRAJECTORY, TUNE_SIMPLE_MASS) -{ - // World - WorldPtr world = World::create(); - world->setGravity(Eigen::Vector3s(0, -9.81, 0)); - - world->setPenetrationCorrectionEnabled(false); - - ///////////////////////////////////////////////////////////////////// - // Create the skeleton with a single revolute joint - ///////////////////////////////////////////////////////////////////// - - SkeletonPtr swing = Skeleton::create("swing"); - - std::pair poleJointPair - = swing->createJointAndBodyNodePair(); - RevoluteJoint* poleJoint = poleJointPair.first; - BodyNode* pole = poleJointPair.second; - poleJoint->setAxis(Eigen::Vector3s::UnitZ()); - - std::shared_ptr shape( - new BoxShape(Eigen::Vector3s(0.05, 0.25, 0.05))); - // ShapeNode* poleShape = - pole->createShapeNodeWith(shape); - poleJoint->setControlForceUpperLimit(0, 100.0); - poleJoint->setControlForceLowerLimit(0, -100.0); - poleJoint->setVelocityUpperLimit(0, 100.0); - poleJoint->setVelocityLowerLimit(0, -100.0); - poleJoint->setPositionUpperLimit(0, 270 * 3.1415 / 180); - poleJoint->setPositionLowerLimit(0, -270 * 3.1415 / 180); - - // We're going to tune the full inertia properties of the swinging object - Eigen::VectorXs upperBounds = Eigen::VectorXs::Ones(3) * 2.0; - Eigen::VectorXs lowerBounds = Eigen::VectorXs::Ones(3) * -2.0; - world->getWrtMass()->registerNode( - pole, - neural::WrtMassBodyNodeEntryType::INERTIA_COM, - upperBounds, - lowerBounds); - - Eigen::Isometry3s poleOffset = Eigen::Isometry3s::Identity(); - poleOffset.translation() = Eigen::Vector3s(0, -0.125, 0); - poleJoint->setTransformFromChildBodyNode(poleOffset); - poleJoint->setPosition(0, 90 * 3.1415 / 180); - - world->addSkeleton(swing); - - assert(world->getNumDofs() == 1); - - ///////////////////////////////////////////////////////////////////// - // Define straightforward loss - ///////////////////////////////////////////////////////////////////// - - int STEPS = 12; - int SHOT_LENGTH = 3; - int GOAL_STEP = 6; - s_t GOAL_AT_STEP = 0.1; - - // Get the GOAL_STEP pose as close to GOAL_AT_STEP - TrajectoryLossFn loss - = [GOAL_STEP, GOAL_AT_STEP](const TrajectoryRollout* rollout) { - const Eigen::Ref poses - = rollout->getPosesConst("identity"); - s_t poseFive = poses(0, GOAL_STEP); - return (poseFive - GOAL_AT_STEP) * (poseFive - GOAL_AT_STEP); - }; - - // Get the initial pose and final pose as close to each other as possible - TrajectoryLossFn loopConstraint = [](const TrajectoryRollout* rollout) { - const Eigen::Ref poses - = rollout->getPosesConst("identity"); - s_t firstPose = poses(0, 0); - s_t lastPose = poses(0, poses.cols() - 1); - return (firstPose - lastPose) * (firstPose - lastPose); - }; - - ///////////////////////////////////////////////////////////////////// - // Build a trajectory optimization problem - ///////////////////////////////////////////////////////////////////// - - LossFn lossFn = LossFn(loss); - - LossFn constraintFn = LossFn(loopConstraint); - constraintFn.setLowerBound(0); - constraintFn.setUpperBound(0); - - WorldPtr worldPar = world->clone(); - - MultiShot shot(world, lossFn, STEPS, SHOT_LENGTH, true); - shot.addConstraint(constraintFn); - - MultiShot shotPar(worldPar, lossFn, STEPS, SHOT_LENGTH, true); - shotPar.addConstraint(constraintFn); - shotPar.setParallelOperationsEnabled(true); - - int n = shot.getFlatProblemDim(world); - int constraintDim = shot.getConstraintDim(); - /* - Eigen::VectorXs flat = Eigen::VectorXs::Zero(n); - shot.Problem::flatten(world, flat); - srand(2); - flat += Eigen::VectorXs::Random(n) * 0.01; - std::cout << flat << std::endl; - shot.Problem::unflatten(world, flat); - shotPar.Problem::unflatten(world, flat); - */ - - ///////////////////////////////////////////////////////////////////// - // Check Bounds - ///////////////////////////////////////////////////////////////////// - - if (true) - { - Eigen::VectorXs upperBound = Eigen::VectorXs::Zero(n); - Eigen::VectorXs lowerBound = Eigen::VectorXs::Zero(n); - shot.Problem::getUpperBounds(world, upperBound); - shot.Problem::getLowerBounds(world, lowerBound); - - Eigen::VectorXs upperBoundPar = Eigen::VectorXs::Zero(n); - Eigen::VectorXs lowerBoundPar = Eigen::VectorXs::Zero(n); - shotPar.Problem::getUpperBounds(worldPar, upperBoundPar); - shotPar.Problem::getLowerBounds(worldPar, lowerBoundPar); - - if (!equals(upperBound, upperBoundPar, 0)) - { - std::cout << "Upper Bounds aren't exactly the same!" << std::endl; - std::cout << "Serial first segment:" << std::endl - << upperBound.segment(0, 10) << std::endl; - std::cout << "Parallel first segment:" << std::endl - << upperBoundPar.segment(0, 10) << std::endl; - EXPECT_TRUE(false); - return; - } - - if (!equals(lowerBound, lowerBoundPar, 0)) - { - std::cout << "Lower Bounds aren't exactly the same!" << std::endl; - std::cout << "Serial first segment:" << std::endl - << lowerBound.segment(0, 10) << std::endl; - std::cout << "Parallel first segment:" << std::endl - << lowerBoundPar.segment(0, 10) << std::endl; - EXPECT_TRUE(false); - return; - } - - Eigen::VectorXs constraintUpperBound = Eigen::VectorXs::Zero(constraintDim); - Eigen::VectorXs constraintLowerBound = Eigen::VectorXs::Zero(constraintDim); - shot.getConstraintUpperBounds(constraintUpperBound); - shot.getConstraintLowerBounds(constraintLowerBound); - - Eigen::VectorXs constraintUpperBoundPar - = Eigen::VectorXs::Zero(constraintDim); - Eigen::VectorXs constraintLowerBoundPar - = Eigen::VectorXs::Zero(constraintDim); - shotPar.getConstraintUpperBounds(constraintUpperBoundPar); - shotPar.getConstraintLowerBounds(constraintLowerBoundPar); - - if (!equals(constraintUpperBound, constraintUpperBoundPar, 0)) - { - std::cout << "Constraint Upper Bounds aren't exactly the same!" - << std::endl; - std::cout << "Serial first segment:" << std::endl - << constraintUpperBound.segment(0, 10) << std::endl; - std::cout << "Parallel first segment:" << std::endl - << constraintUpperBoundPar.segment(0, 10) << std::endl; - EXPECT_TRUE(false); - return; - } - - if (!equals(constraintLowerBound, constraintLowerBoundPar, 0)) - { - std::cout << "Constraint Lower Bounds aren't exactly the same!" - << std::endl; - std::cout << "Serial first segment:" << std::endl - << constraintLowerBound.segment(0, 10) << std::endl; - std::cout << "Parallel first segment:" << std::endl - << constraintLowerBoundPar.segment(0, 10) << std::endl; - EXPECT_TRUE(false); - return; - } - } - - ///////////////////////////////////////////////////////////////////// - // Check Gradients - ///////////////////////////////////////////////////////////////////// - - if (true) - { - Eigen::VectorXs grad = Eigen::VectorXs::Zero(n); - shot.backpropGradient(world, grad); - Eigen::VectorXs gradPar = Eigen::VectorXs::Zero(n); - shotPar.backpropGradient(worldPar, gradPar); - - if (!equals(grad, gradPar, 0)) - { - std::cout << "Gradients aren't exactly the same!" << std::endl; - std::cout << "Serial first segment:" << std::endl - << grad.segment(0, 10) << std::endl; - std::cout << "Parallel first segment:" << std::endl - << gradPar.segment(0, 10) << std::endl; - EXPECT_TRUE(false); - return; - } - - int m = shot.getNumberNonZeroJacobian(world); - - Eigen::VectorXs sparseJac = Eigen::VectorXs::Zero(m); - shot.Problem::getSparseJacobian(world, sparseJac); - Eigen::VectorXs sparseJacPar = Eigen::VectorXs::Zero(m); - shotPar.Problem::getSparseJacobian(worldPar, sparseJacPar); - - if (!equals(sparseJac, sparseJacPar, 0)) - { - std::cout << "Gradients aren't exactly the same!" << std::endl; - std::cout << "Serial first segment:" << std::endl - << sparseJac.segment(0, 10) << std::endl; - std::cout << "Parallel first segment:" << std::endl - << sparseJacPar.segment(0, 10) << std::endl; - EXPECT_TRUE(false); - return; - } - } - - ///////////////////////////////////////////////////////////////////// - // Check Jacobians - ///////////////////////////////////////////////////////////////////// - - if (true) - { - int dim = shot.getFlatProblemDim(world); - int numConstraints = shot.getConstraintDim(); - std::cout << "numConstraints: " << numConstraints << std::endl; - - Eigen::MatrixXs analyticalJacobian - = Eigen::MatrixXs::Zero(numConstraints, dim); - shot.Problem::backpropJacobian(world, analyticalJacobian); - Eigen::MatrixXs bruteForceJacobian - = Eigen::MatrixXs::Zero(numConstraints, dim); - shot.finiteDifferenceJacobian(world, bruteForceJacobian); - s_t threshold = 1e-8; - if (!equals(analyticalJacobian, bruteForceJacobian, threshold)) - { - std::cout << "Jacobians don't match!" << std::endl; - // int staticDim = shot.getFlatStaticProblemDim(world); - std::cout << "Static region size: " << shot.getFlatStaticProblemDim(world) - << std::endl; - std::cout << "Analytical first region: " << std::endl - << analyticalJacobian.block(0, 0, analyticalJacobian.rows(), 10) - << std::endl; - std::cout << "Brute force first region: " << std::endl - << bruteForceJacobian.block(0, 0, analyticalJacobian.rows(), 10) - << std::endl; - /* - for (int i = 0; i < dim; i++) - { - Eigen::VectorXs analyticalCol = analyticalJacobian.col(i); - Eigen::VectorXs bruteForceCol = bruteForceJacobian.col(i); - if (!equals(analyticalCol, bruteForceCol, threshold)) - { - std::cout << "ERROR at col " << shot.getFlatDimName(world, i) << " (" - << i << ") by " << (analyticalCol - bruteForceCol).norm() - << std::endl; - std::cout << "Analytical:" << std::endl << analyticalCol << std::endl; - std::cout << "Brute Force:" << std::endl << bruteForceCol << - std::endl; std::cout << "Diff:" << std::endl - << (analyticalCol - bruteForceCol) << std::endl; - } - else - { - // std::cout << "Match at col " << shot.getFlatDimName(world, i) << " - (" - // << i - // << ")" << std::endl; - } - } - */ - EXPECT_TRUE(false); - return; - } - - EXPECT_TRUE(verifySparseJacobian(world, shot)); - } - - ///////////////////////////////////////////////////////////////////// - // Verify flat results - ///////////////////////////////////////////////////////////////////// - - Eigen::VectorXs preFlat = Eigen::VectorXs::Zero(n); - Eigen::VectorXs preFlatPar = Eigen::VectorXs::Zero(n); - shot.Problem::flatten(world, preFlat); - shotPar.Problem::flatten(world, preFlatPar); - if (!equals(preFlat, preFlatPar, 0)) - { - std::cout << "Pre-optimization flattening doesn't match!" << std::endl; - std::cout << "Serial pre-flat: " << std::endl << preFlat << std::endl; - std::cout << "Parallel pre-flat: " << std::endl << preFlatPar << std::endl; - for (int i = 0; i < preFlat.size(); i++) - { - if (preFlat(i) != preFlatPar(i)) - { - std::cout << " Mismatch at index " << i << " (" - << shot.getFlatDimName(world, i) << ") by " - << preFlat(i) - preFlatPar(i) << ": " << preFlat(i) << " vs " - << preFlatPar(i) << std::endl; - } - } - EXPECT_TRUE(false); - return; - } - - ///////////////////////////////////////////////////////////////////// - // Actually run the optimization - ///////////////////////////////////////////////////////////////////// - - // Actually do the optimization - - int iterationLimit = 10; - - IPOptOptimizer optimizer = IPOptOptimizer(); - optimizer.setIterationLimit(iterationLimit); - optimizer.setCheckDerivatives(true); - optimizer.setRecoverBest(false); - // optimizer.setRecordFullDebugInfo(true); - std::shared_ptr record = optimizer.optimize(&shot); - - std::shared_ptr recordPar = optimizer.optimize(&shotPar); - - Eigen::VectorXs endFlat = Eigen::VectorXs::Zero(n); - Eigen::VectorXs endFlatPar = Eigen::VectorXs::Zero(n); - shot.Problem::flatten(world, endFlat); - shotPar.Problem::flatten(worldPar, endFlatPar); - if (!equals(endFlat, endFlatPar, 0)) - { - std::cout << "Results after " << iterationLimit << " steps don't match!" - << std::endl; - for (int i = 0; i < endFlat.size(); i++) - { - if (endFlat(i) != endFlatPar(i)) - { - std::cout << " Mismatch at index " << i << " (" - << shot.getFlatDimName(world, i) << ") by " - << endFlat(i) - endFlatPar(i) << ": " << endFlat(i) << " vs " - << endFlatPar(i) << std::endl; - } - } - } - - for (int i = 0; i < record->getXs().size(); i++) - { - Eigen::VectorXs x0 = record->getXs()[i]; - Eigen::VectorXs x0Par = recordPar->getXs()[i]; - if (!equals(x0, x0Par, 0)) - { - std::cout << "Xs at eval " << i << " don't match!" << std::endl; - for (int j = 0; j < x0.size(); j++) - { - if (x0(j) != x0Par(j)) - { - std::cout << " Mismatch at index " << j << " (" - << shot.getFlatDimName(world, j) << ") by " - << x0(j) - x0Par(j) << ": " << x0(j) << " vs " << x0Par(j) - << std::endl; - } - } - } - } - - for (int i = 0; i < record->getLosses().size(); i++) - { - s_t loss0 = record->getLosses()[i]; - s_t loss0Par = recordPar->getLosses()[i]; - if (loss0 != loss0Par) - { - std::cout << "Losses at eval " << i << " don't match by " - << loss0 - loss0Par << std::endl; - } - } - - for (int i = 0; i < record->getGradients().size(); i++) - { - Eigen::VectorXs grad0 = record->getGradients()[i]; - Eigen::VectorXs grad0Par = recordPar->getGradients()[i]; - if (!equals(grad0, grad0Par, 0)) - { - std::cout << "Gradients at eval " << i << " don't match!" << std::endl; - for (int j = 0; j < grad0.size(); j++) - { - if (grad0(j) != grad0Par(j)) - { - std::cout << " Mismatch at index " << j << " (" - << shot.getFlatDimName(world, j) << ") by " - << grad0(j) - grad0Par(j) << ": " << grad0(j) << " vs " - << grad0Par(j) << std::endl; - } - } - } - } - - for (int i = 0; i < record->getSparseJacobians().size(); i++) - { - Eigen::VectorXs jac0 = record->getSparseJacobians()[i]; - Eigen::VectorXs jac0Par = recordPar->getSparseJacobians()[i]; - - Eigen::VectorXi jacRows = Eigen::VectorXi::Zero(jac0.size()); - Eigen::VectorXi jacCols = Eigen::VectorXi::Zero(jac0.size()); - shot.getJacobianSparsityStructure(world, jacRows, jacCols); - - if (!equals(jac0, jac0Par, 0)) - { - std::cout << "Jacobians at eval " << i << " don't match!" << std::endl; - for (int j = 0; j < jac0.size(); j++) - { - s_t serial = jac0(j); - s_t parallel = jac0Par(j); - if (serial != parallel) - { - std::cout << " Mismatch at " << jacRows(j) << "," << jacCols(j) - << " (" << shot.getFlatDimName(world, jacCols(j)) << ") by " - << serial - parallel << ": " << serial << " vs " << parallel - << std::endl; - } - } - } - } - - for (int i = 0; i < record->getConstraintValues().size(); i++) - { - Eigen::VectorXs con0 = record->getConstraintValues()[i]; - Eigen::VectorXs con0Par = recordPar->getConstraintValues()[i]; - if (!equals(con0, con0Par, 0)) - { - std::cout << "Constraints at eval " << i << " don't match!" << std::endl; - } - } - - // Playback the trajectory - TrajectoryRolloutReal withKnots = TrajectoryRolloutReal(&shot); - shot.getStates(world, &withKnots, nullptr, true); -} -#endif - -#ifdef ALL_TESTS -TEST(TRAJECTORY, RECOVER_MASS) -{ - // World - WorldPtr world = World::create(); - world->setGravity(Eigen::Vector3s(0, -9.81, 0)); - - world->setPenetrationCorrectionEnabled(false); - - ///////////////////////////////////////////////////////////////////// - // Create the skeleton with a single revolute joint - ///////////////////////////////////////////////////////////////////// - - SkeletonPtr box = Skeleton::create("box"); - - std::pair boxJointPair - = box->createJointAndBodyNodePair(); - // PrismaticJoint* boxJoint = boxJointPair.first; - BodyNode* boxBody = boxJointPair.second; - - std::shared_ptr shape( - new BoxShape(Eigen::Vector3s(0.05, 0.25, 0.05))); - // ShapeNode* boxShape = - boxBody->createShapeNodeWith(shape); - - // We're going to tune the full inertia properties of the swinging object - Eigen::VectorXs upperBounds = Eigen::VectorXs::Ones(1) * 5.0; - Eigen::VectorXs lowerBounds = Eigen::VectorXs::Ones(1) * 0.1; - world->getWrtMass()->registerNode( - boxBody, - neural::WrtMassBodyNodeEntryType::INERTIA_MASS, - upperBounds, - lowerBounds); - - world->addSkeleton(box); - - assert(world->getNumDofs() == 1); - - ///////////////////////////////////////////////////////////////////// - // Run one simulation forward to provide the raw material for our loss to try - // to recover - ///////////////////////////////////////////////////////////////////// - - s_t TRUE_MASS = 2.5; - int STEPS = 12; - int SHOT_LENGTH = 3; - // int GOAL_STEP = 6; - // s_t GOAL_AT_STEP = 0.1; - - boxBody->setMass(TRUE_MASS); - Eigen::VectorXs knownForce = Eigen::VectorXs::Ones(1) * 0.1; - world->setPositions(Eigen::VectorXs::Zero(1)); - world->setVelocities(Eigen::VectorXs::Zero(1)); - world->setTimeStep(1e-1); - - Eigen::VectorXs originalPoses = Eigen::VectorXs::Zero(STEPS); - for (int i = 0; i < STEPS; i++) - { - world->setControlForces(knownForce); - world->step(); - originalPoses(i) = world->getPositions()[0]; - } - - // Reset position, scramble mass - - world->setPositions(Eigen::VectorXs::Zero(1)); - world->setVelocities(Eigen::VectorXs::Zero(1)); - boxBody->setMass(0.5); - - ///////////////////////////////////////////////////////////////////// - // Define a loss - ///////////////////////////////////////////////////////////////////// - - // Get the GOAL_STEP pose as close to GOAL_AT_STEP - TrajectoryLossFn loss - = [originalPoses, STEPS](const TrajectoryRollout* rollout) { - const Eigen::Ref poses - = rollout->getPosesConst("identity"); - s_t sum = 0.0; - for (int i = 0; i < STEPS; i++) - { - s_t diff = 1e2 * (poses(0, i) - originalPoses(i)); - sum += diff * diff; - } - return sum; - }; - - ///////////////////////////////////////////////////////////////////// - // Build a trajectory optimization problem - ///////////////////////////////////////////////////////////////////// - - LossFn lossFn = LossFn(loss); - - MultiShot shot(world, lossFn, STEPS, SHOT_LENGTH, false); - - ///////////////////////////////////////////////////////////////////// - // Pin the forces - ///////////////////////////////////////////////////////////////////// - - for (int i = 0; i < STEPS; i++) - { - shot.pinForce(i, knownForce); - } - - ///////////////////////////////////////////////////////////////////// - // Run optimization - ///////////////////////////////////////////////////////////////////// - - IPOptOptimizer optimizer = IPOptOptimizer(); - optimizer.setIterationLimit(50); - optimizer.setCheckDerivatives(true); - optimizer.setTolerance(1e-9); - // optimizer.setRecordFullDebugInfo(true); - std::shared_ptr record = optimizer.optimize(&shot); +// #ifdef ALL_TESTS +// TEST(TRAJECTORY, TUNE_SIMPLE_MASS) +// { +// // World +// WorldPtr world = World::create(); +// world->setGravity(Eigen::Vector3s(0, -9.81, 0)); + +// world->setPenetrationCorrectionEnabled(false); + +// ///////////////////////////////////////////////////////////////////// +// // Create the skeleton with a single revolute joint +// ///////////////////////////////////////////////////////////////////// + +// SkeletonPtr swing = Skeleton::create("swing"); + +// std::pair poleJointPair +// = swing->createJointAndBodyNodePair(); +// RevoluteJoint* poleJoint = poleJointPair.first; +// BodyNode* pole = poleJointPair.second; +// poleJoint->setAxis(Eigen::Vector3s::UnitZ()); + +// std::shared_ptr shape( +// new BoxShape(Eigen::Vector3s(0.05, 0.25, 0.05))); +// // ShapeNode* poleShape = +// pole->createShapeNodeWith(shape); +// poleJoint->setControlForceUpperLimit(0, 100.0); +// poleJoint->setControlForceLowerLimit(0, -100.0); +// poleJoint->setVelocityUpperLimit(0, 100.0); +// poleJoint->setVelocityLowerLimit(0, -100.0); +// poleJoint->setPositionUpperLimit(0, 270 * 3.1415 / 180); +// poleJoint->setPositionLowerLimit(0, -270 * 3.1415 / 180); + +// // We're going to tune the full inertia properties of the swinging object +// Eigen::VectorXs upperBounds = Eigen::VectorXs::Ones(3) * 2.0; +// Eigen::VectorXs lowerBounds = Eigen::VectorXs::Ones(3) * -2.0; +// world->getWrtMass()->registerNode( +// pole, +// neural::WrtMassBodyNodeEntryType::INERTIA_COM, +// upperBounds, +// lowerBounds); + +// Eigen::Isometry3s poleOffset = Eigen::Isometry3s::Identity(); +// poleOffset.translation() = Eigen::Vector3s(0, -0.125, 0); +// poleJoint->setTransformFromChildBodyNode(poleOffset); +// poleJoint->setPosition(0, 90 * 3.1415 / 180); + +// world->addSkeleton(swing); + +// assert(world->getNumDofs() == 1); + +// ///////////////////////////////////////////////////////////////////// +// // Define straightforward loss +// ///////////////////////////////////////////////////////////////////// + +// int STEPS = 12; +// int SHOT_LENGTH = 3; +// int GOAL_STEP = 6; +// s_t GOAL_AT_STEP = 0.1; + +// // Get the GOAL_STEP pose as close to GOAL_AT_STEP +// TrajectoryLossFn loss +// = [GOAL_STEP, GOAL_AT_STEP](const TrajectoryRollout* rollout) { +// const Eigen::Ref poses +// = rollout->getPosesConst("identity"); +// s_t poseFive = poses(0, GOAL_STEP); +// return (poseFive - GOAL_AT_STEP) * (poseFive - GOAL_AT_STEP); +// }; + +// // Get the initial pose and final pose as close to each other as possible +// TrajectoryLossFn loopConstraint = [](const TrajectoryRollout* rollout) { +// const Eigen::Ref poses +// = rollout->getPosesConst("identity"); +// s_t firstPose = poses(0, 0); +// s_t lastPose = poses(0, poses.cols() - 1); +// return (firstPose - lastPose) * (firstPose - lastPose); +// }; + +// ///////////////////////////////////////////////////////////////////// +// // Build a trajectory optimization problem +// ///////////////////////////////////////////////////////////////////// + +// LossFn lossFn = LossFn(loss); + +// LossFn constraintFn = LossFn(loopConstraint); +// constraintFn.setLowerBound(0); +// constraintFn.setUpperBound(0); + +// WorldPtr worldPar = world->clone(); + +// MultiShot shot(world, lossFn, STEPS, SHOT_LENGTH, true); +// shot.addConstraint(constraintFn); + +// MultiShot shotPar(worldPar, lossFn, STEPS, SHOT_LENGTH, true); +// shotPar.addConstraint(constraintFn); +// shotPar.setParallelOperationsEnabled(true); + +// int n = shot.getFlatProblemDim(world); +// int constraintDim = shot.getConstraintDim(); +// /* +// Eigen::VectorXs flat = Eigen::VectorXs::Zero(n); +// shot.Problem::flatten(world, flat); +// srand(2); +// flat += Eigen::VectorXs::Random(n) * 0.01; +// std::cout << flat << std::endl; +// shot.Problem::unflatten(world, flat); +// shotPar.Problem::unflatten(world, flat); +// */ + +// ///////////////////////////////////////////////////////////////////// +// // Check Bounds +// ///////////////////////////////////////////////////////////////////// + +// if (true) +// { +// Eigen::VectorXs upperBound = Eigen::VectorXs::Zero(n); +// Eigen::VectorXs lowerBound = Eigen::VectorXs::Zero(n); +// shot.Problem::getUpperBounds(world, upperBound); +// shot.Problem::getLowerBounds(world, lowerBound); + +// Eigen::VectorXs upperBoundPar = Eigen::VectorXs::Zero(n); +// Eigen::VectorXs lowerBoundPar = Eigen::VectorXs::Zero(n); +// shotPar.Problem::getUpperBounds(worldPar, upperBoundPar); +// shotPar.Problem::getLowerBounds(worldPar, lowerBoundPar); + +// if (!equals(upperBound, upperBoundPar, 0)) +// { +// std::cout << "Upper Bounds aren't exactly the same!" << std::endl; +// std::cout << "Serial first segment:" << std::endl +// << upperBound.segment(0, 10) << std::endl; +// std::cout << "Parallel first segment:" << std::endl +// << upperBoundPar.segment(0, 10) << std::endl; +// EXPECT_TRUE(false); +// return; +// } + +// if (!equals(lowerBound, lowerBoundPar, 0)) +// { +// std::cout << "Lower Bounds aren't exactly the same!" << std::endl; +// std::cout << "Serial first segment:" << std::endl +// << lowerBound.segment(0, 10) << std::endl; +// std::cout << "Parallel first segment:" << std::endl +// << lowerBoundPar.segment(0, 10) << std::endl; +// EXPECT_TRUE(false); +// return; +// } + +// Eigen::VectorXs constraintUpperBound = Eigen::VectorXs::Zero(constraintDim); +// Eigen::VectorXs constraintLowerBound = Eigen::VectorXs::Zero(constraintDim); +// shot.getConstraintUpperBounds(constraintUpperBound); +// shot.getConstraintLowerBounds(constraintLowerBound); + +// Eigen::VectorXs constraintUpperBoundPar +// = Eigen::VectorXs::Zero(constraintDim); +// Eigen::VectorXs constraintLowerBoundPar +// = Eigen::VectorXs::Zero(constraintDim); +// shotPar.getConstraintUpperBounds(constraintUpperBoundPar); +// shotPar.getConstraintLowerBounds(constraintLowerBoundPar); + +// if (!equals(constraintUpperBound, constraintUpperBoundPar, 0)) +// { +// std::cout << "Constraint Upper Bounds aren't exactly the same!" +// << std::endl; +// std::cout << "Serial first segment:" << std::endl +// << constraintUpperBound.segment(0, 10) << std::endl; +// std::cout << "Parallel first segment:" << std::endl +// << constraintUpperBoundPar.segment(0, 10) << std::endl; +// EXPECT_TRUE(false); +// return; +// } + +// if (!equals(constraintLowerBound, constraintLowerBoundPar, 0)) +// { +// std::cout << "Constraint Lower Bounds aren't exactly the same!" +// << std::endl; +// std::cout << "Serial first segment:" << std::endl +// << constraintLowerBound.segment(0, 10) << std::endl; +// std::cout << "Parallel first segment:" << std::endl +// << constraintLowerBoundPar.segment(0, 10) << std::endl; +// EXPECT_TRUE(false); +// return; +// } +// } + +// ///////////////////////////////////////////////////////////////////// +// // Check Gradients +// ///////////////////////////////////////////////////////////////////// + +// if (true) +// { +// Eigen::VectorXs grad = Eigen::VectorXs::Zero(n); +// shot.backpropGradient(world, grad); +// Eigen::VectorXs gradPar = Eigen::VectorXs::Zero(n); +// shotPar.backpropGradient(worldPar, gradPar); + +// if (!equals(grad, gradPar, 0)) +// { +// std::cout << "Gradients aren't exactly the same!" << std::endl; +// std::cout << "Serial first segment:" << std::endl +// << grad.segment(0, 10) << std::endl; +// std::cout << "Parallel first segment:" << std::endl +// << gradPar.segment(0, 10) << std::endl; +// EXPECT_TRUE(false); +// return; +// } + +// int m = shot.getNumberNonZeroJacobian(world); + +// Eigen::VectorXs sparseJac = Eigen::VectorXs::Zero(m); +// shot.Problem::getSparseJacobian(world, sparseJac); +// Eigen::VectorXs sparseJacPar = Eigen::VectorXs::Zero(m); +// shotPar.Problem::getSparseJacobian(worldPar, sparseJacPar); + +// if (!equals(sparseJac, sparseJacPar, 0)) +// { +// std::cout << "Gradients aren't exactly the same!" << std::endl; +// std::cout << "Serial first segment:" << std::endl +// << sparseJac.segment(0, 10) << std::endl; +// std::cout << "Parallel first segment:" << std::endl +// << sparseJacPar.segment(0, 10) << std::endl; +// EXPECT_TRUE(false); +// return; +// } +// } + +// ///////////////////////////////////////////////////////////////////// +// // Check Jacobians +// ///////////////////////////////////////////////////////////////////// + +// if (true) +// { +// int dim = shot.getFlatProblemDim(world); +// int numConstraints = shot.getConstraintDim(); +// std::cout << "numConstraints: " << numConstraints << std::endl; + +// Eigen::MatrixXs analyticalJacobian +// = Eigen::MatrixXs::Zero(numConstraints, dim); +// shot.Problem::backpropJacobian(world, analyticalJacobian); +// Eigen::MatrixXs bruteForceJacobian +// = Eigen::MatrixXs::Zero(numConstraints, dim); +// shot.finiteDifferenceJacobian(world, bruteForceJacobian); +// s_t threshold = 1e-8; +// if (!equals(analyticalJacobian, bruteForceJacobian, threshold)) +// { +// std::cout << "Jacobians don't match!" << std::endl; +// // int staticDim = shot.getFlatStaticProblemDim(world); +// std::cout << "Static region size: " << shot.getFlatStaticProblemDim(world) +// << std::endl; +// std::cout << "Analytical first region: " << std::endl +// << analyticalJacobian.block(0, 0, analyticalJacobian.rows(), 10) +// << std::endl; +// std::cout << "Brute force first region: " << std::endl +// << bruteForceJacobian.block(0, 0, analyticalJacobian.rows(), 10) +// << std::endl; +// /* +// for (int i = 0; i < dim; i++) +// { +// Eigen::VectorXs analyticalCol = analyticalJacobian.col(i); +// Eigen::VectorXs bruteForceCol = bruteForceJacobian.col(i); +// if (!equals(analyticalCol, bruteForceCol, threshold)) +// { +// std::cout << "ERROR at col " << shot.getFlatDimName(world, i) << " (" +// << i << ") by " << (analyticalCol - bruteForceCol).norm() +// << std::endl; +// std::cout << "Analytical:" << std::endl << analyticalCol << std::endl; +// std::cout << "Brute Force:" << std::endl << bruteForceCol << +// std::endl; std::cout << "Diff:" << std::endl +// << (analyticalCol - bruteForceCol) << std::endl; +// } +// else +// { +// // std::cout << "Match at col " << shot.getFlatDimName(world, i) << " +// (" +// // << i +// // << ")" << std::endl; +// } +// } +// */ +// EXPECT_TRUE(false); +// return; +// } + +// EXPECT_TRUE(verifySparseJacobian(world, shot)); +// } + +// ///////////////////////////////////////////////////////////////////// +// // Verify flat results +// ///////////////////////////////////////////////////////////////////// + +// Eigen::VectorXs preFlat = Eigen::VectorXs::Zero(n); +// Eigen::VectorXs preFlatPar = Eigen::VectorXs::Zero(n); +// shot.Problem::flatten(world, preFlat); +// shotPar.Problem::flatten(world, preFlatPar); +// if (!equals(preFlat, preFlatPar, 0)) +// { +// std::cout << "Pre-optimization flattening doesn't match!" << std::endl; +// std::cout << "Serial pre-flat: " << std::endl << preFlat << std::endl; +// std::cout << "Parallel pre-flat: " << std::endl << preFlatPar << std::endl; +// for (int i = 0; i < preFlat.size(); i++) +// { +// if (preFlat(i) != preFlatPar(i)) +// { +// std::cout << " Mismatch at index " << i << " (" +// << shot.getFlatDimName(world, i) << ") by " +// << preFlat(i) - preFlatPar(i) << ": " << preFlat(i) << " vs " +// << preFlatPar(i) << std::endl; +// } +// } +// EXPECT_TRUE(false); +// return; +// } + +// ///////////////////////////////////////////////////////////////////// +// // Actually run the optimization +// ///////////////////////////////////////////////////////////////////// + +// // Actually do the optimization + +// int iterationLimit = 10; + +// IPOptOptimizer optimizer = IPOptOptimizer(); +// optimizer.setIterationLimit(iterationLimit); +// optimizer.setCheckDerivatives(true); +// optimizer.setRecoverBest(false); +// // optimizer.setRecordFullDebugInfo(true); +// std::shared_ptr record = optimizer.optimize(&shot); + +// std::shared_ptr recordPar = optimizer.optimize(&shotPar); + +// Eigen::VectorXs endFlat = Eigen::VectorXs::Zero(n); +// Eigen::VectorXs endFlatPar = Eigen::VectorXs::Zero(n); +// shot.Problem::flatten(world, endFlat); +// shotPar.Problem::flatten(worldPar, endFlatPar); +// if (!equals(endFlat, endFlatPar, 0)) +// { +// std::cout << "Results after " << iterationLimit << " steps don't match!" +// << std::endl; +// for (int i = 0; i < endFlat.size(); i++) +// { +// if (endFlat(i) != endFlatPar(i)) +// { +// std::cout << " Mismatch at index " << i << " (" +// << shot.getFlatDimName(world, i) << ") by " +// << endFlat(i) - endFlatPar(i) << ": " << endFlat(i) << " vs " +// << endFlatPar(i) << std::endl; +// } +// } +// } + +// for (int i = 0; i < record->getXs().size(); i++) +// { +// Eigen::VectorXs x0 = record->getXs()[i]; +// Eigen::VectorXs x0Par = recordPar->getXs()[i]; +// if (!equals(x0, x0Par, 0)) +// { +// std::cout << "Xs at eval " << i << " don't match!" << std::endl; +// for (int j = 0; j < x0.size(); j++) +// { +// if (x0(j) != x0Par(j)) +// { +// std::cout << " Mismatch at index " << j << " (" +// << shot.getFlatDimName(world, j) << ") by " +// << x0(j) - x0Par(j) << ": " << x0(j) << " vs " << x0Par(j) +// << std::endl; +// } +// } +// } +// } + +// for (int i = 0; i < record->getLosses().size(); i++) +// { +// s_t loss0 = record->getLosses()[i]; +// s_t loss0Par = recordPar->getLosses()[i]; +// if (loss0 != loss0Par) +// { +// std::cout << "Losses at eval " << i << " don't match by " +// << loss0 - loss0Par << std::endl; +// } +// } + +// for (int i = 0; i < record->getGradients().size(); i++) +// { +// Eigen::VectorXs grad0 = record->getGradients()[i]; +// Eigen::VectorXs grad0Par = recordPar->getGradients()[i]; +// if (!equals(grad0, grad0Par, 0)) +// { +// std::cout << "Gradients at eval " << i << " don't match!" << std::endl; +// for (int j = 0; j < grad0.size(); j++) +// { +// if (grad0(j) != grad0Par(j)) +// { +// std::cout << " Mismatch at index " << j << " (" +// << shot.getFlatDimName(world, j) << ") by " +// << grad0(j) - grad0Par(j) << ": " << grad0(j) << " vs " +// << grad0Par(j) << std::endl; +// } +// } +// } +// } + +// for (int i = 0; i < record->getSparseJacobians().size(); i++) +// { +// Eigen::VectorXs jac0 = record->getSparseJacobians()[i]; +// Eigen::VectorXs jac0Par = recordPar->getSparseJacobians()[i]; + +// Eigen::VectorXi jacRows = Eigen::VectorXi::Zero(jac0.size()); +// Eigen::VectorXi jacCols = Eigen::VectorXi::Zero(jac0.size()); +// shot.getJacobianSparsityStructure(world, jacRows, jacCols); + +// if (!equals(jac0, jac0Par, 0)) +// { +// std::cout << "Jacobians at eval " << i << " don't match!" << std::endl; +// for (int j = 0; j < jac0.size(); j++) +// { +// s_t serial = jac0(j); +// s_t parallel = jac0Par(j); +// if (serial != parallel) +// { +// std::cout << " Mismatch at " << jacRows(j) << "," << jacCols(j) +// << " (" << shot.getFlatDimName(world, jacCols(j)) << ") by " +// << serial - parallel << ": " << serial << " vs " << parallel +// << std::endl; +// } +// } +// } +// } + +// for (int i = 0; i < record->getConstraintValues().size(); i++) +// { +// Eigen::VectorXs con0 = record->getConstraintValues()[i]; +// Eigen::VectorXs con0Par = recordPar->getConstraintValues()[i]; +// if (!equals(con0, con0Par, 0)) +// { +// std::cout << "Constraints at eval " << i << " don't match!" << std::endl; +// } +// } + +// // Playback the trajectory +// TrajectoryRolloutReal withKnots = TrajectoryRolloutReal(&shot); +// shot.getStates(world, &withKnots, nullptr, true); +// } +// #endif - // We should have recovered the mass - s_t error = abs(boxBody->getMass() - TRUE_MASS); - if (error > 1e-7) - { - std::cout << "Recovered mass: " << boxBody->getMass() << std::endl; - std::cout << "Error: " << error << std::endl; - // Get the trajectory - TrajectoryRolloutReal withKnots = TrajectoryRolloutReal(&shot); - shot.getStates(world, &withKnots, nullptr, true); - std::cout << "Original: " << std::endl - << originalPoses.transpose() << std::endl; - std::cout << "Forces: " << std::endl - << withKnots.getControlForces("identity") << std::endl; - std::cout << "Positions: " << std::endl - << withKnots.getPoses("identity") << std::endl; - - EXPECT_TRUE(error < 1e-7); - } -} -#endif +// #ifdef ALL_TESTS +// TEST(TRAJECTORY, RECOVER_MASS) +// { +// // World +// WorldPtr world = World::create(); +// world->setGravity(Eigen::Vector3s(0, -9.81, 0)); + +// world->setPenetrationCorrectionEnabled(false); + +// ///////////////////////////////////////////////////////////////////// +// // Create the skeleton with a single revolute joint +// ///////////////////////////////////////////////////////////////////// + +// SkeletonPtr box = Skeleton::create("box"); + +// std::pair boxJointPair +// = box->createJointAndBodyNodePair(); +// // PrismaticJoint* boxJoint = boxJointPair.first; +// BodyNode* boxBody = boxJointPair.second; + +// std::shared_ptr shape( +// new BoxShape(Eigen::Vector3s(0.05, 0.25, 0.05))); +// // ShapeNode* boxShape = +// boxBody->createShapeNodeWith(shape); + +// // We're going to tune the full inertia properties of the swinging object +// Eigen::VectorXs upperBounds = Eigen::VectorXs::Ones(1) * 5.0; +// Eigen::VectorXs lowerBounds = Eigen::VectorXs::Ones(1) * 0.1; +// world->getWrtMass()->registerNode( +// boxBody, +// neural::WrtMassBodyNodeEntryType::INERTIA_MASS, +// upperBounds, +// lowerBounds); + +// world->addSkeleton(box); + +// assert(world->getNumDofs() == 1); + +// ///////////////////////////////////////////////////////////////////// +// // Run one simulation forward to provide the raw material for our loss to try +// // to recover +// ///////////////////////////////////////////////////////////////////// + +// s_t TRUE_MASS = 2.5; +// int STEPS = 12; +// int SHOT_LENGTH = 3; +// // int GOAL_STEP = 6; +// // s_t GOAL_AT_STEP = 0.1; + +// boxBody->setMass(TRUE_MASS); +// Eigen::VectorXs knownForce = Eigen::VectorXs::Ones(1) * 0.1; +// world->setPositions(Eigen::VectorXs::Zero(1)); +// world->setVelocities(Eigen::VectorXs::Zero(1)); +// world->setTimeStep(1e-1); + +// Eigen::VectorXs originalPoses = Eigen::VectorXs::Zero(STEPS); +// for (int i = 0; i < STEPS; i++) +// { +// world->setControlForces(knownForce); +// world->step(); +// originalPoses(i) = world->getPositions()[0]; +// } + +// // Reset position, scramble mass + +// world->setPositions(Eigen::VectorXs::Zero(1)); +// world->setVelocities(Eigen::VectorXs::Zero(1)); +// boxBody->setMass(0.5); + +// ///////////////////////////////////////////////////////////////////// +// // Define a loss +// ///////////////////////////////////////////////////////////////////// + +// // Get the GOAL_STEP pose as close to GOAL_AT_STEP +// TrajectoryLossFn loss +// = [originalPoses, STEPS](const TrajectoryRollout* rollout) { +// const Eigen::Ref poses +// = rollout->getPosesConst("identity"); +// s_t sum = 0.0; +// for (int i = 0; i < STEPS; i++) +// { +// s_t diff = 1e2 * (poses(0, i) - originalPoses(i)); +// sum += diff * diff; +// } +// return sum; +// }; + +// ///////////////////////////////////////////////////////////////////// +// // Build a trajectory optimization problem +// ///////////////////////////////////////////////////////////////////// + +// LossFn lossFn = LossFn(loss); + +// MultiShot shot(world, lossFn, STEPS, SHOT_LENGTH, false); + +// ///////////////////////////////////////////////////////////////////// +// // Pin the forces +// ///////////////////////////////////////////////////////////////////// + +// for (int i = 0; i < STEPS; i++) +// { +// shot.pinForce(i, knownForce); +// } + +// ///////////////////////////////////////////////////////////////////// +// // Run optimization +// ///////////////////////////////////////////////////////////////////// + +// IPOptOptimizer optimizer = IPOptOptimizer(); +// optimizer.setIterationLimit(50); +// optimizer.setCheckDerivatives(true); +// optimizer.setTolerance(1e-9); +// // optimizer.setRecordFullDebugInfo(true); +// std::shared_ptr record = optimizer.optimize(&shot); + +// // We should have recovered the mass +// s_t error = abs(boxBody->getMass() - TRUE_MASS); +// if (error > 1e-7) +// { +// std::cout << "Recovered mass: " << boxBody->getMass() << std::endl; +// std::cout << "Error: " << error << std::endl; +// // Get the trajectory +// TrajectoryRolloutReal withKnots = TrajectoryRolloutReal(&shot); +// shot.getStates(world, &withKnots, nullptr, true); +// std::cout << "Original: " << std::endl +// << originalPoses.transpose() << std::endl; +// std::cout << "Forces: " << std::endl +// << withKnots.getControlForces("identity") << std::endl; +// std::cout << "Positions: " << std::endl +// << withKnots.getPoses("identity") << std::endl; + +// EXPECT_TRUE(error < 1e-7); +// } +// } +// #endif #ifdef ALL_TESTS TEST(TRAJECTORY, CONSTRAINED_CYCLE) diff --git a/unittests/comprehensive/test_cartpoleRealtime.cpp b/unittests/comprehensive/test_cartpoleRealtime.cpp new file mode 100644 index 000000000..857af5772 --- /dev/null +++ b/unittests/comprehensive/test_cartpoleRealtime.cpp @@ -0,0 +1,779 @@ +/* + * Copyright (c) 2011-2019, The DART development contributors + * All rights reserved. + * + * The list of contributors can be found at: + * https://github.com/dartsim/dart/blob/master/LICENSE + * + * This file is provided under the following "BSD-style" License: + * Redistribution and use in source and binary forms, with or + * without modification, are permitted provided that the following + * conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF + * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include "dart/neural/RestorableSnapshot.hpp" +#include "dart/realtime/MPC.hpp" +#include "dart/realtime/MPCLocal.hpp" +#include "dart/realtime/SSID.hpp" +#include "dart/realtime/Ticker.hpp" +#include "dart/server/GUIWebsocketServer.hpp" +#include "dart/simulation/World.hpp" +#include "dart/trajectory/IPOptOptimizer.hpp" +#include "dart/trajectory/LossFn.hpp" +#include "dart/trajectory/MultiShot.hpp" + +#include "TestHelpers.hpp" +#include "stdio.h" + +//#define COM_SSID +//#define MASS_SSID + +using namespace dart; +using namespace math; +using namespace dynamics; +using namespace simulation; +using namespace neural; +using namespace realtime; +using namespace trajectory; +using namespace server; + +std::shared_ptr getSSIDPosLoss() +{ + TrajectoryLossFn loss = [](const TrajectoryRollout* rollout) { + Eigen::MatrixXs rawPos = rollout->getMetadata("sensors"); + Eigen::MatrixXs sensorPositions = rawPos.block(0,1,rawPos.rows(),rawPos.cols()-1); + int steps = rollout->getPosesConst().cols(); + + Eigen::MatrixXs posError = rollout->getPosesConst() - sensorPositions; + + //std::cout << "Pos In Buffer: " << std::endl << sensorPositions << std::endl; + //std::cout << "Pos In Traj : " << std::endl << rollout->getPosesConst() << std::endl; + + s_t sum = 0.0; + for (int i = 0; i < steps; i++) + { + sum += posError.col(i).squaredNorm(); + } + return sum; + }; + + TrajectoryLossFnAndGrad lossGrad = [](const TrajectoryRollout* rollout, + TrajectoryRollout* gradWrtRollout // OUT + ) { + gradWrtRollout->getPoses().setZero(); + gradWrtRollout->getVels().setZero(); + gradWrtRollout->getControlForces().setZero(); + int steps = rollout->getPosesConst().cols(); + + Eigen::MatrixXs rawPos = rollout->getMetadata("sensors"); + Eigen::MatrixXs sensorPositions = rawPos.block(0,1,rawPos.rows(),rawPos.cols()-1); + + Eigen::MatrixXs posError = rollout->getPosesConst() - sensorPositions; + + gradWrtRollout->getPoses() = 2 * posError; + s_t sum = 0.0; + for (int i = 0; i < steps; i++) + { + sum += posError.col(i).squaredNorm(); + } + return sum; + }; + + return std::make_shared(loss, lossGrad); +} + +std::shared_ptr getSSIDVelPosLoss() +{ + TrajectoryLossFn loss = [](const TrajectoryRollout* rollout) { + + Eigen::MatrixXs rawPos = rollout->getMetadata("sensors"); + Eigen::MatrixXs rawVel = rollout->getMetadata("velocities"); + Eigen::MatrixXs sensorPositions = rawPos.block(0,1,rawPos.rows(),rawPos.cols()-1); + Eigen::MatrixXs sensorVelocities = rawVel.block(0,1,rawVel.rows(),rawVel.cols()-1); + int steps = rollout->getPosesConst().cols(); + + Eigen::MatrixXs posError = rollout->getPosesConst() - sensorPositions; + Eigen::MatrixXs velError = rollout->getVelsConst() - sensorVelocities; + + // std::cout << "Pos Error: " << std::endl << posError << std::endl; + + s_t sum = 0.0; + for (int i = 0; i < steps; i++) + { + sum += posError.col(i).squaredNorm(); + sum += velError.col(i).squaredNorm(); + } + return sum; + }; + + TrajectoryLossFnAndGrad lossGrad = [](const TrajectoryRollout* rollout, + TrajectoryRollout* gradWrtRollout // OUT + ) { + gradWrtRollout->getPoses().setZero(); + gradWrtRollout->getVels().setZero(); + gradWrtRollout->getControlForces().setZero(); + int steps = rollout->getPosesConst().cols(); + + Eigen::MatrixXs rawPos = rollout->getMetadata("sensors"); + Eigen::MatrixXs rawVel = rollout->getMetadata("velocities"); + Eigen::MatrixXs sensorPositions = rawPos.block(0,1,rawPos.rows(),rawPos.cols()-1); + Eigen::MatrixXs sensorVelocities = rawVel.block(0,1,rawVel.rows(),rawVel.cols()-1); + + Eigen::MatrixXs posError = rollout->getPosesConst() - sensorPositions; + Eigen::MatrixXs velError = rollout->getVelsConst() - sensorVelocities; + + gradWrtRollout->getPoses() = 2 * posError; + gradWrtRollout->getVels() = 2 * velError; + s_t sum = 0.0; + for (int i = 0; i < steps; i++) + { + sum += posError.col(i).squaredNorm(); + sum += velError.col(i).squaredNorm(); + } + return sum; + }; + + return std::make_shared(loss, lossGrad); +} + +std::shared_ptr getSSIDVelLoss() +{ + TrajectoryLossFn loss = [](const TrajectoryRollout* rollout) { + Eigen::MatrixXs rawVel = rollout->getMetadata("velocities"); + Eigen::MatrixXs sensorVelocities = rawVel.block(0,1,rawVel.rows(),rawVel.cols()-1); + int steps = rollout->getVelsConst().cols(); + + Eigen::MatrixXs velError = rollout->getVelsConst() - sensorVelocities; + + // std::cout << "Pos Error: " << std::endl << posError << std::endl; + + s_t sum = 0.0; + for (int i = 0; i < steps; i++) + { + sum += velError.col(i).squaredNorm(); + } + return sum; + }; + + TrajectoryLossFnAndGrad lossGrad = [](const TrajectoryRollout* rollout, + TrajectoryRollout* gradWrtRollout // OUT + ) { + gradWrtRollout->getPoses().setZero(); + gradWrtRollout->getVels().setZero(); + gradWrtRollout->getControlForces().setZero(); + int steps = rollout->getVelsConst().cols(); + + Eigen::MatrixXs rawVel = rollout->getMetadata("velocities"); + Eigen::MatrixXs sensorVelocities = rawVel.block(0,1,rawVel.rows(),rawVel.cols()-1); + + Eigen::MatrixXs velError = rollout->getVelsConst() - sensorVelocities; + + gradWrtRollout->getVels() = 2 * velError; + s_t sum = 0.0; + for (int i = 0; i < steps; i++) + { + sum += velError.col(i).squaredNorm(); + } + return sum; + }; + + return std::make_shared(loss, lossGrad); +} + +WorldPtr createWorld(s_t timestep) +{ + WorldPtr world = World::create(); + world->setGravity(Eigen::Vector3s(0, -9.81, 0)); + SkeletonPtr cartpole = Skeleton::create("cartpole"); + // Create Cart + std::pair sledPair + = cartpole->createJointAndBodyNodePair(nullptr); + sledPair.first->setAxis(Eigen::Vector3s(1, 0, 0)); + std::shared_ptr sledShapeBox( + new BoxShape(Eigen::Vector3s(0.5, 0.1, 0.1))); + ShapeNode* sledShape + = sledPair.second->createShapeNodeWith(sledShapeBox); + sledShape->getVisualAspect()->setColor(Eigen::Vector3s(0.5, 0.5, 0.5)); + + // Create Pole + std::pair armPair + = cartpole->createJointAndBodyNodePair(sledPair.second); + armPair.first->setAxis(Eigen::Vector3s(0, 0, 1)); + std::shared_ptr armShapeBox( + new BoxShape(Eigen::Vector3s(0.1, 1.0, 0.1))); + ShapeNode* armShape + = armPair.second->createShapeNodeWith(armShapeBox); + armShape->getVisualAspect()->setColor(Eigen::Vector3s(0.7, 0.7, 0.7)); + Eigen::Isometry3s armOffset = Eigen::Isometry3s::Identity(); + armOffset.translation() = Eigen::Vector3s(0, -0.5, 0); + armPair.first->setTransformFromChildBodyNode(armOffset); + + world->addSkeleton(cartpole); + cartpole->setControlForceUpperLimit(0, 15); + cartpole->setControlForceLowerLimit(0, -15); + cartpole->setControlForceUpperLimit(1, 0); + cartpole->setControlForceLowerLimit(1, 0); + cartpole->setVelocityUpperLimit(0, 1000); + cartpole->setVelocityLowerLimit(0, -1000); + cartpole->setPositionUpperLimit(0, 10); + cartpole->setPositionLowerLimit(0, -10); + cartpole->setPosition(0, 0); + cartpole->setPosition(1, 30.0 / 180.0 * 3.1415); + world->setTimeStep(timestep); + + return world; +} + +std::mt19937 initializeRandom() +{ + std::random_device rd{}; + std::mt19937 gen{rd()}; + return gen; +} + +Eigen::VectorXs rand_normal(size_t length, s_t mean, s_t stddev, std::mt19937 random_gen) +{ + Eigen::VectorXs result = Eigen::VectorXs::Zero(length); + std::normal_distribution<> dist{mean, stddev}; + + for(int i = 0; i < length; i++) + { + result(i) += (s_t)(dist(random_gen)); + } + return result; +} + +void recordObs(size_t now, SSID* ssid, WorldPtr realtimeWorld) +{ + ssid->registerLock(); + ssid->registerControls(now, realtimeWorld->getControlForces()); + ssid->registerSensors(now, realtimeWorld->getPositions(),0); + ssid->registerSensors(now, realtimeWorld->getVelocities(),1); + ssid->registerUnlock(); +} + +void recordObsWithNoise(size_t now, SSID* ssid, WorldPtr realtimeWorld, s_t noise_scale, std::mt19937 random_gen) +{ + ssid->registerLock(); + Eigen::VectorXs control_force = realtimeWorld->getControlForces(); + //Eigen::VectorXs force_eps = rand_normal(control_force.size(), 0, noise_scale, random_gen); + ssid->registerControls(now, control_force); + + Eigen::VectorXs position = realtimeWorld->getPositions(); + Eigen::VectorXs position_eps = rand_normal(position.size(), 0, noise_scale, random_gen); + ssid->registerSensors(now, position + position_eps, 0); + + Eigen::VectorXs velocity = realtimeWorld->getVelocities(); + Eigen::VectorXs velocity_eps = rand_normal(velocity.size(), 0, noise_scale, random_gen); + ssid->registerSensors(now, velocity + velocity_eps, 1); + ssid->registerUnlock(); +} + +#ifdef COM_SSID +TEST(REALTIME, CARTPOLE_REALTIME_COM) +{ + WorldPtr world = createWorld(1.0 / 100); + + // Initialize Hyperparameters + Eigen::Vector3s beta(0.1, 1, 0); + world->setLinkBetaIndex(beta,1); + + Eigen::Vector2s sensorDims(world->getNumDofs(), world->getNumDofs()); + + int steps = 300; + int millisPerTimestep = world->getTimeStep() * 1000; + int planningHorizonMillis = steps * millisPerTimestep; + + int inferenceSteps = 5; + int inferenceHistoryMillis = inferenceSteps * millisPerTimestep; + s_t scale = 1.0; + + s_t goalX = 1.0; + + TrajectoryLossFn loss = [&goalX](const TrajectoryRollout* rollout) { + int steps = rollout->getPosesConst("identity").cols(); + s_t sum = 0.0; + for (int i = 0; i < steps; i++) + { + // rollout->getVelsConst().col(i).squaredNorm() + s_t xPos = rollout->getPosesConst()(0, i); + s_t theta = rollout->getPosesConst()(1, i); + sum += (goalX - xPos) * (goalX - xPos) + theta * theta; + } + return sum; + }; + + TrajectoryLossFnAndGrad lossGrad = + [&goalX]( + const TrajectoryRollout* rollout, + TrajectoryRollout* gradWrtRollout // OUT + ) { + gradWrtRollout->getPoses().setZero(); + gradWrtRollout->getVels().setZero(); + gradWrtRollout->getControlForces().setZero(); + int steps = rollout->getPosesConst().cols(); + for (int i = 0; i < steps; i++) + { + gradWrtRollout->getPoses()(0, i) + = 2 * (rollout->getPosesConst()(0, i) - goalX); + gradWrtRollout->getPoses()(1, i) = 2 * rollout->getPosesConst()(1, i); + // gradWrtRollout->getVels().col(i) = 2 * + // rollout->getVelsConst().col(i); + } + + s_t sum = 0.0; + for (int i = 0; i < steps; i++) + { + // rollout->getVelsConst().col(i).squaredNorm() + s_t xPos = rollout->getPosesConst()(0, i); + s_t theta = rollout->getPosesConst()(1, i); + sum += (goalX - xPos) * (goalX - xPos) + theta * theta; + } + return sum; + }; + + std::shared_ptr ssidWorld = world->clone(); + + ssidWorld->tuneMass( + world->getBodyNodeByIndex(1), + WrtMassBodyNodeEntryType::INERTIA_COM_MU, + Eigen::VectorXs::Ones(1) * 0.5, + Eigen::VectorXs::Ones(1) * -0.5); + + SSID ssid = SSID(ssidWorld, + getSSIDVelPosLoss(), + inferenceHistoryMillis, + sensorDims, + inferenceSteps, + scale); + + std::mutex lock; + ssid.attachMutex(lock); + + ssid.setInitialPosEstimator( + [](Eigen::MatrixXs sensors, long) { + return sensors.col(0); + }); + + ssid.setInitialVelEstimator( + [](Eigen::MatrixXs sensors, long){ + return sensors.col(0); + }); + + world->clearTunableMassThisInstance(); + MPCLocal mpcLocal = MPCLocal(world, + std::make_shared(loss, lossGrad), + planningHorizonMillis, + scale); + mpcLocal.setSilent(true); + mpcLocal.setMaxIterations(7); + mpcLocal.setEnableLineSearch(false); + mpcLocal.setEnableOptimizationGuards(true); + + bool init_flag = true; + ssid.registerInferListener([&](long time, + Eigen::VectorXs pos, + Eigen::VectorXs vel, + Eigen::VectorXs mu, + long) { + mpcLocal.recordGroundTruthState(time, pos, vel, mu); + mpcLocal.setMUchange(mu(0)); + if(!init_flag) + { + s_t old_mu = world->getLinkMUIndex(1); + world->setLinkMUIndex(0.9*old_mu+0.1*mu(0),1); + } + else + { + world->setLinkMUIndex(mu(0),1); + } + + }); + + std::function getControlForces = [&]() { + Eigen::VectorXs forces = mpcLocal.getControlForceNow(); + // ssid.registerControlsNow(forces); + return forces; + }; + std::function + recordState + = [&](Eigen::VectorXs pos, Eigen::VectorXs vel, Eigen::VectorXs mu) { + mpcLocal.recordGroundTruthStateNow(pos, vel, mu); + }; + + std::shared_ptr realtimeUnderlyingWorld = world->clone(); + + GUIWebsocketServer server; + server.createSphere( + "goal", + 0.1, + Eigen::Vector3s(goalX, 1.0, 0), + Eigen::Vector3s(1.0, 0.0, 0.0)); + server.registerDragListener("goal", [&](Eigen::Vector3s dragTo) { + goalX = dragTo(0); + dragTo(1) = 1.0; + dragTo(2) = 0.0; + server.setObjectPosition("goal", dragTo); + }); + std::string key = "com mu"; + + Ticker ticker = Ticker(1*realtimeUnderlyingWorld->getTimeStep()); + + auto sledBodyVisual = realtimeUnderlyingWorld->getSkeleton("cartpole") + ->getBodyNodes()[0] + ->getShapeNodesWith()[0] + ->getVisualAspect(); + Eigen::Vector3s originalColor = sledBodyVisual->getColor(); + float mu = 0.05; + float id_mu = -0.2; + size_t total_step = 0; + realtimeUnderlyingWorld->setLinkMUIndex(mu,1); + ssidWorld->setLinkMUIndex(id_mu,1); + world->setLinkMUIndex(id_mu,1); + // Sanity Check + std::cout<getLinkBetas()<getLinkBetas()<getLinkBetas()<setControlForces(mpcforces); + + if (server.getKeysDown().count("a")) + { + Eigen::VectorXs perturbedForces + = realtimeUnderlyingWorld->getControlForces(); + perturbedForces(0) = -15.0; + realtimeUnderlyingWorld->setControlForces(perturbedForces); + sledBodyVisual->setColor(Eigen::Vector3s(1, 0, 0)); + } + else if (server.getKeysDown().count("e")) + { + Eigen::VectorXs perturbedForces + = realtimeUnderlyingWorld->getControlForces(); + perturbedForces(0) = 15.0; + realtimeUnderlyingWorld->setControlForces(perturbedForces); + sledBodyVisual->setColor(Eigen::Vector3s(0, 1, 0)); + } + else + { + sledBodyVisual->setColor(originalColor); + } + if (server.getKeysDown().count(",")) + { + // Increase mu + mu = 1.0; + realtimeUnderlyingWorld->setLinkMUIndex(mu,1); + } + else if (server.getKeysDown().count("o")) + { + // Decrease mass + mu = 0; + realtimeUnderlyingWorld->setLinkMUIndex(mu,1); + } + + recordObs(now, &ssid, realtimeUnderlyingWorld); + + realtimeUnderlyingWorld->step(); + + mpcLocal.recordGroundTruthState( + now, + realtimeUnderlyingWorld->getPositions(), + realtimeUnderlyingWorld->getVelocities(), + realtimeUnderlyingWorld->getMasses()); // May be a problem? + + if(total_step%5==0) + { + id_mu = world->getLinkMUIndex(1); + server.renderWorld(realtimeUnderlyingWorld); + server.createText(key,"Current MU: "+std::to_string(id_mu),Eigen::Vector2i(100,100),Eigen::Vector2i(200,200)); + total_step = 0; + } + total_step += 1; + }); + + mpcLocal.registerReplanningListener( + [&](long , + const trajectory::TrajectoryRollout* rollout, + long ) { + server.renderTrajectoryLines(world, rollout->getPosesConst()); + }); + + server.registerConnectionListener([&]() { + ticker.start(); + mpcLocal.start(); + ssid.start(); + }); + server.registerShutdownListener([&]() { mpcLocal.stop(); }); + server.serve(8070); + server.blockWhileServing(); +} +#endif + +#ifdef MASS_SSID +TEST(REALTIME, CARTPOLE_MPC_MASS) +{ + WorldPtr world = createWorld(1.0/100); + + // Initialize Hyperparameters + std::mt19937 rand_gen = initializeRandom(); + s_t noise_stddev = 0.01; + + // planning parameters + int steps = 300; + int millisPerTimestep = world->getTimeStep() * 1000; + int planningHorizonMillis = steps * millisPerTimestep; + + // SSID Parameters + s_t scale = 1.0; + int inferenceSteps = 10; + int inferenceHistoryMillis = inferenceSteps * millisPerTimestep; + + s_t goalX = 1.0; + + TrajectoryLossFn loss = [&goalX](const TrajectoryRollout* rollout) { + int steps = rollout->getPosesConst("identity").cols(); + s_t sum = 0.0; + for (int i = 0; i < steps; i++) + { + s_t xPos = rollout->getPosesConst()(0, i); + s_t theta = rollout->getPosesConst()(1, i); + sum += (goalX - xPos) * (goalX - xPos) + theta * theta; + } + return sum; + }; + + TrajectoryLossFnAndGrad lossGrad = + [&goalX]( + const TrajectoryRollout* rollout, + TrajectoryRollout* gradWrtRollout // OUT + ) { + gradWrtRollout->getPoses().setZero(); + gradWrtRollout->getVels().setZero(); + gradWrtRollout->getControlForces().setZero(); + int steps = rollout->getPosesConst().cols(); + for (int i = 0; i < steps; i++) + { + gradWrtRollout->getPoses()(0, i) + = 2 * (rollout->getPosesConst()(0, i) - goalX); + gradWrtRollout->getPoses()(1, i) = 2 * rollout->getPosesConst()(1, i); + } + s_t sum = 0.0; + for (int i = 0; i < steps; i++) + { + s_t xPos = rollout->getPosesConst()(0, i); + s_t theta = rollout->getPosesConst()(1, i); + sum += (goalX - xPos) * (goalX - xPos) + theta * theta; + } + return sum; + }; + + std::shared_ptr ssidWorld = world->clone(); + ssidWorld->tuneMass( + world->getBodyNodeByIndex(0), + WrtMassBodyNodeEntryType::INERTIA_MASS, + Eigen::VectorXs::Ones(1) * 5.0, + Eigen::VectorXs::Ones(1) * 0.2); + + ssidWorld->tuneMass( + world->getBodyNodeByIndex(1), + WrtMassBodyNodeEntryType::INERTIA_MASS, + Eigen::VectorXs::Ones(1) * 5.0, + Eigen::VectorXs::Ones(1) * 0.2); + + Eigen::VectorXs sensorDims = Eigen::VectorXs::Zero(2); + sensorDims(0) = world->getNumDofs(); + sensorDims(1) = world->getNumDofs(); + SSID ssid = SSID(ssidWorld, + getSSIDVelPosLoss(), + inferenceHistoryMillis, + sensorDims, + inferenceSteps, + scale); + + std::mutex lock; + std::mutex param_lock; + ssid.attachMutex(lock); + ssid.attachParamMutex(param_lock); + + ssid.setInitialPosEstimator( + [](Eigen::MatrixXs sensors, long) { + return sensors.col(0); + }); + + ssid.setInitialVelEstimator( + [](Eigen::MatrixXs sensors, long){ + return sensors.col(0); + }); + + Eigen::Vector2i index; + index(0) = 0; + index(1) = 1; + ssid.setSSIDIndex(index); + + world->clearTunableMassThisInstance(); + MPCLocal mpcLocal = MPCLocal(world, + std::make_shared(loss, lossGrad), + planningHorizonMillis, + scale); + mpcLocal.setSilent(true); + mpcLocal.setMaxIterations(7); + mpcLocal.setEnableLineSearch(false); + mpcLocal.setEnableOptimizationGuards(true); + + ssid.registerInferListener([&](long time, + Eigen::VectorXs pos, + Eigen::VectorXs vel, + Eigen::VectorXs mass, + long) { + mpcLocal.recordGroundTruthState(time, pos, vel, mass); + // If we need to do SSID with multiple node this detection need to work with vector + mpcLocal.setParameterChange(mass); + world->setLinkMassIndex(mass(0), 0); + world->setLinkMassIndex(mass(1), 1); + }); + + std::shared_ptr realtimeUnderlyingWorld = world->clone(); + GUIWebsocketServer server; + + server.createSphere( + "goal", + 0.1, + Eigen::Vector3s(goalX, 1.0, 0), + Eigen::Vector3s(1.0, 0.0, 0.0)); + server.registerDragListener("goal", [&](Eigen::Vector3s dragTo) { + goalX = dragTo(0); + dragTo(1) = 1.0; + dragTo(2) = 0.0; + server.setObjectPosition("goal", dragTo); + }); + std::string key = "mass"; + + Ticker ticker = Ticker(scale*realtimeUnderlyingWorld->getTimeStep()); + + auto sledBodyVisual = realtimeUnderlyingWorld->getSkeleton("cartpole") + ->getBodyNodes()[0] + ->getShapeNodesWith()[0] + ->getVisualAspect(); + Eigen::Vector3s originalColor = sledBodyVisual->getColor(); + Eigen::Vector2s masses(2.0, 1.5); + Eigen::Vector2s id_masses(1.0, 1.0); + size_t total_step = 0; + realtimeUnderlyingWorld->setLinkMassIndex(masses(0), 0); + realtimeUnderlyingWorld->setLinkMassIndex(masses(1), 1); + ssidWorld->setLinkMassIndex(id_masses(0), 0); + ssidWorld->setLinkMassIndex(id_masses(1), 1); + world->setLinkMassIndex(id_masses(0), 0); + world->setLinkMassIndex(id_masses(1), 1); + + ticker.registerTickListener([&](long now) { + Eigen::VectorXs mpcforces = mpcLocal.getControlForce(now); + realtimeUnderlyingWorld->setControlForces(mpcforces); + + if (server.getKeysDown().count("a")) + { + Eigen::VectorXs perturbedForces + = realtimeUnderlyingWorld->getControlForces(); + perturbedForces(0) = -15.0; + realtimeUnderlyingWorld->setControlForces(perturbedForces); + sledBodyVisual->setColor(Eigen::Vector3s(1, 0, 0)); + } + else if (server.getKeysDown().count("e")) + { + Eigen::VectorXs perturbedForces + = realtimeUnderlyingWorld->getControlForces(); + perturbedForces(0) = 15.0; + realtimeUnderlyingWorld->setControlForces(perturbedForces); + sledBodyVisual->setColor(Eigen::Vector3s(0, 1, 0)); + } + else + { + sledBodyVisual->setColor(originalColor); + } + if (server.getKeysDown().count(",")) + { + // Increase mass + masses(0) = 3.0; + masses(1) = 2.5; + realtimeUnderlyingWorld->setLinkMassIndex(masses(0), 0); + realtimeUnderlyingWorld->setLinkMassIndex(masses(1), 1); + } + else if (server.getKeysDown().count("o")) + { + // Decrease mass + masses(0) = 1.0; + masses(1) = 0.5; + realtimeUnderlyingWorld->setLinkMassIndex(masses(0), 0); + realtimeUnderlyingWorld->setLinkMassIndex(masses(1), 1); + } + + //recordObs(now, &ssid, realtimeUnderlyingWorld); + recordObsWithNoise(now, &ssid, realtimeUnderlyingWorld, noise_stddev, rand_gen); + realtimeUnderlyingWorld->step(); + id_masses(0) = world->getLinkMassIndex(0); + id_masses(1) = world->getLinkMassIndex(1); + mpcLocal.recordGroundTruthState( + now, + realtimeUnderlyingWorld->getPositions(), + realtimeUnderlyingWorld->getVelocities(), + realtimeUnderlyingWorld->getMasses()); + if(total_step % 5 == 0) + { + server.renderWorld(realtimeUnderlyingWorld); + server.createText(key, + "Current Masses: "+std::to_string(id_masses(0))+" "+std::to_string(id_masses(1)), + Eigen::Vector2i(100,100), + Eigen::Vector2i(200,200)); + total_step = 0; + } + total_step += 1; + }); + + mpcLocal.registerReplanningListener( + [&](long /* time */, + const trajectory::TrajectoryRollout* rollout, + long /* duration */) { + server.renderTrajectoryLines(world, rollout->getPosesConst()); + }); + + server.registerConnectionListener([&]() { + ticker.start(); + mpcLocal.start(); + ssid.start(); + ssid.startSlow(); + }); + server.registerShutdownListener([&]() { mpcLocal.stop(); }); + server.serve(8070); + server.blockWhileServing(); +} +#endif diff --git a/unittests/comprehensive/test_cartpoleSSID.cpp b/unittests/comprehensive/test_cartpoleSSID.cpp new file mode 100644 index 000000000..fe9d7394b --- /dev/null +++ b/unittests/comprehensive/test_cartpoleSSID.cpp @@ -0,0 +1,746 @@ +/* + * Copyright (c) 2011-2019, The DART development contributors + * All rights reserved. + * + * The list of contributors can be found at: + * https://github.com/dartsim/dart/blob/master/LICENSE + * + * This file is provided under the following "BSD-style" License: + * Redistribution and use in source and binary forms, with or + * without modification, are permitted provided that the following + * conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF + * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include "dart/neural/RestorableSnapshot.hpp" +#include "dart/realtime/MPC.hpp" +#include "dart/realtime/MPCLocal.hpp" +#include "dart/realtime/MPCRemote.hpp" +#include "dart/realtime/SSID.hpp" +#include "dart/realtime/Ticker.hpp" +#include "dart/server/GUIWebsocketServer.hpp" +#include "dart/simulation/World.hpp" +#include "dart/trajectory/IPOptOptimizer.hpp" +#include "dart/trajectory/LossFn.hpp" +#include "dart/trajectory/MultiShot.hpp" + +#include "TestHelpers.hpp" +#include "stdio.h" + +#define MASS_SSID +#define COM_SSID +#define MOI_SSID +#define DAMP_SSID +#define SPRING_SSID + +using namespace dart; +using namespace math; +using namespace dynamics; +using namespace simulation; +using namespace neural; +using namespace realtime; +using namespace trajectory; +using namespace server; + +std::shared_ptr getSSIDPosLoss() +{ + TrajectoryLossFn loss = [](const TrajectoryRollout* rollout) { + Eigen::MatrixXs rawPos = rollout->getMetadata("sensors"); + Eigen::MatrixXs sensorPositions = rawPos.block(0,1,rawPos.rows(),rawPos.cols()-1); + int steps = rollout->getPosesConst().cols(); + + Eigen::MatrixXs posError = rollout->getPosesConst() - sensorPositions; + + //std::cout << "Pos In Buffer: " << std::endl << sensorPositions << std::endl; + //std::cout << "Pos In Traj : " << std::endl << rollout->getPosesConst() << std::endl; + + s_t sum = 0.0; + for (int i = 0; i < steps; i++) + { + sum += posError.col(i).squaredNorm(); + } + return sum; + }; + + TrajectoryLossFnAndGrad lossGrad = [](const TrajectoryRollout* rollout, + TrajectoryRollout* gradWrtRollout // OUT + ) { + gradWrtRollout->getPoses().setZero(); + gradWrtRollout->getVels().setZero(); + gradWrtRollout->getControlForces().setZero(); + int steps = rollout->getPosesConst().cols(); + + Eigen::MatrixXs rawPos = rollout->getMetadata("sensors"); + Eigen::MatrixXs sensorPositions = rawPos.block(0,1,rawPos.rows(),rawPos.cols()-1); + + Eigen::MatrixXs posError = rollout->getPosesConst() - sensorPositions; + + gradWrtRollout->getPoses() = 2 * posError; + s_t sum = 0.0; + for (int i = 0; i < steps; i++) + { + sum += posError.col(i).squaredNorm(); + } + return sum; + }; + + return std::make_shared(loss, lossGrad); +} + +std::shared_ptr getSSIDVelPosLoss() +{ + TrajectoryLossFn loss = [](const TrajectoryRollout* rollout) { + + Eigen::MatrixXs rawPos = rollout->getMetadata("sensors"); + Eigen::MatrixXs rawVel = rollout->getMetadata("velocities"); + Eigen::MatrixXs sensorPositions = rawPos.block(0,1,rawPos.rows(),rawPos.cols()-1); + Eigen::MatrixXs sensorVelocities = rawVel.block(0,1,rawVel.rows(),rawVel.cols()-1); + int steps = rollout->getPosesConst().cols(); + + Eigen::MatrixXs posError = rollout->getPosesConst() - sensorPositions; + Eigen::MatrixXs velError = rollout->getVelsConst() - sensorVelocities; + + // std::cout << "Pos Error: " << std::endl << posError << std::endl; + + s_t sum = 0.0; + for (int i = 0; i < steps; i++) + { + sum += posError.col(i).squaredNorm(); + sum += velError.col(i).squaredNorm(); + } + return sum; + }; + + TrajectoryLossFnAndGrad lossGrad = [](const TrajectoryRollout* rollout, + TrajectoryRollout* gradWrtRollout // OUT + ) { + gradWrtRollout->getPoses().setZero(); + gradWrtRollout->getVels().setZero(); + gradWrtRollout->getControlForces().setZero(); + int steps = rollout->getPosesConst().cols(); + + Eigen::MatrixXs rawPos = rollout->getMetadata("sensors"); + Eigen::MatrixXs rawVel = rollout->getMetadata("velocities"); + Eigen::MatrixXs sensorPositions = rawPos.block(0,1,rawPos.rows(),rawPos.cols()-1); + Eigen::MatrixXs sensorVelocities = rawVel.block(0,1,rawVel.rows(),rawVel.cols()-1); + + Eigen::MatrixXs posError = rollout->getPosesConst() - sensorPositions; + Eigen::MatrixXs velError = rollout->getVelsConst() - sensorVelocities; + + gradWrtRollout->getPoses() = 2 * posError; + gradWrtRollout->getVels() = 2 * velError; + s_t sum = 0.0; + for (int i = 0; i < steps; i++) + { + sum += posError.col(i).squaredNorm(); + sum += velError.col(i).squaredNorm(); + } + return sum; + }; + + return std::make_shared(loss, lossGrad); +} + +std::shared_ptr getSSIDVelLoss() +{ + TrajectoryLossFn loss = [](const TrajectoryRollout* rollout) { + Eigen::MatrixXs rawVel = rollout->getMetadata("velocities"); + Eigen::MatrixXs sensorVelocities = rawVel.block(0,1,rawVel.rows(),rawVel.cols()-1); + int steps = rollout->getVelsConst().cols(); + + Eigen::MatrixXs velError = rollout->getVelsConst() - sensorVelocities; + + // std::cout << "Pos Error: " << std::endl << posError << std::endl; + + s_t sum = 0.0; + for (int i = 0; i < steps; i++) + { + sum += velError.col(i).squaredNorm(); + } + return sum; + }; + + TrajectoryLossFnAndGrad lossGrad = [](const TrajectoryRollout* rollout, + TrajectoryRollout* gradWrtRollout // OUT + ) { + gradWrtRollout->getPoses().setZero(); + gradWrtRollout->getVels().setZero(); + gradWrtRollout->getControlForces().setZero(); + int steps = rollout->getVelsConst().cols(); + + Eigen::MatrixXs rawVel = rollout->getMetadata("velocities"); + Eigen::MatrixXs sensorVelocities = rawVel.block(0,1,rawVel.rows(),rawVel.cols()-1); + + Eigen::MatrixXs velError = rollout->getVelsConst() - sensorVelocities; + + gradWrtRollout->getVels() = 2 * velError; + s_t sum = 0.0; + for (int i = 0; i < steps; i++) + { + sum += velError.col(i).squaredNorm(); + } + return sum; + }; + + return std::make_shared(loss, lossGrad); +} + +WorldPtr createWorld(s_t timestep) +{ + WorldPtr world = World::create(); + world->setGravity(Eigen::Vector3s(0, -9.81, 0)); + SkeletonPtr cartpole = Skeleton::create("cartpole"); + // Create Cart + std::pair sledPair + = cartpole->createJointAndBodyNodePair(nullptr); + sledPair.first->setAxis(Eigen::Vector3s(1, 0, 0)); + std::shared_ptr sledShapeBox( + new BoxShape(Eigen::Vector3s(0.5, 0.1, 0.1))); + ShapeNode* sledShape + = sledPair.second->createShapeNodeWith(sledShapeBox); + sledShape->getVisualAspect()->setColor(Eigen::Vector3s(0.5, 0.5, 0.5)); + + // Create Pole + std::pair armPair + = cartpole->createJointAndBodyNodePair(sledPair.second); + armPair.first->setAxis(Eigen::Vector3s(0, 0, 1)); + std::shared_ptr armShapeBox( + new BoxShape(Eigen::Vector3s(0.1, 1.0, 0.1))); + ShapeNode* armShape + = armPair.second->createShapeNodeWith(armShapeBox); + armShape->getVisualAspect()->setColor(Eigen::Vector3s(0.7, 0.7, 0.7)); + Eigen::Isometry3s armOffset = Eigen::Isometry3s::Identity(); + armOffset.translation() = Eigen::Vector3s(0, -0.5, 0); + armPair.first->setTransformFromChildBodyNode(armOffset); + + world->addSkeleton(cartpole); + cartpole->setControlForceUpperLimit(0, 15); + cartpole->setControlForceLowerLimit(0, -15); + cartpole->setControlForceUpperLimit(1, 0); + cartpole->setControlForceLowerLimit(1, 0); + cartpole->setVelocityUpperLimit(0, 1000); + cartpole->setVelocityLowerLimit(0, -1000); + cartpole->setPositionUpperLimit(0, 10); + cartpole->setPositionLowerLimit(0, -10); + cartpole->setPosition(0, 0); + cartpole->setPosition(1, 30.0 / 180.0 * 3.1415); + world->setTimeStep(timestep); + + return world; +} + +std::mt19937 initializeRandom() +{ + std::random_device rd{}; + std::mt19937 gen{rd()}; + return gen; +} + +Eigen::VectorXs rand_normal(size_t length, s_t mean, s_t stddev, std::mt19937 random_gen) +{ + Eigen::VectorXs result = Eigen::VectorXs::Zero(length); + std::normal_distribution<> dist{mean, stddev}; + + for(int i = 0; i < length; i++) + { + result(i) += (s_t)(dist(random_gen)); + } + return result; +} + +void recordObs(size_t now, SSID* ssid, WorldPtr realtimeWorld) +{ + ssid->registerControls(now, realtimeWorld->getControlForces()); + ssid->registerSensors(now, realtimeWorld->getPositions(), 0); + ssid->registerSensors(now, realtimeWorld->getVelocities(), 1); +} + +void recordObsWithNoise(size_t now, SSID* ssid, WorldPtr realtimeWorld, s_t noise_scale, std::mt19937 random_gen) +{ + Eigen::VectorXs control_force = realtimeWorld->getControlForces(); + ssid->registerControls(now, control_force); + + Eigen::VectorXs position = realtimeWorld->getPositions(); + Eigen::VectorXs position_eps = rand_normal(position.size(), 0, noise_scale, random_gen); + ssid->registerSensors(now, position + position_eps, 0); + + Eigen::VectorXs velocity = realtimeWorld->getVelocities(); + Eigen::VectorXs velocity_eps = rand_normal(velocity.size(), 0, noise_scale, random_gen); + ssid->registerSensors(now, velocity + velocity_eps, 1); +} + +#ifdef MASS_SSID +TEST(REALTIME, CARTPOLE_MASS_SSID) +{ + //////////////////////////////////////////////////////////// + // Create a cartpole example + //////////////////////////////////////////////////////////// + + // World + WorldPtr world = createWorld(1.0/100); + #ifdef WITH_NOISE + std::mt19937 random_gen = initializeRandom(); + s_t noise_scale = 0.01; + #endif + + std::cout << "A_k Matrix of cartpole\n" + << world->getSkeleton(0)->getLinkAkMatrixIndex(1) + << std::endl; + + world->tuneMass( + world->getBodyNodeByIndex(1), + WrtMassBodyNodeEntryType::INERTIA_MASS, + Eigen::VectorXs::Ones(1) * 5.0, + Eigen::VectorXs::Ones(1) * 0.2); + + std::shared_ptr lossFn = getSSIDPosLoss(); + //std::shared_ptr lossFn = getSSIDVelLoss(); + //std::shared_ptr lossFn = getSSIDVelPosLoss(); + + // 300 timesteps + s_t scale = 1.0; + int millisPerTimestep = world->getTimeStep() * 1000; + int steps = 10; + int inferenceHistoryMillis = steps * millisPerTimestep; + // int advanceSteps = 70; + Eigen::VectorXs sensorDims = Eigen::VectorXs::Zero(2); + sensorDims(0) = world->getNumDofs(); + sensorDims(1) = world->getNumDofs(); + SSID ssid = SSID(world, + lossFn, + inferenceHistoryMillis, + sensorDims, + steps, + scale); + Eigen::VectorXi ssid_index = Eigen::VectorXi::Ones(1); + ssid.setSSIDMassIndex(ssid_index); + ssid.setInitialPosEstimator( + [](Eigen::MatrixXs sensors, long /* timestamp */) { + return sensors.col(0); + }); + + ssid.setInitialVelEstimator( + [](Eigen::MatrixXs sensors, long) + { + return sensors.col(0); + } + ); + float realmass = 2.0; + world->setLinkMassIndex(realmass, 1); + float init_mass = 1.0; + for (int i = 0; i < 100; i++) + { + long time = i * millisPerTimestep; + Eigen::VectorXs forces = Eigen::VectorXs::Ones(world->getNumDofs()); + #ifdef WITH_NOISE + // Eigen::VectorXs forces_eps = rand_normal(forces.size(), 0, 0.01, random_gen); + Eigen::VectorXs forces_eps = Eigen::VectorXs::Zero(forces.size()); + world->setControlForces(forces + forces_eps); + #else + world->setControlForces(forces); + #endif + + #ifndef WITH_NOISE + recordObs(time, &ssid, world); + #else + recordObsWithNoise(time, &ssid, world, noise_scale, random_gen); + #endif + world->step(); + + if(i%5==0 && i >= steps+10) + { + world->setLinkMassIndex(init_mass, 1); + ssid.runInference(time); + init_mass = world->getLinkMassIndex(1); + EXPECT_TRUE(abs(init_mass - realmass) <= 1e-3); + world->setLinkMassIndex(realmass, 1); + } + } +} +#endif + +#ifdef COM_SSID +TEST(REALTIME, CARTPOLE_COM_SSID) +{ + //////////////////////////////////////////////////////////// + // Create a cartpole example + //////////////////////////////////////////////////////////// + + // World + WorldPtr world = createWorld(1.0/100); + #ifdef WITH_NOISE + std::mt19937 random_gen = initializeRandom(); + s_t noise_scale = 0.01; + #endif + + world->tuneMass( + world->getBodyNodeByIndex(1), + WrtMassBodyNodeEntryType::INERTIA_COM, + Eigen::VectorXs::Ones(3) * 0.2, + Eigen::VectorXs::Ones(3) * -0.2); + + std::shared_ptr lossFn = getSSIDPosLoss(); + //std::shared_ptr lossFn = getSSIDVelLoss(); + //std::shared_ptr lossFn = getSSIDVelPosLoss(); + + // 300 timesteps + s_t scale = 1.0; + int millisPerTimestep = world->getTimeStep() * 1000; + int steps = 10; + int inferenceHistoryMillis = steps * millisPerTimestep; + // int advanceSteps = 70; + Eigen::VectorXs sensorDims = Eigen::VectorXs::Zero(2); + sensorDims(0) = world->getNumDofs(); + sensorDims(1) = world->getNumDofs(); + SSID ssid = SSID(world, + lossFn, + inferenceHistoryMillis, + sensorDims, + steps, + scale); + + Eigen::VectorXi ssid_index = Eigen::VectorXi::Ones(1); + ssid.setSSIDCOMIndex(ssid_index); + ssid.setInitialPosEstimator( + [](Eigen::MatrixXs sensors, long /* timestamp */) { + return sensors.col(0); + }); + + ssid.setInitialVelEstimator( + [](Eigen::MatrixXs sensors, long) + { + return sensors.col(0); + } + ); + Eigen::VectorXs real_com = world->getLinkCOMIndex(1); + Eigen::VectorXs init_com = Eigen::Vector3s(0.1, 0.1,real_com(2)); + + for (int i = 0; i < 100; i++) + { + long time = i * millisPerTimestep; + Eigen::VectorXs forces = Eigen::VectorXs::Ones(world->getNumDofs()); + #ifdef WITH_NOISE + // Eigen::VectorXs forces_eps = rand_normal(forces.size(), 0, 0.01, random_gen); + Eigen::VectorXs forces_eps = Eigen::VectorXs::Zero(forces.size()); + world->setControlForces(forces + forces_eps); + #else + world->setControlForces(forces); + #endif + + #ifndef WITH_NOISE + recordObs(time, &ssid, world); + #else + recordObsWithNoise(time, &ssid, world, noise_scale, random_gen); + #endif + world->step(); + + if(i%10==0 && i >= steps+10) + { + world->setLinkCOMIndex(init_com, 1); + ssid.runInference(time); + init_com = world->getLinkCOMIndex(1); + EXPECT_TRUE((init_com - real_com).norm() <= 1e-3); + world->setLinkCOMIndex(real_com, 1); + } + } +} +#endif + +#ifdef MOI_SSID +TEST(REALTIME, CARTPOLE_MOI_SSID) +{ + //////////////////////////////////////////////////////////// + // Create a cartpole example + //////////////////////////////////////////////////////////// + + // World + WorldPtr world = createWorld(1.0/100); + #ifdef WITH_NOISE + std::mt19937 random_gen = initializeRandom(); + s_t noise_scale = 0.01; + #endif + + world->tuneMass( + world->getBodyNodeByIndex(1), + WrtMassBodyNodeEntryType::INERTIA_DIAGONAL_NOMASS, + Eigen::VectorXs::Ones(3) * 0.1, + Eigen::VectorXs::Zero(3)); + + std::shared_ptr lossFn = getSSIDPosLoss(); + //std::shared_ptr lossFn = getSSIDVelLoss(); + //std::shared_ptr lossFn = getSSIDVelPosLoss(); + + // 300 timesteps + s_t scale = 1.0; + int millisPerTimestep = world->getTimeStep() * 1000; + int steps = 20; + int inferenceHistoryMillis = steps * millisPerTimestep; + // int advanceSteps = 70; + Eigen::VectorXs sensorDims = Eigen::VectorXs::Zero(2); + sensorDims(0) = world->getNumDofs(); + sensorDims(1) = world->getNumDofs(); + SSID ssid = SSID(world, + lossFn, + inferenceHistoryMillis, + sensorDims, + steps, + scale); + + Eigen::VectorXi ssid_index = Eigen::VectorXi::Ones(1); + ssid.setSSIDMOIIndex(ssid_index); + ssid.setInitialPosEstimator( + [](Eigen::MatrixXs sensors, long /* timestamp */) { + return sensors.col(0); + }); + + ssid.setInitialVelEstimator( + [](Eigen::MatrixXs sensors, long) + { + return sensors.col(0); + } + ); + Eigen::VectorXs real_moi = Eigen::Vector3s(0.02, 0.02, 0.05); + world->setLinkDiagIIndex(real_moi, 1); + Eigen::VectorXs init_moi = Eigen::Vector3s(0.02, 0.02, 0.02); + + for (int i = 0; i < 100; i++) + { + long time = i * millisPerTimestep; + Eigen::VectorXs forces = Eigen::VectorXs::Ones(world->getNumDofs()); + #ifdef WITH_NOISE + // Eigen::VectorXs forces_eps = rand_normal(forces.size(), 0, 0.01, random_gen); + Eigen::VectorXs forces_eps = Eigen::VectorXs::Zero(forces.size()); + world->setControlForces(forces + forces_eps); + #else + world->setControlForces(forces); + #endif + + #ifndef WITH_NOISE + recordObs(time, &ssid, world); + #else + recordObsWithNoise(time, &ssid, world, noise_scale, random_gen); + #endif + world->step(); + + if(i%10==0 && i >= steps+10) + { + world->setLinkDiagIIndex(init_moi, 1); + ssid.runInference(time); + init_moi = world->getLinkDiagIIndex(1); + EXPECT_TRUE(abs(init_moi(2) - real_moi(2)) <= 1e-3); + world->setLinkDiagIIndex(real_moi, 1); + } + } +} +#endif + +#ifdef DAMP_SSID +TEST(REALTIME, CARTPOLE_DAMPING_SSID) +{ + //////////////////////////////////////////////////////////// + // Create a cartpole example + //////////////////////////////////////////////////////////// + + // World + WorldPtr world = createWorld(1.0/100); + #ifdef WITH_NOISE + std::mt19937 random_gen = initializeRandom(); + s_t noise_scale = 0.01; + #endif + Eigen::VectorXi dofs_index = Eigen::VectorXi::Zero(1); + dofs_index(0) = 1; + + world->tuneDamping( + world->getJointIndex(1), + WrtDampingJointEntryType::DAMPING, + dofs_index, + Eigen::VectorXs::Ones(1) * 2.0, + Eigen::VectorXs::Ones(1) * 0.01); + + std::shared_ptr lossFn = getSSIDPosLoss(); + //std::shared_ptr lossFn = getSSIDVelLoss(); + //std::shared_ptr lossFn = getSSIDVelPosLoss(); + + // 300 timesteps + s_t scale = 1.0; + int millisPerTimestep = world->getTimeStep() * 1000; + int steps = 10; + int inferenceHistoryMillis = steps * millisPerTimestep; + // int advanceSteps = 70; + Eigen::VectorXs sensorDims = Eigen::VectorXs::Zero(2); + sensorDims(0) = world->getNumDofs(); + sensorDims(1) = world->getNumDofs(); + SSID ssid = SSID(world, + lossFn, + inferenceHistoryMillis, + sensorDims, + steps, + scale); + ssid.setSSIDDampIndex(dofs_index); + ssid.setInitialPosEstimator( + [](Eigen::MatrixXs sensors, long /* timestamp */) { + return sensors.col(0); + }); + + ssid.setInitialVelEstimator( + [](Eigen::MatrixXs sensors, long) + { + return sensors.col(0); + } + ); + Eigen::Vector1s realdamp(0.3); + world->setJointDampingCoeffIndex(realdamp, 1); + Eigen::Vector1s init_damp(0.1); + for (int i = 0; i < 100; i++) + { + long time = i * millisPerTimestep; + Eigen::VectorXs forces = Eigen::VectorXs::Ones(world->getNumDofs()); + #ifdef WITH_NOISE + // Eigen::VectorXs forces_eps = rand_normal(forces.size(), 0, 0.01, random_gen); + Eigen::VectorXs forces_eps = Eigen::VectorXs::Zero(forces.size()); + world->setControlForces(forces + forces_eps); + #else + world->setControlForces(forces); + #endif + + #ifndef WITH_NOISE + recordObs(time, &ssid, world); + #else + recordObsWithNoise(time, &ssid, world, noise_scale, random_gen); + #endif + world->step(); + + if(i%5==0 && i >= steps+10) + { + world->setJointDampingCoeffIndex(init_damp, 1); + ssid.runInference(time); + init_damp = world->getJointDampingCoeffIndex(1); + EXPECT_TRUE(abs(init_damp(0) - realdamp(0)) <= 1e-3); + world->setJointDampingCoeffIndex(realdamp, 1); + } + } +} +#endif + +#ifdef SPRING_SSID +TEST(REALTIME, CARTPOLE_SPRING_SSID) +{ + //////////////////////////////////////////////////////////// + // Create a cartpole example + //////////////////////////////////////////////////////////// + + // World + WorldPtr world = createWorld(1.0/100); + #ifdef WITH_NOISE + std::mt19937 random_gen = initializeRandom(); + s_t noise_scale = 0.01; + #endif + + std::cout << "A_k Matrix of cartpole\n" + << world->getSkeleton(0)->getLinkAkMatrixIndex(1) + << std::endl; + Eigen::VectorXi dofs_index = Eigen::VectorXi::Zero(1); + dofs_index(0) = 1; + + world->tuneSpring( + world->getJointIndex(1), + WrtSpringJointEntryType::SPRING, + dofs_index, + Eigen::VectorXs::Ones(1) * 2.0, + Eigen::VectorXs::Ones(1) * 0.01); + + std::shared_ptr lossFn = getSSIDPosLoss(); + //std::shared_ptr lossFn = getSSIDVelLoss(); + //std::shared_ptr lossFn = getSSIDVelPosLoss(); + + // 300 timesteps + s_t scale = 1.0; + int millisPerTimestep = world->getTimeStep() * 1000; + int steps = 10; + int inferenceHistoryMillis = steps * millisPerTimestep; + // int advanceSteps = 70; + Eigen::VectorXs sensorDims = Eigen::VectorXs::Zero(2); + sensorDims(0) = world->getNumDofs(); + sensorDims(1) = world->getNumDofs(); + SSID ssid = SSID(world, + lossFn, + inferenceHistoryMillis, + sensorDims, + steps, + scale); + ssid.setSSIDSpringIndex(dofs_index); + ssid.setInitialPosEstimator( + [](Eigen::MatrixXs sensors, long /* timestamp */) { + return sensors.col(0); + }); + + ssid.setInitialVelEstimator( + [](Eigen::MatrixXs sensors, long) + { + return sensors.col(0); + } + ); + Eigen::Vector1s realspring(0.3); + world->setJointSpringStiffIndex(realspring, 1); + Eigen::Vector1s init_spring(0.1); + for (int i = 0; i < 100; i++) + { + long time = i * millisPerTimestep; + Eigen::VectorXs forces = Eigen::VectorXs::Ones(world->getNumDofs()); + #ifdef WITH_NOISE + // Eigen::VectorXs forces_eps = rand_normal(forces.size(), 0, 0.01, random_gen); + Eigen::VectorXs forces_eps = Eigen::VectorXs::Zero(forces.size()); + world->setControlForces(forces + forces_eps); + #else + world->setControlForces(forces); + #endif + + #ifndef WITH_NOISE + recordObs(time, &ssid, world); + #else + recordObsWithNoise(time, &ssid, world, noise_scale, random_gen); + #endif + world->step(); + + if(i%5==0 && i >= steps+10) + { + world->setJointSpringStiffIndex(init_spring, 1); + ssid.runInference(time); + init_spring = world->getJointSpringStiffIndex(1); + EXPECT_TRUE(abs(init_spring(0)-realspring(0)) <= 1e-3); + world->setJointSpringStiffIndex(realspring, 1); + } + } +} +#endif \ No newline at end of file diff --git a/unittests/comprehensive/test_iLQR.cpp b/unittests/comprehensive/test_iLQR.cpp new file mode 100644 index 000000000..7a94ef043 --- /dev/null +++ b/unittests/comprehensive/test_iLQR.cpp @@ -0,0 +1,190 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include + + +#include + +#include "dart/neural/RestorableSnapshot.hpp" +#include "dart/realtime/MPC.hpp" +#include "dart/realtime/iLQRLocal.hpp" +#include "dart/realtime/SSID.hpp" +#include "dart/realtime/Ticker.hpp" +#include "dart/simulation/World.hpp" +#include "dart/trajectory/IPOptOptimizer.hpp" +#include "dart/trajectory/LossFn.hpp" +#include "dart/trajectory/MultiShot.hpp" +#include "dart/trajectory/TrajectoryRollout.hpp" +#include "dart/utils/UniversalLoader.hpp" + +#include "TestHelpers.hpp" +#include "stdio.h" + +#define iLQR_TEST + +using namespace dart; +using namespace math; +using namespace dynamics; +using namespace simulation; +using namespace neural; +using namespace realtime; +using namespace trajectory; + +WorldPtr createWorld(s_t timestep) +{ + WorldPtr world = dart::utils::UniversalLoader::loadWorld("dart://sample/skel/cartpole.skel"); + world->setState(Eigen::Vector4s(0, 170.0/180 * 3.14, 0, 0)); + world->removeDofFromActionSpace(1); + world->setTimeStep(timestep); + return world; +} + +#ifdef iLQR_TEST +TEST(REALTIME, CARTPOLE_ILQR) +{ + // Create the world + WorldPtr world = createWorld(1.0/100); + + // Create iLQR instance + int steps = 400; + int millisPerTimestep = world->getTimeStep() * 1000; + int planningHorizonMillis = steps * millisPerTimestep; + + // Create Goal + Eigen::VectorXs runningStateWeight = Eigen::VectorXs::Zero(2 * 2); + Eigen::VectorXs runningActionWeight = Eigen::VectorXs::Zero(1); + Eigen::VectorXs finalStateWeight = Eigen::VectorXs::Zero(2 * 2); + finalStateWeight(0) = 10.0; + finalStateWeight(1) = 50.0; + finalStateWeight(2) = 10.0; + finalStateWeight(3) = 10.0; + runningActionWeight(0) = 0.01; + + std::shared_ptr costFn + = std::make_shared(runningStateWeight, + runningActionWeight, + finalStateWeight, + world); + costFn->setTimeStep(world->getTimeStep()); + Eigen::VectorXs goal = Eigen::VectorXs::Zero(4); + goal(0) = 0.5; + costFn->setTarget(goal); + std::cout << "Before MPC Local Initialization" << std::endl; + std::cout << "Planning Millis: " << planningHorizonMillis << std::endl; + iLQRLocal ilqr = iLQRLocal( + world, 1, planningHorizonMillis, 1.0); + ilqr.setCostFn(costFn); + + Eigen::VectorXs init_state = world->getState(); + + std::cout << "mpcLocal Created Successfully" << std::endl; + int maxIter = 10; + ilqr.setSilent(true); + ilqr.setMaxIterations(maxIter); + ilqr.setAlpha(0.5); + ilqr.setPatience(1); + ilqr.setActionBound(100.0); + + // Initialize a fresh rollout for loss computation + TrajectoryRolloutReal rollout = ilqr.createRollout(steps, world->getNumDofs(), + world->getMassDims(), + world->getDampingDims(), + world->getSpringDims()); + + // Define a lambda function for simulate traj + auto simulate_traj = [&](std::vector X, std::vector U, bool render) + { + world->setState(init_state); + if(render) + { + for(int i = 0; i < steps-1; i++) + { + rollout.getPoses().col(i) = world->getPositions(); + rollout.getVels().col(i) = world->getVelocities(); + rollout.getControlForces().col(i) = world->mapToForceSpaceVector(U[i]); + world->setAction(U[i]); + world->step(); + X[i+1] = world->getState(); + usleep(10000); + } + rollout.getPoses().col(steps-1) = world->getPositions(); + rollout.getVels().col(steps-1) = world->getVelocities(); + s_t loss = costFn->computeLoss(&rollout); + return loss; + } + else + { + for(int i = 0; i < steps-1; i++) + { + rollout.getPoses().col(i) = world->getPositions(); + rollout.getVels().col(i) = world->getVelocities(); + rollout.getControlForces().col(i) = world->mapToForceSpaceVector(U[i]); + world->setAction(U[i]); + world->step(); + X[i+1] = world->getState(); + } + rollout.getPoses().col(steps-1) = world->getPositions(); + rollout.getVels().col(steps-1) = world->getVelocities(); + s_t loss = costFn->computeLoss(&rollout); + return loss; + } + }; + // Instead of starting a single thread, iLQR Trajectory optimization from starting state + s_t init_cost = simulate_traj(ilqr.getStatesFromiLQRBuffer(), ilqr.getActionsFromiLQRBuffer(), false); + std::cout << "Initial Cost: " << init_cost << std::endl; + ilqr.setCurrentCost(init_cost); + s_t prev_cost = 1e10; + s_t threshold = 0.01; + + // Print out current parameters settings + std::cout << "Alpha: " << ilqr.getAlpha() << "\n" + << "MU: " << ilqr.getMU() << std::endl; + + int iter = 0; + while(iter < maxIter) + { + // Set the world to initial state + world->setState(init_state); + + bool forwardFlag = ilqr.ilqrForward(world); + bool backwardFlag = false; + if(!forwardFlag) + { + std::cout << "Optimization Terminated, Exiting ..." < +#include +#include +#include +#include +#include +#include +#include +#include + + +#include + +#include "dart/neural/RestorableSnapshot.hpp" +#include "dart/realtime/MPC.hpp" +#include "dart/realtime/iLQRLocal.hpp" +#include "dart/realtime/SSID.hpp" +#include "dart/realtime/Ticker.hpp" +#include "dart/server/GUIWebsocketServer.hpp" +#include "dart/simulation/World.hpp" +#include "dart/trajectory/IPOptOptimizer.hpp" +#include "dart/trajectory/LossFn.hpp" +#include "dart/trajectory/MultiShot.hpp" +#include "dart/trajectory/TrajectoryRollout.hpp" +#include "dart/utils/UniversalLoader.hpp" +#include "dart/utils/sdf/sdf.hpp" +#include "dart/utils/urdf/urdf.hpp" + +#include "TestHelpers.hpp" +#include "stdio.h" + +//#define iLQR_MPC_TEST + +using namespace dart; +using namespace math; +using namespace dynamics; +using namespace simulation; +using namespace neural; +using namespace realtime; +using namespace trajectory; +using namespace server; + +#ifdef iLQR_MPC_TEST +TEST(REALTIME, CARTPOLE_MPC) +{ + std::shared_ptr world = dart::utils::UniversalLoader::loadWorld( + "dart://sample/skel/half_cheetah.skel"); + world->setPositions(Eigen::VectorXs::Zero(world->getNumDofs())); + world->setVelocities(Eigen::VectorXs::Zero(world->getNumDofs())); + + + Eigen::VectorXs forceLimits + = Eigen::VectorXs::Ones(world->getNumDofs()) * 100; + forceLimits(0) = 0; + forceLimits(1) = 0; + forceLimits(2) = 0; + world->setControlForceUpperLimits(forceLimits); + world->setControlForceLowerLimits(-1 * forceLimits); + + world->setTimeStep(2.0 / 1000); + + int steps = 200; + int millisPerTimestep = world->getTimeStep() * 1000; + int planningHorizonMillis = steps * millisPerTimestep; + + world->removeDofFromActionSpace(0); + world->removeDofFromActionSpace(1); + world->removeDofFromActionSpace(2); + // Create Goal + Eigen::VectorXs runningStateWeight = Eigen::VectorXs::Ones(2 * world->getNumDofs())*5; + Eigen::VectorXs runningActionWeight = Eigen::VectorXs::Ones(world->getNumDofs()-3)*0.1; + Eigen::VectorXs finalStateWeight = Eigen::VectorXs::Ones(2 * world->getNumDofs())*1; + finalStateWeight(0) = 20.0; + finalStateWeight(1) = 20.0; + // Speed must be zero + finalStateWeight(world->getNumDofs()) = 20.0; + finalStateWeight(world->getNumDofs() + 1) = 20.0; + // Running State need to have some weight + runningStateWeight(0) = 10; + runningStateWeight(1) = 10; + + std::shared_ptr costFn + = std::make_shared(runningStateWeight, + runningActionWeight, + finalStateWeight, + world); + costFn->setTimeStep(world->getTimeStep()); + Eigen::VectorXs goal = world->getState(); + goal(0) += 2.5; + goal(1) = 0.5; + goal(2) += 2.5; + costFn->setTarget(goal); + std::cout << "Before MPC Local Initialization" << std::endl; + iLQRLocal mpcLocal = iLQRLocal( + world, costFn, world->getNumDofs()-3, planningHorizonMillis, 1.0); + + std::cout << "mpcLocal Created Successfully" << std::endl; + + mpcLocal.setSilent(true); + mpcLocal.setMaxIterations(3); + mpcLocal.setPatience(1); + mpcLocal.setEnableLineSearch(false); + mpcLocal.setEnableOptimizationGuards(true); + mpcLocal.setActionBound(20.0); + mpcLocal.setAlpha(1); + + std::shared_ptr realtimeUnderlyingWorld = world->clone(); + GUIWebsocketServer server; + + server.createSphere("goal", 0.1, + Eigen::Vector3s(goal(0),1.0,0), + Eigen::Vector3s(1.0, 0.0, 0.0)); + server.registerDragListener("goal", [&](Eigen::Vector3s dragTo){ + goal(0) = dragTo(0); + dragTo(1) = 1.0; + dragTo(2) = 0.0; + costFn->setTarget(goal); + server.setObjectPosition("goal", dragTo); + }); + std::cout << "Reach Here Before Ticker" << std::endl; + Ticker ticker = Ticker(1*realtimeUnderlyingWorld->getTimeStep()); + + long total_steps = 0; + ticker.registerTickListener([&](long now) { + // Eigen::VectorXs mpcforces = mpcLocal.getControlForce(now); + Eigen::VectorXs mpcforces = mpcLocal.computeForce(realtimeUnderlyingWorld->getState(), now); + //std::cout << "MPC Force: \n" << mpcforces + // << "\nRef forces: \n" << feedback_forces << std::endl; + // TODO: Currently the forces are almost zero need to figure out why + // std::cout <<"Force:\n" << mpcforces << std::endl; + //Eigen::VectorXs mpcforces = mpcLocal.getControlForce(now); + realtimeUnderlyingWorld->setControlForces(mpcforces); + + realtimeUnderlyingWorld->step(); + mpcLocal.recordGroundTruthState( + now, + realtimeUnderlyingWorld->getPositions(), + realtimeUnderlyingWorld->getVelocities(), + realtimeUnderlyingWorld->getMasses()); + + if(total_steps % 5 == 0) + { + server.renderWorld(realtimeUnderlyingWorld); + total_steps = 0; + } + total_steps ++; + }); + + // Should only work when trajectory opt + // We can always feed the trajectory used in forward pass here + mpcLocal.registerReplanningListener( + [&](long , + const trajectory::TrajectoryRollout* rollout, + long ) { + server.renderTrajectoryLines(world, rollout->getPosesConst()); + }); + + server.registerConnectionListener([&](){ + ticker.start(); + // mpcLocal.start(); + mpcLocal.setPredictUsingFeedback(true); + mpcLocal.ilqrstart(); + }); + server.registerShutdownListener([&]() {mpcLocal.stop(); }); + server.serve(8070); + server.blockWhileServing(); +} +#endif \ No newline at end of file diff --git a/unittests/comprehensive/test_iLQRRealtime.cpp b/unittests/comprehensive/test_iLQRRealtime.cpp new file mode 100644 index 000000000..b6ad89a4e --- /dev/null +++ b/unittests/comprehensive/test_iLQRRealtime.cpp @@ -0,0 +1,582 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include + + +#include +#include +#include + +#include "dart/neural/RestorableSnapshot.hpp" +#include "dart/realtime/MPC.hpp" +#include "dart/realtime/iLQRLocal.hpp" +#include "dart/realtime/SSID.hpp" +#include "dart/realtime/Ticker.hpp" +#include "dart/server/GUIWebsocketServer.hpp" +#include "dart/simulation/World.hpp" +#include "dart/trajectory/IPOptOptimizer.hpp" +#include "dart/trajectory/LossFn.hpp" +#include "dart/trajectory/MultiShot.hpp" +#include "dart/trajectory/TrajectoryRollout.hpp" +#include "dart/utils/UniversalLoader.hpp" + +#include "TestHelpers.hpp" +#include "stdio.h" + +#define iLQR_MPC_TEST + +using namespace dart; +using namespace math; +using namespace dynamics; +using namespace simulation; +using namespace neural; +using namespace realtime; +using namespace trajectory; +using namespace server; + +std::shared_ptr getSSIDPosLoss() +{ + TrajectoryLossFn loss = [](const TrajectoryRollout* rollout) { + Eigen::MatrixXs rawPos = rollout->getMetadata("sensors"); + Eigen::MatrixXs sensorPositions = rawPos.block(0,1,rawPos.rows(),rawPos.cols()-1); + int steps = rollout->getPosesConst().cols(); + + Eigen::MatrixXs posError = rollout->getPosesConst() - sensorPositions; + + //std::cout << "Pos In Buffer: " << std::endl << sensorPositions << std::endl; + //std::cout << "Pos In Traj : " << std::endl << rollout->getPosesConst() << std::endl; + + s_t sum = 0.0; + for (int i = 0; i < steps; i++) + { + sum += posError.col(i).squaredNorm(); + } + return sum; + }; + + TrajectoryLossFnAndGrad lossGrad = [](const TrajectoryRollout* rollout, + TrajectoryRollout* gradWrtRollout // OUT + ) { + gradWrtRollout->getPoses().setZero(); + gradWrtRollout->getVels().setZero(); + gradWrtRollout->getControlForces().setZero(); + int steps = rollout->getPosesConst().cols(); + + Eigen::MatrixXs rawPos = rollout->getMetadata("sensors"); + Eigen::MatrixXs sensorPositions = rawPos.block(0,1,rawPos.rows(),rawPos.cols()-1); + + Eigen::MatrixXs posError = rollout->getPosesConst() - sensorPositions; + + gradWrtRollout->getPoses() = 2 * posError; + s_t sum = 0.0; + for (int i = 0; i < steps; i++) + { + sum += posError.col(i).squaredNorm(); + } + return sum; + }; + + return std::make_shared(loss, lossGrad); +} + +std::shared_ptr getSSIDVelPosLoss() +{ + TrajectoryLossFn loss = [](const TrajectoryRollout* rollout) { + + Eigen::MatrixXs rawPos = rollout->getMetadata("sensors"); + Eigen::MatrixXs rawVel = rollout->getMetadata("velocities"); + Eigen::MatrixXs sensorPositions = rawPos.block(0,1,rawPos.rows(),rawPos.cols()-1); + Eigen::MatrixXs sensorVelocities = rawVel.block(0,1,rawVel.rows(),rawVel.cols()-1); + int steps = rollout->getPosesConst().cols(); + + Eigen::MatrixXs posError = rollout->getPosesConst() - sensorPositions; + Eigen::MatrixXs velError = rollout->getVelsConst() - sensorVelocities; + + // std::cout << "Pos Error: " << std::endl << posError << std::endl; + + s_t sum = 0.0; + for (int i = 0; i < steps; i++) + { + sum += posError.col(i).squaredNorm(); + sum += velError.col(i).squaredNorm(); + } + return sum; + }; + + TrajectoryLossFnAndGrad lossGrad = [](const TrajectoryRollout* rollout, + TrajectoryRollout* gradWrtRollout // OUT + ) { + gradWrtRollout->getPoses().setZero(); + gradWrtRollout->getVels().setZero(); + gradWrtRollout->getControlForces().setZero(); + int steps = rollout->getPosesConst().cols(); + + Eigen::MatrixXs rawPos = rollout->getMetadata("sensors"); + Eigen::MatrixXs rawVel = rollout->getMetadata("velocities"); + Eigen::MatrixXs sensorPositions = rawPos.block(0,1,rawPos.rows(),rawPos.cols()-1); + Eigen::MatrixXs sensorVelocities = rawVel.block(0,1,rawVel.rows(),rawVel.cols()-1); + + Eigen::MatrixXs posError = rollout->getPosesConst() - sensorPositions; + Eigen::MatrixXs velError = rollout->getVelsConst() - sensorVelocities; + + gradWrtRollout->getPoses() = 2 * posError; + gradWrtRollout->getVels() = 2 * velError; + s_t sum = 0.0; + for (int i = 0; i < steps; i++) + { + sum += posError.col(i).squaredNorm(); + sum += velError.col(i).squaredNorm(); + } + return sum; + }; + + return std::make_shared(loss, lossGrad); +} + +std::shared_ptr getSSIDVelLoss() +{ + TrajectoryLossFn loss = [](const TrajectoryRollout* rollout) { + Eigen::MatrixXs rawVel = rollout->getMetadata("velocities"); + Eigen::MatrixXs sensorVelocities = rawVel.block(0,1,rawVel.rows(),rawVel.cols()-1); + int steps = rollout->getVelsConst().cols(); + + Eigen::MatrixXs velError = rollout->getVelsConst() - sensorVelocities; + + // std::cout << "Pos Error: " << std::endl << posError << std::endl; + + s_t sum = 0.0; + for (int i = 0; i < steps; i++) + { + sum += velError.col(i).squaredNorm(); + } + return sum; + }; + + TrajectoryLossFnAndGrad lossGrad = [](const TrajectoryRollout* rollout, + TrajectoryRollout* gradWrtRollout // OUT + ) { + gradWrtRollout->getPoses().setZero(); + gradWrtRollout->getVels().setZero(); + gradWrtRollout->getControlForces().setZero(); + int steps = rollout->getVelsConst().cols(); + + Eigen::MatrixXs rawVel = rollout->getMetadata("velocities"); + Eigen::MatrixXs sensorVelocities = rawVel.block(0,1,rawVel.rows(),rawVel.cols()-1); + + Eigen::MatrixXs velError = rollout->getVelsConst() - sensorVelocities; + + gradWrtRollout->getVels() = 2 * velError; + s_t sum = 0.0; + for (int i = 0; i < steps; i++) + { + sum += velError.col(i).squaredNorm(); + } + return sum; + }; + + return std::make_shared(loss, lossGrad); +} + +WorldPtr createWorld(s_t timestep) +{ + WorldPtr world = dart::utils::UniversalLoader::loadWorld("dart://sample/skel/cartpole.skel"); + world->setState(Eigen::Vector4s(0, 175.0 / 180 * 3.14, 0, 0)); + world->removeDofFromActionSpace(1); + + world->setTimeStep(timestep); + + return world; +} + +std::mt19937 initializeRandom() +{ + std::random_device rd{}; + std::mt19937 gen{rd()}; + return gen; +} + +Eigen::VectorXs rand_normal(size_t length, s_t mean, s_t stddev, std::mt19937 random_gen) +{ + Eigen::VectorXs result = Eigen::VectorXs::Zero(length); + std::normal_distribution<> dist{mean, stddev}; + + for(int i = 0; i < length; i++) + { + result(i) += (s_t)(dist(random_gen)); + } + return result; +} + +std::vector convert2stdvec(Eigen::VectorXs vec) +{ + std::vector stdvec; + for(int i = 0; i < vec.size(); i++) + { + stdvec.push_back(vec(i)); + } + return stdvec; +} + +void recordObs(size_t now, SSID* ssid, WorldPtr realtimeWorld) +{ + ssid->registerLock(); + ssid->registerControls(now, realtimeWorld->getControlForces()); + ssid->registerSensors(now, realtimeWorld->getPositions(),0); + ssid->registerSensors(now, realtimeWorld->getVelocities(),1); + ssid->registerUnlock(); +} + +void recordObsWithNoise(size_t now, SSID* ssid, WorldPtr realtimeWorld, s_t noise_scale, std::mt19937 random_gen) +{ + ssid->registerLock(); + Eigen::VectorXs control_force = realtimeWorld->getControlForces(); + // Eigen::VectorXs force_eps = rand_normal(control_force.size(), 0, noise_scale, random_gen); + ssid->registerControls(now, control_force); + + Eigen::VectorXs position = realtimeWorld->getPositions(); + Eigen::VectorXs position_eps = rand_normal(position.size(), 0, noise_scale, random_gen); + ssid->registerSensors(now, position + position_eps, 0); + + Eigen::VectorXs velocity = realtimeWorld->getVelocities(); + Eigen::VectorXs velocity_eps = rand_normal(velocity.size(), 0, noise_scale, random_gen); + ssid->registerSensors(now, velocity + velocity_eps, 1); + ssid->registerUnlock(); +} + +Eigen::MatrixXs std2eigen(std::vector record) +{ + size_t num_record = record.size(); + Eigen::MatrixXs eigen_record = Eigen::MatrixXs::Zero(num_record, record[0].size()); + std::cout << "Size: " << eigen_record.cols() << " " << eigen_record.rows() << std::endl; + for(int i = 0; i < num_record; i++) + { + eigen_record.row(i) = record[i]; + } + return eigen_record; +} + +#ifdef iLQR_MPC_TEST +TEST(REALTIME, CARTPOLE_MPC_MASS) +{ + WorldPtr world = createWorld(1.0 / 100); + + // Initialize Hyper Parameters + int steps = 100; + int millisPerTimestep = world->getTimeStep() * 1000; + int planningHorizonMillis = steps * millisPerTimestep; + + // For add noise in measurement + std::mt19937 rand_gen = initializeRandom(); + s_t noise_scale = 1. / 180 * 3.14; + + // For SSID + s_t scale = 1.0; + size_t ssid_index = 0; + size_t ssid_index2 = 1; + int inferenceSteps = 10; + int inferenceHistoryMillis = inferenceSteps * millisPerTimestep; + + std::shared_ptr ssidWorld = world->clone(); + ssidWorld->tuneMass( + world->getBodyNodeByIndex(ssid_index), + WrtMassBodyNodeEntryType::INERTIA_MASS, + Eigen::VectorXs::Ones(1) * 5.0, + Eigen::VectorXs::Ones(1) * 0.2); + + ssidWorld->tuneMass( + world->getBodyNodeByIndex(ssid_index2), + WrtMassBodyNodeEntryType::INERTIA_MASS, + Eigen::VectorXs::Ones(1) * 5.0, + Eigen::VectorXs::Ones(1) * 0.2); + + ssidWorld->tuneDamping( + world->getJointIndex(1), + WrtDampingJointEntryType::DAMPING, + Eigen::VectorXi::Ones(1), + Eigen::VectorXs::Ones(1), + Eigen::VectorXs::Ones(1) * 0.01); + + Eigen::Vector2s sensorDims(world->getNumDofs(), world->getNumDofs()); + Eigen::Vector2i ssid_idx(0,1); + + SSID ssid = SSID(ssidWorld, + getSSIDPosLoss(), + inferenceHistoryMillis, + sensorDims, + inferenceSteps, + scale); + std::mutex lock; + std::mutex param_lock; + ssid.attachMutex(lock); + ssid.attachParamMutex(param_lock); + ssid.setSSIDMassIndex(Eigen::Vector2i(0, 1)); + ssid.setSSIDDampIndex(Eigen::VectorXi::Ones(1)); + ssid.useConfidence(); + ssid.useHeuristicWeight(); + ssid.useSmoothing(); + ssid.setTemperature(Eigen::Vector3s(0.5, 5, 2)); + // ssid.setThreshs(0.1, 0.5); // As in paper + ssid.setEWiseThreshs(Eigen::Vector3s(0.1, 0.1, 0.02), 0.5); + + + ssid.setInitialPosEstimator( + [](Eigen::MatrixXs sensors, long) + { + return sensors.col(0); + }); + + ssid.setInitialVelEstimator( + [](Eigen::MatrixXs sensors, long) + { + return sensors.col(0); + }); + + world->clearTunableMassThisInstance(); + // Create Goal + Eigen::VectorXs runningStateWeight = Eigen::VectorXs::Zero(2 * 2); + Eigen::VectorXs runningActionWeight = Eigen::VectorXs::Zero(1); + Eigen::VectorXs finalStateWeight = Eigen::VectorXs::Zero(2 * 2); + finalStateWeight(0) = 10.0; + finalStateWeight(1) = 50.0; + finalStateWeight(2) = 10.0; + finalStateWeight(3) = 10.0; + runningActionWeight(0) = 0.01; + + std::shared_ptr costFn + = std::make_shared(runningStateWeight, + runningActionWeight, + finalStateWeight, + world); + + costFn->setSSIDMassNodeIndex(ssid_idx); + // costFn->enableSSIDLoss(0.01); + costFn->setTimeStep(world->getTimeStep()); + Eigen::VectorXs goal = Eigen::VectorXs::Zero(4); + goal(0) = 1.0; + costFn->setTarget(goal); + std::cout << "Before MPC Local Initialization" << std::endl; + iLQRLocal mpcLocal = iLQRLocal( + world, 1, planningHorizonMillis, 1.0); + mpcLocal.setCostFn(costFn); + + std::cout << "mpcLocal Created Successfully" << std::endl; + + mpcLocal.setSilent(true); + mpcLocal.setMaxIterations(5); + mpcLocal.setPatience(3); + mpcLocal.setEnableLineSearch(false); + mpcLocal.setEnableOptimizationGuards(true); + mpcLocal.setActionBound(20.0); + mpcLocal.setAlpha(1); + + //mpcLocal.disableAdaptiveTime(); + + // bool init_flag = true; + + ssid.registerInferListener([&](long, + Eigen::VectorXs, + Eigen::VectorXs, + Eigen::VectorXs params, + long, + bool steadyFound){ + // mpcLocal.recordGroundTruthState(time, pos, vel, mass); //TODO: This will cause problem ... But Why + // If there are more than one type of parameters will this work? + if(steadyFound) + { + mpcLocal.setCandidateHorizon(planningHorizonMillis); + } + else + { + mpcLocal.setCandidateHorizon((size_t)(1*planningHorizonMillis)); + } + world->setLinkMassIndex(params(0), ssid_index); + world->setLinkMassIndex(params(1), ssid_index2); + // The rest of segment should be damping + world->setJointDampingCoeffIndex(params.segment(2,1), 1); + }); + + + std::shared_ptr realtimeUnderlyingWorld = world->clone(); + GUIWebsocketServer server; + + server.createSphere("goal", 0.1, + Eigen::Vector3s(goal(0), 0.7, 0), + Eigen::Vector4s(1.0, 0.0, 0.0, 1.0)); + server.registerDragListener("goal", [&](Eigen::Vector3s dragTo){ + goal(0) = dragTo(0); + dragTo(1) = 0.3; + dragTo(2) = 0.0; + costFn->setTarget(goal); + server.setObjectPosition("goal", dragTo); + }); + + std::string key = "mass"; + + Ticker ticker = Ticker(scale * realtimeUnderlyingWorld->getTimeStep()); + + auto sledBodyVisual = realtimeUnderlyingWorld->getSkeleton(0) + ->getBodyNodes()[0] + ->getShapeNodesWith()[0] + ->getVisualAspect(); + Eigen::Vector3s originalColor = sledBodyVisual->getColor(); + long total_steps = 0; + + Eigen::Vector3s params(1.0, 0.5, 0.2); + Eigen::Vector3s id_params(1.5, 1.0, 0.4); + + realtimeUnderlyingWorld->setLinkMassIndex(params(0), ssid_index); + realtimeUnderlyingWorld->setLinkMassIndex(params(1), ssid_index2); + realtimeUnderlyingWorld->setJointDampingCoeffIndex(params.segment(2,1), 1); + ssidWorld->setLinkMassIndex(id_params(0), ssid_index); + ssidWorld->setLinkMassIndex(id_params(1), ssid_index2); + ssidWorld->setJointDampingCoeffIndex(id_params.segment(2,1),1); + world->setLinkMassIndex(id_params(0), ssid_index); + world->setLinkMassIndex(id_params(1), ssid_index2); + world->setJointDampingCoeffIndex(id_params.segment(2,1),1); + + int cnt = 0; + int filecnt = 0; + bool renderIsReady = false; + bool record = false; + std::vector real_record; + std::vector id_record; + ticker.registerTickListener([&](long now) { + if(renderIsReady) + { + Eigen::VectorXs mpcforces = mpcLocal.computeForce(realtimeUnderlyingWorld->getState(), now); + Eigen::VectorXs force_eps = rand_normal(mpcforces.size(), 0, noise_scale, rand_gen); + realtimeUnderlyingWorld->setControlForces(mpcforces + force_eps); + } + if (server.getKeysDown().count("a")) + { + Eigen::VectorXs perturbedForces + = realtimeUnderlyingWorld->getControlForces(); + perturbedForces(0) = -15.0; + realtimeUnderlyingWorld->setControlForces(perturbedForces); + sledBodyVisual->setColor(Eigen::Vector3s(1, 0, 0)); + } + else if (server.getKeysDown().count("d")) + { + Eigen::VectorXs perturbedForces + = realtimeUnderlyingWorld->getControlForces(); + perturbedForces(0) = 15.0; + realtimeUnderlyingWorld->setControlForces(perturbedForces); + sledBodyVisual->setColor(Eigen::Vector3s(0, 1, 0)); + } + else + { + sledBodyVisual->setColor(originalColor); + } + if (server.getKeysDown().count(",") || cnt == 150) + { + // Increase mass + params(0) = 3.0; + params(1) = 2.5; + realtimeUnderlyingWorld->setLinkMassIndex(params(0), ssid_index); + realtimeUnderlyingWorld->setLinkMassIndex(params(1), ssid_index2); + } + else if (server.getKeysDown().count("o") || cnt == 600) + { + // Decrease mass + params(0) = 1.0; + params(2) = 0.1; + realtimeUnderlyingWorld->setLinkMassIndex(params(0), ssid_index); + realtimeUnderlyingWorld->setJointDampingCoeffIndex(params.segment(2,1), 1); + } + else if(server.getKeysDown().count("s")) + { + renderIsReady = true; + record = true; + ssid.start(); + //ssid.startSlow(); // Which should be useless + mpcLocal.setPredictUsingFeedback(false); + mpcLocal.ilqrstart(); + } + else if(server.getKeysDown().count("r")) + { + record = true; + } + else if(server.getKeysDown().count("p") || cnt == 940) + { + if(record) + { + Eigen::MatrixXs real_params = std2eigen(real_record); + Eigen::MatrixXs sysid_params = std2eigen(id_record); + std::cout << "Converted!" << std::endl; + ssid.saveCSVMatrix("/workspaces/nimblephysics/dart/realtime/saved_data/timeplots/cartpole_10_real_"+std::to_string(filecnt)+".csv", real_params); + ssid.saveCSVMatrix("/workspaces/nimblephysics/dart/realtime/saved_data/timeplots/cartpole_10_identified_"+std::to_string(filecnt)+".csv", sysid_params); + filecnt ++; + } + record = false; + } + // recordObs(now, &ssid, realtimeUnderlyingWorld); + if(renderIsReady) + { + // if((realtimeUnderlyingWorld->getState()-goal).norm()<0.2) + // { + // std::cout << "+++++++++++++++++++++++++++++++" << std::endl; + // std::cout << "Target Reached in: " << cnt << " Steps" << std::endl; + // std::cout << "+++++++++++++++++++++++++++++++" << std::endl; + // //ssid.stop(); + // //mpcLocal.stop(); + // //exit(1); + // } + recordObsWithNoise(now, &ssid, realtimeUnderlyingWorld, noise_scale, rand_gen); + realtimeUnderlyingWorld->step(); + cnt++; + } + id_params(0) = world->getLinkMassIndex(ssid_index); + id_params(1) = world->getLinkMassIndex(ssid_index2); + id_params(2) = world->getJointDampingCoeffIndex(1)(0); + if(renderIsReady) + { + mpcLocal.recordGroundTruthState( + now, + realtimeUnderlyingWorld->getPositions(), + realtimeUnderlyingWorld->getVelocities(), + realtimeUnderlyingWorld->getMasses()); // TODO: Why only mass + } + + if(total_steps % 5 == 0) + { + server.renderWorld(realtimeUnderlyingWorld); + server.createText(key, + "Cur Params: "+std::to_string(id_params(0))+" "+std::to_string(id_params(1))+" "+std::to_string(id_params(2)) + +"Real Params: "+std::to_string(params(0))+" "+std::to_string(params(1))+" "+std::to_string(params(2)), + Eigen::Vector2i(100,100), + Eigen::Vector2i(200,200)); + if(record && renderIsReady) + { + id_record.push_back(id_params); + real_record.push_back(params); + } + total_steps = 0; + } + total_steps ++; + + + }); + + // Should only work when trajectory opt + // We can always feed the trajectory used in forward pass here + mpcLocal.registerReplanningListener( + [&](long , + const trajectory::TrajectoryRollout* rollout, + long ) { + server.renderTrajectoryLines(world, rollout->getPosesConst()); + }); + + server.registerConnectionListener([&](){ + ticker.start(); + }); + server.registerShutdownListener([&]() {mpcLocal.stop(); }); + server.serve(8070); + server.blockWhileServing(); +} +#endif \ No newline at end of file diff --git a/unittests/comprehensive/test_whip2D.cpp b/unittests/comprehensive/test_whip2D.cpp new file mode 100644 index 000000000..2e7fcb51d --- /dev/null +++ b/unittests/comprehensive/test_whip2D.cpp @@ -0,0 +1,540 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include + + +#include +#include +#include + +#include "dart/neural/RestorableSnapshot.hpp" +#include "dart/realtime/MPC.hpp" +#include "dart/realtime/iLQRLocal.hpp" +#include "dart/realtime/SSID.hpp" +#include "dart/realtime/Ticker.hpp" +#include "dart/server/GUIWebsocketServer.hpp" +#include "dart/simulation/World.hpp" +#include "dart/dynamics/ShapeNode.hpp" +#include "dart/trajectory/IPOptOptimizer.hpp" +#include "dart/trajectory/LossFn.hpp" +#include "dart/trajectory/MultiShot.hpp" +#include "dart/trajectory/TrajectoryRollout.hpp" +#include "dart/utils/UniversalLoader.hpp" +#include "dart/utils/urdf/urdf.hpp" + +#include "TestHelpers.hpp" +#include "stdio.h" + +#define iLQR_MPC_TEST + +using namespace dart; +using namespace math; +using namespace dynamics; +using namespace simulation; +using namespace neural; +using namespace realtime; +using namespace trajectory; +using namespace server; + +std::shared_ptr getSSIDPosLoss() +{ + TrajectoryLossFn loss = [](const TrajectoryRollout* rollout) { + Eigen::MatrixXs rawPos = rollout->getMetadata("sensors"); + Eigen::MatrixXs sensorPositions = rawPos.block(0,1,rawPos.rows(),rawPos.cols()-1); + int steps = rollout->getPosesConst().cols(); + + Eigen::MatrixXs posError = rollout->getPosesConst() - sensorPositions; + + //std::cout << "Pos In Buffer: " << std::endl << sensorPositions << std::endl; + //std::cout << "Pos In Traj : " << std::endl << rollout->getPosesConst() << std::endl; + + s_t sum = 0.0; + for (int i = 0; i < steps; i++) + { + sum += posError.col(i).squaredNorm(); + } + return sum; + }; + + TrajectoryLossFnAndGrad lossGrad = [](const TrajectoryRollout* rollout, + TrajectoryRollout* gradWrtRollout // OUT + ) { + gradWrtRollout->getPoses().setZero(); + gradWrtRollout->getVels().setZero(); + gradWrtRollout->getControlForces().setZero(); + int steps = rollout->getPosesConst().cols(); + + Eigen::MatrixXs rawPos = rollout->getMetadata("sensors"); + Eigen::MatrixXs sensorPositions = rawPos.block(0,1,rawPos.rows(),rawPos.cols()-1); + + Eigen::MatrixXs posError = rollout->getPosesConst() - sensorPositions; + + gradWrtRollout->getPoses() = 2 * posError; + s_t sum = 0.0; + for (int i = 0; i < steps; i++) + { + sum += posError.col(i).squaredNorm(); + } + return sum; + }; + + return std::make_shared(loss, lossGrad); +} + +std::shared_ptr getSSIDVelPosLoss() +{ + TrajectoryLossFn loss = [](const TrajectoryRollout* rollout) { + + Eigen::MatrixXs rawPos = rollout->getMetadata("sensors"); + Eigen::MatrixXs rawVel = rollout->getMetadata("velocities"); + Eigen::MatrixXs sensorPositions = rawPos.block(0,1,rawPos.rows(),rawPos.cols()-1); + Eigen::MatrixXs sensorVelocities = rawVel.block(0,1,rawVel.rows(),rawVel.cols()-1); + int steps = rollout->getPosesConst().cols(); + + Eigen::MatrixXs posError = rollout->getPosesConst() - sensorPositions; + Eigen::MatrixXs velError = rollout->getVelsConst() - sensorVelocities; + + // std::cout << "Pos Error: " << std::endl << posError << std::endl; + + s_t sum = 0.0; + for (int i = 0; i < steps; i++) + { + sum += posError.col(i).squaredNorm(); + sum += velError.col(i).squaredNorm(); + } + return sum; + }; + + TrajectoryLossFnAndGrad lossGrad = [](const TrajectoryRollout* rollout, + TrajectoryRollout* gradWrtRollout // OUT + ) { + gradWrtRollout->getPoses().setZero(); + gradWrtRollout->getVels().setZero(); + gradWrtRollout->getControlForces().setZero(); + int steps = rollout->getPosesConst().cols(); + + Eigen::MatrixXs rawPos = rollout->getMetadata("sensors"); + Eigen::MatrixXs rawVel = rollout->getMetadata("velocities"); + Eigen::MatrixXs sensorPositions = rawPos.block(0,1,rawPos.rows(),rawPos.cols()-1); + Eigen::MatrixXs sensorVelocities = rawVel.block(0,1,rawVel.rows(),rawVel.cols()-1); + + Eigen::MatrixXs posError = rollout->getPosesConst() - sensorPositions; + Eigen::MatrixXs velError = rollout->getVelsConst() - sensorVelocities; + + gradWrtRollout->getPoses() = 2 * posError; + gradWrtRollout->getVels() = 2 * velError; + s_t sum = 0.0; + for (int i = 0; i < steps; i++) + { + sum += posError.col(i).squaredNorm(); + sum += velError.col(i).squaredNorm(); + } + return sum; + }; + + return std::make_shared(loss, lossGrad); +} + +std::shared_ptr getSSIDVelLoss() +{ + TrajectoryLossFn loss = [](const TrajectoryRollout* rollout) { + Eigen::MatrixXs rawVel = rollout->getMetadata("velocities"); + Eigen::MatrixXs sensorVelocities = rawVel.block(0,1,rawVel.rows(),rawVel.cols()-1); + int steps = rollout->getVelsConst().cols(); + + Eigen::MatrixXs velError = rollout->getVelsConst() - sensorVelocities; + + // std::cout << "Pos Error: " << std::endl << posError << std::endl; + + s_t sum = 0.0; + for (int i = 0; i < steps; i++) + { + sum += velError.col(i).squaredNorm(); + } + return sum; + }; + + TrajectoryLossFnAndGrad lossGrad = [](const TrajectoryRollout* rollout, + TrajectoryRollout* gradWrtRollout // OUT + ) { + gradWrtRollout->getPoses().setZero(); + gradWrtRollout->getVels().setZero(); + gradWrtRollout->getControlForces().setZero(); + int steps = rollout->getVelsConst().cols(); + + Eigen::MatrixXs rawVel = rollout->getMetadata("velocities"); + Eigen::MatrixXs sensorVelocities = rawVel.block(0,1,rawVel.rows(),rawVel.cols()-1); + + Eigen::MatrixXs velError = rollout->getVelsConst() - sensorVelocities; + + gradWrtRollout->getVels() = 2 * velError; + s_t sum = 0.0; + for (int i = 0; i < steps; i++) + { + sum += velError.col(i).squaredNorm(); + } + return sum; + }; + + return std::make_shared(loss, lossGrad); +} + + +WorldPtr createWorld(s_t timestep) +{ + std::shared_ptr world = dart::utils::UniversalLoader::loadWorld( + "dart://sample/skel/half_cheetah.skel"); + world->setTimeStep(timestep); + + SkeletonPtr skel = world->getSkeleton(0); + for(int i = 0; i < skel->getNumBodyNodes(); i++) + { + // xmate3p->getBodyNode(i)->removeAllShapeNodes(); + for( dart::dynamics::ShapeNode* shapenode :skel->getBodyNode(i)->getShapeNodes()) + { + // Collision handling may crash current iLQR + shapenode->removeCollisionAspect(); + } + + } + return world; +} + +void clearShapeNodes(WorldPtr world) +{ + SkeletonPtr skel = world->getSkeleton(0); + for(int i = 0; i < skel->getNumBodyNodes(); i++) + { + skel->getBodyNode(i)->removeAllShapeNodes(); + } +} + +WorldPtr cloneWorld(WorldPtr world, bool keep_shapeNode) +{ + WorldPtr cloneWorld = world->clone(); + // Default the robot skeleton is the first one + SkeletonPtr skel = cloneWorld->getSkeleton(0); + if(!keep_shapeNode) + { + clearShapeNodes(cloneWorld); + } + return cloneWorld; +} + +std::mt19937 initializeRandom() +{ + std::random_device rd{}; + std::mt19937 gen{rd()}; + return gen; +} + +Eigen::VectorXs rand_normal(size_t length, s_t mean, s_t stddev, std::mt19937 random_gen) +{ + Eigen::VectorXs result = Eigen::VectorXs::Zero(length); + std::normal_distribution<> dist{mean, stddev}; + + for(int i = 0; i < length; i++) + { + result(i) += (s_t)(dist(random_gen)); + } + return result; +} + +void recordObs(size_t now, SSID* ssid, WorldPtr realtimeWorld) +{ + ssid->registerLock(); + ssid->registerControls(now, realtimeWorld->getControlForces()); + ssid->registerSensors(now, realtimeWorld->getPositions(),0); + ssid->registerSensors(now, realtimeWorld->getVelocities(),1); + ssid->registerUnlock(); +} + +void recordObsWithNoise(size_t now, SSID* ssid, WorldPtr realtimeWorld, s_t noise_scale, std::mt19937 random_gen) +{ + ssid->registerLock(); + Eigen::VectorXs control_force = realtimeWorld->getControlForces(); + // Eigen::VectorXs force_eps = rand_normal(control_force.size(), 0, noise_scale, random_gen); + ssid->registerControls(now, control_force); + + Eigen::VectorXs position = realtimeWorld->getPositions(); + Eigen::VectorXs position_eps = rand_normal(position.size(), 0, noise_scale, random_gen); + ssid->registerSensors(now, position + position_eps, 0); + + Eigen::VectorXs velocity = realtimeWorld->getVelocities(); + Eigen::VectorXs velocity_eps = rand_normal(velocity.size(), 0, noise_scale, random_gen); + ssid->registerSensors(now, velocity + velocity_eps, 1); + ssid->registerUnlock(); +} + +#ifdef iLQR_MPC_TEST +TEST(REALTIME, CARTPOLE_MPC_MASS) +{ + WorldPtr world = createWorld(2.0 / 1000); + + // Initialize Hyper Parameters + // TODO: Need to find out + int steps = 100; + int millisPerTimestep = world->getTimeStep() * 1000; + int planningHorizonMillis = steps * millisPerTimestep; + + // For add noise in measurement + std::mt19937 rand_gen = initializeRandom(); + s_t noise_scale = 0.01; + + // For SSID + s_t scale = 1.0; + size_t ssid_index = 5; + size_t ssid_index2 = 6; + int inferenceSteps = 10; + int inferenceHistoryMillis = inferenceSteps * millisPerTimestep; + + std::shared_ptr ssidWorld = cloneWorld(world, false); + ssidWorld->tuneMass( + world->getBodyNodeByIndex(ssid_index), + WrtMassBodyNodeEntryType::INERTIA_MASS, + Eigen::VectorXs::Ones(1) * 5.0, + Eigen::VectorXs::Ones(1) * 0.2); + + ssidWorld->tuneMass( + world->getBodyNodeByIndex(ssid_index2), + WrtMassBodyNodeEntryType::INERTIA_MASS, + Eigen::VectorXs::Ones(1) * 5.0, + Eigen::VectorXs::Ones(1) * 0.2); + + Eigen::Vector2s sensorDims(world->getNumDofs(), world->getNumDofs()); + std::vector ssid_idx{ssid_index};//, ssid_index2}; + + SSID ssid = SSID(ssidWorld, + getSSIDPosLoss(), + inferenceHistoryMillis, + sensorDims, + inferenceSteps, + scale); + std::mutex lock; + std::mutex param_lock; + ssid.attachMutex(lock); + ssid.attachParamMutex(param_lock); + // ssid.setSSIDIndex(Eigen::Vector2i(ssid_index, ssid_index2)); + Eigen::VectorXi index; + index = Eigen::VectorXi::Zero(1); + index(0) = ssid_index; + ssid.setSSIDIndex(index); + + ssid.setInitialPosEstimator( + [](Eigen::MatrixXs sensors, long) + { + return sensors.col(0); + }); + + ssid.setInitialVelEstimator( + [](Eigen::MatrixXs sensors, long) + { + return sensors.col(0); + }); + + world->clearTunableMassThisInstance(); + // Create Goal + int dofs = 3; + Eigen::VectorXs runningStateWeight = Eigen::VectorXs::Zero(2 * dofs); + Eigen::VectorXs runningActionWeight = Eigen::VectorXs::Ones(dofs) * 0.1; + Eigen::VectorXs finalStateWeight = Eigen::VectorXs::Ones(2 * dofs) * 100.0; + + runningStateWeight.segment(0, dofs) = Eigen::VectorXs::Ones(dofs) * 0.01; + finalStateWeight.segment(dofs, dofs) *= 0.5; + finalStateWeight(2) *= 5; + + std::shared_ptr realtimeUnderlyingWorld = cloneWorld(world,true); + + clearShapeNodes(world); + + std::shared_ptr costFn + = std::make_shared(runningStateWeight, + runningActionWeight, + finalStateWeight, + world); + + costFn->setSSIDNodeIndex(ssid_idx); + // costFn->enableSSIDLoss(0.01); + costFn->setTimeStep(world->getTimeStep()); + Eigen::Vector6s goal; + goal << 30.0 / 180.0 * 3.1415, 30.0 / 180.0 * 3.1415, 60.0 / 180.0 * 3.1415, 0, 0, 0; + + + costFn->setTarget(goal); + std::cout << "Goal: " << goal << std::endl; + iLQRLocal mpcLocal = iLQRLocal( + world, costFn, dofs, planningHorizonMillis, 1.0); + mpcLocal.setSilent(true); + mpcLocal.setMaxIterations(5); + mpcLocal.setPatience(1); + mpcLocal.setEnableLineSearch(false); + mpcLocal.setEnableOptimizationGuards(true); + mpcLocal.setActionBound(100.0); + mpcLocal.setAlpha(1); + + ssid.registerInferListener([&](long, + Eigen::VectorXs, + Eigen::VectorXs, + Eigen::VectorXs mass, + long){ + // mpcLocal.recordGroundTruthState(time, pos, vel, mass); //TODO: This will cause problem ... But Why + mpcLocal.setParameterChange(mass); + world->setLinkMassIndex(mass(0), ssid_index); + world->setLinkMassIndex(mass(1), ssid_index2); + }); + + + + GUIWebsocketServer server; + + /* + server.createSphere("goal", 0.1, + Eigen::Vector3s(goal(0),1.0,0), + Eigen::Vector3s(1.0, 0.0, 0.0)); + server.registerDragListener("goal", [&](Eigen::Vector3s dragTo){ + goal(0) = dragTo(0); + dragTo(1) = 1.0; + dragTo(2) = 0.0; + costFn->setTarget(goal); + server.setObjectPosition("goal", dragTo); + }); + */ + + std::string key = "mass"; + + Ticker ticker = Ticker(scale * realtimeUnderlyingWorld->getTimeStep()); + long total_steps = 0; + + Eigen::Vector2s masses; + masses(0) = world->getLinkMassIndex(ssid_index); + // masses(1) = 0; + masses(1) = world->getLinkMassIndex(ssid_index2); + Eigen::Vector2s id_masses(1.0, 1.0); + + realtimeUnderlyingWorld->setLinkMassIndex(masses(0), ssid_index); + realtimeUnderlyingWorld->setLinkMassIndex(masses(1), ssid_index2); + ssidWorld->setLinkMassIndex(id_masses(0), ssid_index); + ssidWorld->setLinkMassIndex(id_masses(1), ssid_index2); + world->setLinkMassIndex(id_masses(0), ssid_index); + world->setLinkMassIndex(id_masses(1), ssid_index2); + // Preload visualization + bool renderIsReady = false; + ticker.registerTickListener([&](long now) { + // Eigen::VectorXs mpcforces = mpcLocal.getControlForce(now); + Eigen::VectorXs mpcforces; + if(renderIsReady) + { + mpcforces = mpcLocal.computeForce(realtimeUnderlyingWorld->getState(), now); + //mpcforces = Eigen::VectorXs::Ones(dofs); + } + else + { + mpcforces = Eigen::VectorXs::Zero(dofs); + } + //std::cout << "MPC Force: \n" << mpcforces + // << "\nRef forces: \n" << feedback_forces << std::endl; + // TODO: Currently the forces are almost zero need to figure out why + // std::cout <<"Force:\n" << mpcforces << std::endl; + //Eigen::VectorXs mpcforces = mpcLocal.getControlForce(now); + Eigen::VectorXs force_eps = rand_normal(mpcforces.size(), 0, noise_scale, rand_gen); + realtimeUnderlyingWorld->setControlForces(mpcforces + force_eps); + if (server.getKeysDown().count("a")) + { + Eigen::VectorXs perturbedForces + = realtimeUnderlyingWorld->getControlForces(); + perturbedForces(0) = -15.0; + // perturbedForces(6) = -15.0; + realtimeUnderlyingWorld->setControlForces(perturbedForces); + } + else if (server.getKeysDown().count("e")) + { + Eigen::VectorXs perturbedForces + = realtimeUnderlyingWorld->getControlForces(); + perturbedForces(0) = 15.0; + // perturbedForces(6) = 15.0; + realtimeUnderlyingWorld->setControlForces(perturbedForces); + } + + if (server.getKeysDown().count(",")) + { + // Increase mass + masses(0) = 3.0; + masses(1) = 2.5; + realtimeUnderlyingWorld->setLinkMassIndex(masses(0), ssid_index); + realtimeUnderlyingWorld->setLinkMassIndex(masses(1), ssid_index2); + } + else if (server.getKeysDown().count("o")) + { + // Decrease mass + masses(0) = 1.0; + masses(1) = 0.5; + realtimeUnderlyingWorld->setLinkMassIndex(masses(0), ssid_index); + realtimeUnderlyingWorld->setLinkMassIndex(masses(1), ssid_index2); + } + else if(server.getKeysDown().count("s")) + { + renderIsReady = true; + ssid.start(); + ssid.startSlow(); + // mpcLocal.setPredictUsingFeedback(true); + mpcLocal.ilqrstart(); + } + // recordObs(now, &ssid, realtimeUnderlyingWorld); + if(renderIsReady) + { + recordObsWithNoise(now, &ssid, realtimeUnderlyingWorld, noise_scale, rand_gen); + realtimeUnderlyingWorld->step(); + } + id_masses(0) = world->getLinkMassIndex(ssid_index); + id_masses(1) = world->getLinkMassIndex(ssid_index2); + if(renderIsReady) + { + mpcLocal.recordGroundTruthState( + now, + realtimeUnderlyingWorld->getPositions(), + realtimeUnderlyingWorld->getVelocities(), + realtimeUnderlyingWorld->getMasses()); + } + + if(total_steps % 5 == 0) + { + server.renderWorld(realtimeUnderlyingWorld); + server.createText(key, + "Current Masses: "+std::to_string(id_masses(0))+" "+std::to_string(id_masses(1))+" "+ + "Real Masses: "+std::to_string(masses(0))+" "+std::to_string(masses(1)), + Eigen::Vector2i(100,100), + Eigen::Vector2i(400,400)); + // server.createText(key, + // "Current Masses: "+std::to_string(id_masses(0))+" "+"Real Masses: " + std::to_string(masses(0)), + // Eigen::Vector2i(100, 100), + // Eigen::Vector2i(400, 400)); + total_steps = 0; + } + total_steps ++; + }); + + // Should only work when trajectory opt + // We can always feed the trajectory used in forward pass here + mpcLocal.registerReplanningListener( + [&](long , + const trajectory::TrajectoryRollout* rollout, + long ) { + server.renderTrajectoryLines(world, rollout->getPosesConst()); + }); + + server.registerConnectionListener([&](){ + ticker.start(); + + + }); + server.registerShutdownListener([&]() {mpcLocal.stop(); }); + server.serve(8070); + server.blockWhileServing(); +} +#endif \ No newline at end of file