diff --git a/binding/python/mc_rbdyn/mc_rbdyn.pxd b/binding/python/mc_rbdyn/mc_rbdyn.pxd index 717ef0a393..9602d014a7 100644 --- a/binding/python/mc_rbdyn/mc_rbdyn.pxd +++ b/binding/python/mc_rbdyn/mc_rbdyn.pxd @@ -89,11 +89,14 @@ cdef class CylindricalSurface(Surface): cdef CylindricalSurface CylindricalSurfaceFromPtr(c_mc_rbdyn.CylindricalSurface*) cdef class Contact(object): + cdef shared_ptr[c_mc_rbdyn.Contact] ptr cdef c_mc_rbdyn.Contact * impl cdef cppbool own_impl__ cdef Contact ContactFromC(const c_mc_rbdyn.Contact &, cppbool copy=?) +cdef Contact ContactFromPtr(shared_ptr[c_mc_rbdyn.Contact], cppbool copy=?) + cdef class ContactVector(object): cdef vector[c_mc_rbdyn.Contact] * v cdef cppbool own_impl__ diff --git a/binding/python/mc_rbdyn/mc_rbdyn.pyx b/binding/python/mc_rbdyn/mc_rbdyn.pyx index 07df7aacd8..0fece31204 100644 --- a/binding/python/mc_rbdyn/mc_rbdyn.pyx +++ b/binding/python/mc_rbdyn/mc_rbdyn.pyx @@ -1191,6 +1191,18 @@ cdef Contact ContactFromC(const c_mc_rbdyn.Contact& c, cppbool copy=True): else: ret.own_impl__ = False ret.impl = &(c_mc_rbdyn.const_cast_contact(c)) + ret.ptr = shared_ptr[c_mc_rbdyn.Contact]() + return ret + +cdef Contact ContactFromPtr(shared_ptr[c_mc_rbdyn.Contact] ptr, cppbool copy=True): + cdef Contact ret = Contact(skip_alloc = True) + if copy: + ret.impl = new c_mc_rbdyn.Contact(deref(ptr.get())) + ret.own_impl__ = True + else: + ret.ptr = ptr + ret.impl = ptr.get() + ret.own_impl__ = False return ret cdef class ContactVector(object): diff --git a/binding/python/mc_solver/c_mc_solver.pxd b/binding/python/mc_solver/c_mc_solver.pxd index 19fdb95eb4..d8f42b6036 100644 --- a/binding/python/mc_solver/c_mc_solver.pxd +++ b/binding/python/mc_solver/c_mc_solver.pxd @@ -74,7 +74,7 @@ cdef extern from "" namespace "mc_solver": cdef cppclass QPSolver: void addConstraintSet(const ConstraintSet&) void removeConstraintSet(const ConstraintSet&) - const vector[c_mc_rbdyn.Contact] & contacts() + const vector[shared_ptr[c_mc_rbdyn.Contact]] & contacts() void setContacts(const vector[c_mc_rbdyn.Contact]&) void addTask(c_qp.Task *) void addTask(c_mc_tasks.MetaTask *) diff --git a/binding/python/mc_solver/mc_solver.pyx b/binding/python/mc_solver/mc_solver.pyx index cec3402ab8..431c4756c5 100644 --- a/binding/python/mc_solver/mc_solver.pyx +++ b/binding/python/mc_solver/mc_solver.pyx @@ -174,7 +174,7 @@ cdef class QPSolver(object): def contacts(self): ret = [] for i in range(self.impl.contacts().size()): - ret.append(mc_rbdyn.ContactFromC(self.impl.contacts()[i])) + ret.append(mc_rbdyn.ContactFromPtr(self.impl.contacts()[i], copy=False)) return ret def addTask(self, mc_tasks.MetaTask task): self.impl.addTask(task.mt_base) diff --git a/include/mc_control/Contact.h b/include/mc_control/Contact.h index 6ca3cdf12b..aded2a5c61 100644 --- a/include/mc_control/Contact.h +++ b/include/mc_control/Contact.h @@ -31,8 +31,10 @@ struct MC_CONTROL_DLLAPI Contact const std::string & r1Surface, const std::string & r2Surface, double friction = mc_rbdyn::Contact::defaultFriction, - const Eigen::Vector6d & dof = Eigen::Vector6d::Ones()) - : r1(r1), r2(r2), r1Surface(r1Surface), r2Surface(r2Surface), friction(friction), dof(dof) + const Eigen::Vector6d & dof = Eigen::Vector6d::Ones(), + std::optional feasiblePolytope = std::nullopt) + : r1(r1), r2(r2), r1Surface(r1Surface), r2Surface(r2Surface), friction(friction), dof(dof), + feasiblePolytope(feasiblePolytope) { } @@ -42,6 +44,7 @@ struct MC_CONTROL_DLLAPI Contact std::string r2Surface; mutable double friction; mutable Eigen::Vector6d dof; + mutable std::optional feasiblePolytope; bool operator==(const Contact & rhs) const { diff --git a/include/mc_control/MCController.h b/include/mc_control/MCController.h index 772368c755..5c5cb31876 100644 --- a/include/mc_control/MCController.h +++ b/include/mc_control/MCController.h @@ -264,15 +264,19 @@ struct MC_CONTROL_DLLAPI MCController /** Add a contact between two robots * - * No effect if the contact is already present. + * No effect if the contact is already present and hasn't changed. * + * Appropriate constraints will be updated/recreated as needed if contact properties changed (dof, friction, + * feasibility polytopes, etc) + * + * \param c Contact to add + * \param show When true print information */ - void addContact(const Contact & c); + void addContact(const Contact & c, bool show = true); /** Remove a contact between two robots * - * No effect if the contact is already absent. - * + * No effect if the contact does not exist */ void removeContact(const Contact & c); diff --git a/include/mc_rbdyn/Contact.h b/include/mc_rbdyn/Contact.h index 08832eda39..c8ad877f3e 100644 --- a/include/mc_rbdyn/Contact.h +++ b/include/mc_rbdyn/Contact.h @@ -8,9 +8,12 @@ #include #include +#include + #include #include +#include #include @@ -43,12 +46,18 @@ MC_RBDYN_DLLAPI std::vector computePoints(const mc_rbdyn::Surf const mc_rbdyn::Surface & envSurface, const sva::PTransformd & X_es_rs); +struct FeasiblePolytope +{ + Eigen::MatrixXd planeNormals; + Eigen::VectorXd planeConstants; +}; + struct ContactImpl; struct MC_RBDYN_DLLAPI Contact { public: - constexpr static int nrConeGen = 4; + constexpr static int nrConeGen = 4; // FIXME: use it in tvmqpsolver... constexpr static double defaultFriction = 0.7; constexpr static unsigned int nrBilatPoints = 4; @@ -124,6 +133,23 @@ struct MC_RBDYN_DLLAPI Contact /** Set the contact friction */ void friction(double friction); + void feasiblePolytopeR1(const FeasiblePolytope & polytope); + const std::optional & feasiblePolytopeR1() const noexcept; + + void feasiblePolytopeR2(const FeasiblePolytope & polytope); + const std::optional & feasiblePolytopeR2() const noexcept; + + /** Get the TVM polytope associated to robot 1 of this contact + * + * FIXME Returns a non-const reference from a const method because it is most often used to register dependencies + * between TVM nodes which require non-const objects + */ + mc_tvm::FeasiblePolytope & tvmPolytopeR1() const; + + /** Get the TVM polytope associated to robot 2 of this contact + */ + mc_tvm::FeasiblePolytope & tvmPolytopeR2() const; + std::pair surfaces() const; sva::PTransformd X_0_r1s(const mc_rbdyn::Robot & robot) const; @@ -164,6 +190,20 @@ struct MC_RBDYN_DLLAPI Contact const sva::PTransformd & X_b1_b2, const std::vector & points) const; + //! If present superseeds friction cone constraints + // feasible polytope for r1 of contact + std::optional feasiblePolytopeR1_; + + // feasible polytope for r2 of contact + std::optional feasiblePolytopeR2_; + + // Secures threaded access to the contact object + mutable std::mutex contactMutex_; + + /* mutable to allow initialization in const method */ + mutable mc_tvm::PolytopePtr tvm_polytopeR1_; + mutable mc_tvm::PolytopePtr tvm_polytopeR2_; + public: static mc_rbdyn::Contact load(const mc_rbdyn::Robots & robots, const mc_rtc::Configuration & config); diff --git a/include/mc_rbdyn/Robot.h b/include/mc_rbdyn/Robot.h index f3dd837fe1..d14a28b3f9 100644 --- a/include/mc_rbdyn/Robot.h +++ b/include/mc_rbdyn/Robot.h @@ -632,7 +632,12 @@ struct MC_RBDYN_DLLAPI Robot const Eigen::Vector3d & zmpTarget() const; /** Compute and returns the mass of the robot */ - inline double mass() const noexcept { return mass_; } + inline double mass() const noexcept + { + double mass = 0.; + for(const auto & b : mb().bodies()) { mass += b.inertia().mass(); } + return mass; + } /** @name Joint sensors * diff --git a/include/mc_rbdyn/fwd.h b/include/mc_rbdyn/fwd.h index a23d4a75a7..9c990982b1 100644 --- a/include/mc_rbdyn/fwd.h +++ b/include/mc_rbdyn/fwd.h @@ -30,4 +30,7 @@ using ConstFramePtr = std::shared_ptr; struct ForceSensor; +struct Contact; +using ContactPtr = std::shared_ptr; + } // namespace mc_rbdyn diff --git a/include/mc_rtc/logging.h b/include/mc_rtc/logging.h index 26554de520..af4e41505e 100644 --- a/include/mc_rtc/logging.h +++ b/include/mc_rtc/logging.h @@ -61,6 +61,13 @@ void critical(const S & format, Args &&... args) } } +/** + * Print an error message + * + * Displayed in red on terminals with color support + * + * @tparam Args Print all args using fmt format. The first argument can be an fmt formatting string + */ template void error(const S & format, Args &&... args) { @@ -71,6 +78,22 @@ void error(const S & format, Args &&... args) } } +/** + * Print optional error message + */ +template +void error(bool print, Args &&... args) +{ + if(print) { error(std::forward(args)...); } +} + +/** + * Print a warning message + * + * Displayed in yellow on terminals with color support + * + * @tparam Args Print all args using fmt format. The first argument can be an fmt formatting string + */ template void warning(const S & format, Args &&... args) { @@ -81,6 +104,22 @@ void warning(const S & format, Args &&... args) } } +/** + * Print optional warning message + */ +template +void warning(bool print, Args &&... args) +{ + if(print) { warning(std::forward(args)...); } +} + +/** + * Print an information message + * + * Displayed in blue on terminals with color support + * + * @tparam Args Print all args using fmt format. The first argument can be an fmt formatting string + */ template void info(const S & format, Args &&... args) { @@ -91,6 +130,22 @@ void info(const S & format, Args &&... args) } } +/** + * Print optional information message + */ +template +void info(bool print, Args &&... args) +{ + if(print) { info(std::forward(args)...); } +} + +/** + * Print a success message + * + * Displayed in green on terminals with color support + * + * @tparam Args Print all args using fmt format. The first argument can be an fmt formatting string + */ template void success(const S & format, Args &&... args) { @@ -101,6 +156,20 @@ void success(const S & format, Args &&... args) } } +/** + * Print optional success message + */ +template +void success(bool print, Args &&... args) +{ + if(print) { success(std::forward(args)...); } +} + +/** + * Displays a system notification + * + * @tparam Args Print all args using fmt format. The first argument can be an fmt formatting string + */ template void notify(const S & format, Args &&... args) { @@ -111,6 +180,15 @@ void notify(const S & format, Args &&... args) } } +/** + * Displays an optional system notification (libnotify, WinToast, etc.) + */ +template +void notify(bool print, Args &&... args) +{ + if(print) { notify(std::forward(args)...); } +} + } // namespace log } // namespace mc_rtc diff --git a/include/mc_solver/QPSolver.h b/include/mc_solver/QPSolver.h index b8caa74be9..d2e0001f65 100644 --- a/include/mc_solver/QPSolver.h +++ b/include/mc_solver/QPSolver.h @@ -220,7 +220,7 @@ struct MC_SOLVER_DLLAPI QPSolver virtual void setContacts(ControllerToken, const std::vector & contacts) = 0; /** Returns the current set of contacts */ - inline const std::vector & contacts() const noexcept { return contacts_; } + inline const std::vector> & contacts() const noexcept { return contacts_; } /** Returns the current set of constraints */ inline const std::vector & constraints() const noexcept { return constraints_; } @@ -287,7 +287,7 @@ struct MC_SOLVER_DLLAPI QPSolver std::shared_ptr logger() const; /** Set the GUI helper for this solver instance */ - void gui(std::shared_ptr gui); + virtual void gui(std::shared_ptr gui); /** Access to the gui instance */ std::shared_ptr gui() const; @@ -305,7 +305,7 @@ struct MC_SOLVER_DLLAPI QPSolver double timeStep; /** Holds mc_rbdyn::Contact in the solver */ - std::vector contacts_; + std::vector> contacts_; /** Holds MetaTask currently in the solver */ std::vector metaTasks_; diff --git a/include/mc_solver/TVMQPSolver.h b/include/mc_solver/TVMQPSolver.h index a7de3e2a12..31db4b9b7e 100644 --- a/include/mc_solver/TVMQPSolver.h +++ b/include/mc_solver/TVMQPSolver.h @@ -28,9 +28,12 @@ struct MC_SOLVER_DLLAPI TVMQPSolver final : public QPSolver ~TVMQPSolver() final = default; + void gui(std::shared_ptr gui) override; + void setContacts(ControllerToken, const std::vector & contacts) final; const sva::ForceVecd desiredContactForce(const mc_rbdyn::Contact & id) const final; + const sva::ForceVecd desiredContactForce2(const mc_rbdyn::Contact & id) const; double solveTime() final; @@ -42,6 +45,9 @@ struct MC_SOLVER_DLLAPI TVMQPSolver final : public QPSolver /** Access the internal problem (const) */ inline const tvm::LinearizedControlProblem & problem() const noexcept { return problem_; } + /** Access the dynamics constraints map */ + inline const std::unordered_map & dynamics() const noexcept { return dynamics_; }; + /** Helper to get a \ref TVMQPSolver from a \ref QPSolver instance * * The caller should make sure the cast is valid by checking the QPSolver backend. @@ -66,6 +72,22 @@ struct MC_SOLVER_DLLAPI TVMQPSolver final : public QPSolver return static_cast(solver); } + /** Save the problem graph to a dot file that can be visualized with graphviz or other related tools + * + * The generated graph is located in /mc_rtc_tvm_graph_.dot + * + * To generate the graph, use (graphviz needs to be installed): + * \code + * dot -Tps /tmp/mc_rtc_tvm_graph-latest.dot -o /tmp/tvm_graph.ps + * \endcode + * + * @return true on success + **/ + bool saveGraphDotFile() const; + + /** Same as saveGraphDotFile but with a custom filename */ + bool saveGraphDotFile(const std::string & filename) const; + private: /** Control problem */ tvm::LinearizedControlProblem problem_; @@ -80,10 +102,14 @@ struct MC_SOLVER_DLLAPI TVMQPSolver final : public QPSolver tvm::VariableVector f1_; /** Constraints on f1 */ std::vector f1Constraints_; + /** Target tasks on f1 */ + std::vector f1Targets_; /** Force variables on r2 side (if any) */ tvm::VariableVector f2_; /** Constraints on f2 */ std::vector f2Constraints_; + /** Target tasks on f2 */ + std::vector f2Targets_; }; /** Related contact functions */ std::vector contactsData_; @@ -141,15 +167,46 @@ struct MC_SOLVER_DLLAPI TVMQPSolver final : public QPSolver size_t getContactIdx(const mc_rbdyn::Contact & contact); void addContact(const mc_rbdyn::Contact & contact); - using ContactIterator = std::vector::iterator; + using ContactIterator = std::vector>::iterator; ContactIterator removeContact(size_t idx); + + /** + * @brief Update or create and add an mc_tvm::ContactFunction (geometric constraint) to the problem, + * and update solver contacts_ and contactsData_ vectors + * + * hasWork becomes true if friction or polytope changed, in this case dynamics function must be updated + * + * dofs changing do not influence the dynamics so does not trigger hasWork + * + * @param contact the mc_rbdyn::Contact to add or update + * @return std::tuple of contact id / hasWork + */ std::tuple addVirtualContactImpl(const mc_rbdyn::Contact & contact); + + /** + * @brief If the robot has a dynamic constraint, add the contact's influence to it. + * + * This creates force variables for each contact point and constraints on them (either friction cone + * or feasiblePolytope) and adds them to the problem. + * + * The tvm dependency between the force variables and the DynamicFunction is done here with addContact + * + * @param robot Robot name + * @param frame Contact frame + * @param points Contact points in the frame's parent body's frame + * @param forces Ref to where the forces tvm variables created by this contact will be stored + * @param constraints Ref to where the constraints on these forces will be stored + * @param targets Ref to where the target functions for these forces will be stored + * @param contact mc_rbydn::Contact object for contact friction or feasiblePolytope + * @param dir Contact direction + */ void addContactToDynamics(const std::string & robot, const mc_rbdyn::RobotFrame & frame, const std::vector & points, tvm::VariableVector & forces, std::vector & constraints, - const Eigen::MatrixXd & frictionCone, + std::vector & targets, + mc_rbdyn::Contact & contact, double dir); }; diff --git a/include/mc_tvm/DynamicFunction.h b/include/mc_tvm/DynamicFunction.h index 2e2156a0ca..90d61d6525 100644 --- a/include/mc_tvm/DynamicFunction.h +++ b/include/mc_tvm/DynamicFunction.h @@ -38,7 +38,7 @@ struct MC_TVM_DLLAPI DynamicFunction : public tvm::function::abstract::LinearFun /** Construct the equation of motion for a given robot */ DynamicFunction(const mc_rbdyn::Robot & robot); - /** Add a contact to the function + /** Add a 3d contact to the function * * This adds forces variables for every contact point belonging to the * robot of this dynamic function. @@ -51,9 +51,35 @@ struct MC_TVM_DLLAPI DynamicFunction : public tvm::function::abstract::LinearFun * * Returns the force variables that were created by this contact */ - const tvm::VariableVector & addContact(const mc_rbdyn::RobotFrame & frame, - std::vector points, - double dir); + const tvm::VariableVector & addContact3d(const mc_rbdyn::RobotFrame & frame, + std::vector points, + double dir); + + /** Add a surface contact to the function + * + * This adds a 6d wrench variable for the surface contact. + * + * \param frame Contact frame + * + * \param contact Contact object + * + * Returns the wrench variable that was created by this contact + */ + const tvm::VariablePtr & addContact6d(const mc_rbdyn::RobotFrame & frame, mc_rbdyn::Contact & contact); + + /** Add a surface contact to the function using a pre-existing wrench variable + * + * This adds the dependency to a pre-existing force variable + * + * \param frame Contact frame + * + * \param variables Pre-existing variable to use for dependency + * + * \param contact Contact object + * + * Returns the wrench variable that was created by this contact + */ + void addContact6d(const mc_rbdyn::RobotFrame & frame, const tvm::VariablePtr & variable, mc_rbdyn::Contact & contact); /** Removes the contact associated to the given frame * @@ -69,6 +95,31 @@ struct MC_TVM_DLLAPI DynamicFunction : public tvm::function::abstract::LinearFun */ sva::ForceVecd contactForce(const mc_rbdyn::RobotFrame & f) const; + /** + * @brief Finds and returns the force/wrench variables existing for this frame in this dynamics constraint. + * This is used to check if a contact force decision variable was already created for this contact by + * the other robot dynamics constraint, to reuse it. + * + * @param contactFrameName Name of the frame to check (names are unique within a robot so this is sufficient) + * @return The variable vector of the forces/wrench variable(s) associated to this frame (empty if there are none) + */ + const tvm::VariableVector getForceVariables(const std::string & contactFrameName); + // FIXME Handle offsets between contact frames + + /** + * @brief Returns a map of the force/wrench variables taken into account in this dynamics constraint, and + * their plücker transform towards the CoM. + * This should be used by other tasks for multi-contact balancing. + * + * @return map of the X_w_C transforms. + */ + const std::map & getCoMWrenchTransforms() const noexcept + { + return CoMWrenchTransforms_; + }; + // FIXME add this as an output of the dynamic function (and dependency in the tasks) so that the graph is up to date + // when it is called + protected: void updateb(); @@ -107,9 +158,52 @@ struct MC_TVM_DLLAPI DynamicFunction : public tvm::function::abstract::LinearFun Eigen::MatrixXd force_jac_; Eigen::MatrixXd full_jac_; }; - std::vector contacts_; - std::vector::const_iterator findContact(const mc_rbdyn::RobotFrame & frame) const; + /** Holds data for the contact wrenches part of the motion equation */ + struct WrenchContact + { + /** Constructor for 6D wrench */ + WrenchContact(const mc_rbdyn::RobotFrame & frame, mc_rbdyn::Contact & contact); + + /** Alternate constructor reusing a pre existing wrench var */ + WrenchContact(const mc_rbdyn::RobotFrame & frame, const tvm::VariablePtr & wrench, mc_rbdyn::Contact & contact); + + /** Update jacobian */ + void updateWrenchJacobian(DynamicFunction & parent); + + /** Return the contact wrench */ + sva::ForceVecd wrench() const; + + /** Associated frame */ + mc_rbdyn::ConstRobotFramePtr frame_; + + /** 6D wrench var associated to a contact */ + tvm::VariablePtr wrench_; + + /** Pointer to contact for relative transform */ + mc_rbdyn::Contact * contact_; + + /** Bool to know if variable was created by this dyn function or another */ + bool hasVariable_; + + /** RBDyn jacobian */ + rbd::Jacobian jac_; + /** RBDyn jacobian blocks */ + rbd::Blocks blocks_; + + /** Used for intermediate Jacobian computation */ + Eigen::MatrixXd full_jac_; + }; + + std::vector contactForces_; + + std::vector::const_iterator findContactForce(const mc_rbdyn::RobotFrame & frame) const; + + std::vector contactWrenches_; + + std::vector::const_iterator findContactWrench(const mc_rbdyn::RobotFrame & frame) const; + + std::map CoMWrenchTransforms_; void updateJacobian(); }; diff --git a/include/mc_tvm/FeasiblePolytope.h b/include/mc_tvm/FeasiblePolytope.h new file mode 100644 index 0000000000..917922b7ba --- /dev/null +++ b/include/mc_tvm/FeasiblePolytope.h @@ -0,0 +1,167 @@ +/* + * Copyright 2015-2020 CNRS-UM LIRMM, CNRS-AIST JRL + */ + +#pragma once + +#include +#include + +#include +#include +#include + +#include + +#include + +namespace mc_tvm +{ + +/** A Feasible Polytope is a set of planes that can be used as a constraint + * for a variable. It is created from an mc_rbdyn::Contact. + * + * Outputs: + * - Normals: Matrix of the planes normals + * - Offsets: Vector of the planes offsets + */ +struct MC_TVM_DLLAPI FeasiblePolytope : public tvm::graph::abstract::Node +{ + friend struct mc_rbdyn::Contact; + +private: + struct NewPolytopeToken + { + }; + +public: + SET_OUTPUTS(FeasiblePolytope, Polytope) + SET_UPDATES(FeasiblePolytope, Polytope) + + /** + * @brief Construct a new Feasible Polytope object + * + * @param contact rbdyn contact associated + */ + FeasiblePolytope(NewPolytopeToken, const mc_rbdyn::Contact & contact, const int & rIndex); + + FeasiblePolytope(const FeasiblePolytope &) = delete; + FeasiblePolytope & operator=(const FeasiblePolytope &) = delete; + + /** Access the normals matrix of the polytope */ + inline const Eigen::MatrixXd & normals() const noexcept { return normals_; } + + /** Access the offsets vector of the polytope */ + inline const Eigen::VectorXd & offsets() const noexcept { return offsets_; } + + /** Access the associated contact */ + inline const mc_rbdyn::Contact & contact() const noexcept { return contact_; } + + void updatePolytope(); + +private: + /** Parent instance */ + const mc_rbdyn::Contact & contact_; + + /** Robot index for this polytope */ + const int rIndex_; + + /** Set of planes */ + Eigen::MatrixXd normals_; + Eigen::VectorXd offsets_; + + // Function to generate 3d normals of a friction cone (not the generating rays) + Eigen::MatrixX3d generatePolyhedralConeHRep(int numberOfFrictionSides, + Eigen::Matrix3d rotX_r1_r2, + double m_frictionCoef) + { + Eigen::MatrixX3d HRep(numberOfFrictionSides, 3); + Eigen::Vector3d contactNormal(Eigen::Vector3d::UnitZ()); + Eigen::Vector3d tan(Eigen::Vector3d::UnitX()); + + // The angle to the contact normal of the friction cone is atan(mu) + // (mu for external approximation, mu/sqrt(2) for internal, let's pick internal for H-rep) + // But here for hrep we want the normals of the linearized cone's faces + // --> there is a 90° angle to add to get the face normal + double angle = (M_PI / 2.) + atan(m_frictionCoef / sqrt(2)); + // This is the first face normal + Eigen::Vector3d normal = Eigen::AngleAxisd(angle, tan) * contactNormal; + + // step is the scale decomposition (precision) with which to compute the actual cone (linearization) + double step = (M_PI * 2.) / numberOfFrictionSides; + + // here we compute the hrep: the rows will be the normals of the cone faces in the controlled frame + for(int i = 0; i < numberOfFrictionSides; i++) + { + // Rotation around contact normal for each decomposed angle, then transposed in the relative contact frame + HRep.row(i) = rotX_r1_r2.transpose() * Eigen::AngleAxisd(step * i, contactNormal) * normal; + } + + return HRep; + } + + // Function to generate 6D CoP and rotational friction constraints from the points of a rectangular surface contact + Eigen::MatrixXd computeSurfaceTorqueConstraint(const mc_rbdyn::Surface & surface, double frictionCoeff) + { + const auto & surfacePoints = surface.points(); + // Using the inner friction coeff approximation (linearization) + const auto mu = frictionCoeff / sqrt(2); + // Find boundaries in surface frame along the surface's sagital (x) and lateral (y) direction + double minSagital = std::numeric_limits::max(); + double minLateral = std::numeric_limits::max(); + double maxSagital = -std::numeric_limits::max(); + double maxLateral = -std::numeric_limits::max(); + + Eigen::Vector3d surfaceCenter(0., 0., 0.); + for(const auto & point : surfacePoints) + { + // Points are defined in body frame, convert to surface frame + Eigen::Vector3d surfacePoint = surface.X_b_s().rotation() * (point.translation() - surface.X_b_s().translation()); + double x = surfacePoint.x(); + double y = surfacePoint.y(); + minSagital = std::min(minSagital, x); + maxSagital = std::max(maxSagital, x); + minLateral = std::min(minLateral, y); + maxLateral = std::max(maxLateral, y); + surfaceCenter += surfacePoint; + } + // Getting the surface center (s) in the contact surface frame (c) + surfaceCenter /= double(surfacePoints.size()); + sva::PTransformd X_c_s = sva::PTransformd(surfaceCenter); + + // Getting half-width Y and half-length X + auto X = (maxSagital - minSagital) / 2.; + auto Y = (maxLateral - minLateral) / 2.; + + // The CoP constraints on tau x and tau y are from equations 18 and 19 of + // + // The tau z constraints are the 8 derived inequalities from equation 20 + // The other ones come from the friction cone and are already taken into account in the polytope + + // Assuming this is a rectangular contact + Eigen::Matrix centeredSurfaceConstraintMat; + // clang-format off + centeredSurfaceConstraintMat << + // mx, my, mz, fx, fy, fz, + -1, 0, 0, 0, 0, -Y, + +1, 0, 0, 0, 0, -Y, + 0, -1, 0, 0, 0, -X, + 0, +1, 0, 0, 0, -X, + +mu, +mu, -1, -Y, -X, -(X + Y) * mu, + +mu, -mu, -1, -Y, +X, -(X + Y) * mu, + -mu, +mu, -1, +Y, -X, -(X + Y) * mu, + -mu, -mu, -1, +Y, +X, -(X + Y) * mu, + +mu, +mu, +1, +Y, +X, -(X + Y) * mu, + +mu, -mu, +1, +Y, -X, -(X + Y) * mu, + -mu, +mu, +1, -Y, +X, -(X + Y) * mu, + -mu, -mu, +1, -Y, -X, -(X + Y) * mu; + // clang-format on + + // Transform constraints from surface center to contact frame + Eigen::Matrix surfaceConstraintMat = centeredSurfaceConstraintMat * X_c_s.dualMatrix(); + + return surfaceConstraintMat; + } +}; + +} // namespace mc_tvm diff --git a/include/mc_tvm/ForceInPolytopeFunction.h b/include/mc_tvm/ForceInPolytopeFunction.h new file mode 100644 index 0000000000..414562843a --- /dev/null +++ b/include/mc_tvm/ForceInPolytopeFunction.h @@ -0,0 +1,74 @@ +/* + * Copyright 2015-2022 CNRS-UM LIRMM, CNRS-AIST JRL + */ + +#pragma once + +#include + +#include + +#include +#include + +namespace mc_tvm +{ + +/** This is a linear function to constraint a variable in a set of planes + * + * By providing a set of planes, the function can be used to keep the + * variable in the convex polytope (then the function rhs must be set to <= 0.0) + * + * It is written to constraint a force in its feasible polytope but can be applied to any case + */ +struct MC_TVM_DLLAPI ForceInPolytopeFunction : public tvm::function::abstract::LinearFunction +{ +public: + using Output = tvm::function::abstract::LinearFunction::Output; + DISABLE_OUTPUTS(Output::JDot) + // Since this is a linear function no need to update the value compared to the regular functions + // We expect the format Ax <= b, a linear function enforces Jac * var(s) + B /operator/ rhs + // This means we need to update Jacobian and B depending on the normals and offsets of the poly + SET_UPDATES(ForceInPolytopeFunction, Jacobian, B, Resize) + + /** + * @brief Constructor + * + * @param contact Contact containing the polytope + * @param forceVars Force variable to constraint in the polytope + * @param rIndex Index of the robot to know which of the contact polytopes to use + * @param hasForceVar Whether or not the force variable was created for this robot, true if yes, + * false if it was for another (then we must transform the force between the two contact frames) + */ + ForceInPolytopeFunction(const mc_rbdyn::Contact & contact, + const tvm::VariableVector & forceVars, + const int & rIndex, + const bool & hasForceVar); + + inline const bool & constraintSizeChanged() const noexcept { return constraintSizeChanged_; } + void constraintSizeChanged(bool changed) { constraintSizeChanged_ = changed; } + +protected: + void updateJacobian(); + void updateb(); + // Resize function dim to polytope size + void resizeToPoly(); + + // rbdyn contact object that contains the feasible polytope + const mc_rbdyn::Contact & contact_; + // index of the robot exerting the tracked force + const int rIndex_; + // ref to the tvm polytope for this function (chosen between R1 or R2 of the contact) + mc_tvm::FeasiblePolytope & tvmPoly_; + // force variables that this function acts on + // expected: 4 3d forces or 1 6d wrench + tvm::VariableVector forceVars_; + // bool if the forceVar was created for this robot or the other one (to transform or not between contact frames) + const bool hasForceVar_; + // logic boolean to remove and re add this function to the problem if the dimension changed + bool constraintSizeChanged_; +}; + +using ForceInPolytopeFunctionPtr = std::shared_ptr; + +} // namespace mc_tvm diff --git a/include/mc_tvm/Momentum.h b/include/mc_tvm/Momentum.h index d792c28491..8036a34b26 100644 --- a/include/mc_tvm/Momentum.h +++ b/include/mc_tvm/Momentum.h @@ -26,7 +26,7 @@ namespace mc_tvm struct MC_TVM_DLLAPI Momentum : public tvm::graph::abstract::Node { SET_OUTPUTS(Momentum, Momentum, Jacobian, Velocity, NormalAcceleration, JDot) - SET_UPDATES(Momentum, Momentum, Jacobian, NormalAcceleration, JDot) + SET_UPDATES(Momentum, Momentum, Jacobian, Velocity, NormalAcceleration, JDot) friend struct Robot; @@ -59,6 +59,9 @@ struct MC_TVM_DLLAPI Momentum : public tvm::graph::abstract::Node inline Robot & robot() noexcept { return com_.robot(); } + /** Access the underlying CentroidalMomentumMatrix object to perform computations */ + inline const rbd::CentroidalMomentumMatrix & CMM() const noexcept { return mat_; } + private: CoM & com_; rbd::CentroidalMomentumMatrix mat_; @@ -67,6 +70,7 @@ struct MC_TVM_DLLAPI Momentum : public tvm::graph::abstract::Node void updateMomentum(); sva::ForceVecd velocity_ = sva::ForceVecd::Zero(); + void updateVelocity(); sva::ForceVecd normalAcceleration_; void updateNormalAcceleration(); diff --git a/include/mc_tvm/fwd.h b/include/mc_tvm/fwd.h index caaf7d551b..5b41acde18 100644 --- a/include/mc_tvm/fwd.h +++ b/include/mc_tvm/fwd.h @@ -15,6 +15,9 @@ using CoMPtr = std::unique_ptr; struct Convex; using ConvexPtr = std::unique_ptr; +struct FeasiblePolytope; +using PolytopePtr = std::unique_ptr; + struct Frame; using FramePtr = std::unique_ptr; diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 087ac464c2..a97e6cfbf8 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -302,6 +302,8 @@ set(mc_tvm_HDR ${mc_tvm_HDR_DIR}/ContactFunction.h ${mc_tvm_HDR_DIR}/Convex.h ${mc_tvm_HDR_DIR}/DynamicFunction.h + ${mc_tvm_HDR_DIR}/FeasiblePolytope.h + ${mc_tvm_HDR_DIR}/ForceInPolytopeFunction.h ${mc_tvm_HDR_DIR}/Frame.h ${mc_tvm_HDR_DIR}/FrameVelocity.h ${mc_tvm_HDR_DIR}/GazeFunction.h @@ -327,6 +329,8 @@ set(mc_tvm_SRC mc_tvm/ContactFunction.cpp mc_tvm/Convex.cpp mc_tvm/DynamicFunction.cpp + mc_tvm/FeasiblePolytope.cpp + mc_tvm/ForceInPolytopeFunction.cpp mc_tvm/Frame.cpp mc_tvm/FrameVelocity.cpp mc_tvm/GazeFunction.cpp diff --git a/src/mc_control/MCController.cpp b/src/mc_control/MCController.cpp index 52d33908d1..fce799817e 100644 --- a/src/mc_control/MCController.cpp +++ b/src/mc_control/MCController.cpp @@ -908,6 +908,8 @@ void MCController::updateContacts() auto r2Index = robot(r2).robotIndex(); contacts.emplace_back(robots(), r1Index, r2Index, c.r1Surface, c.r2Surface, c.friction); contacts.back().dof(c.dof); + // XXX check if need to update things here, transfer polytopes etc + if(solver().backend() == Backend::Tasks) { auto cId = contacts.back().contactId(robots()); @@ -1011,7 +1013,7 @@ void MCController::removeCollisions(const std::string & r1, const std::string & cc->reset(); } -void MCController::addContact(const Contact & c) +void MCController::addContact(const Contact & c, bool show) { { // Ensure that optional robots have a name for correct unique set insertion // TODO: it would be better not to store the robots name as optional @@ -1022,28 +1024,37 @@ void MCController::addContact(const Contact & c) } auto [it, inserted] = contacts_.insert(c); + contacts_changed_ |= inserted; const auto & r1 = c.r1.value(); const auto & r2 = c.r2.value(); + // contact already exists, checks if it has changed if(!inserted) { + if(c.feasiblePolytope) + { + it->feasiblePolytope = c.feasiblePolytope; + contacts_changed_ = true; + } + if(it->dof != c.dof) { - mc_rtc::log::info("Changed contact DoF {}::{}/{}::{} to {}", r1, c.r1Surface, r2, c.r2Surface, + mc_rtc::log::info(show, "Changed contact DoF {}::{}/{}::{} to {}", r1, c.r1Surface, r2, c.r2Surface, MC_FMT_STREAMED(c.dof.transpose())); it->dof = c.dof; contacts_changed_ = true; } if(it->friction != c.friction) { - mc_rtc::log::info("Changed contact friction {}::{}/{}::{} to {}", r1, c.r1Surface, r2, c.r2Surface, c.friction); + mc_rtc::log::info(show, "Changed contact friction {}::{}/{}::{} to {}", r1, c.r1Surface, r2, c.r2Surface, + c.friction); it->friction = c.friction; contacts_changed_ = true; } } else { - mc_rtc::log::info("Add contact {}::{}/{}::{} (DoF: {})", r1, c.r1Surface, r2, c.r2Surface, + mc_rtc::log::info(show, "Add contact {}::{}/{}::{} (DoF: {})", r1, c.r1Surface, r2, c.r2Surface, MC_FMT_STREAMED(c.dof.transpose())); } } diff --git a/src/mc_rbdyn/Contact.cpp b/src/mc_rbdyn/Contact.cpp index 3cbff4259f..dc92816225 100644 --- a/src/mc_rbdyn/Contact.cpp +++ b/src/mc_rbdyn/Contact.cpp @@ -9,6 +9,8 @@ #include #include +#include + #include #include @@ -28,7 +30,6 @@ constexpr double Contact::defaultFriction; struct ContactImpl { -public: unsigned int r1Index; unsigned int r2Index; std::shared_ptr r1Surface; @@ -228,14 +229,20 @@ std::vector Contact::loadVector(const mc_rbdyn::Robots & robo Contact::Contact(const Contact & contact) { + std::lock_guard lock(contactMutex_); impl.reset(new ContactImpl({contact.r1Index(), contact.r2Index(), contact.r1Surface()->copy(), contact.r2Surface()->copy(), contact.X_r2s_r1s(), contact.friction(), contact.isFixed(), contact.X_b_s(), contact.ambiguityId()})); dof_ = contact.dof(); + feasiblePolytopeR1_ = contact.feasiblePolytopeR1(); + feasiblePolytopeR2_ = contact.feasiblePolytopeR2(); + this->tvm_polytopeR1_ = std::move(contact.tvm_polytopeR1_); + this->tvm_polytopeR2_ = std::move(contact.tvm_polytopeR2_); } Contact & Contact::operator=(const Contact & rhs) { + std::lock_guard lock(contactMutex_); if(this == &rhs) { return *this; } this->impl->r1Index = rhs.r1Index(); this->impl->r2Index = rhs.r2Index(); @@ -247,6 +254,10 @@ Contact & Contact::operator=(const Contact & rhs) this->impl->friction = rhs.friction(); this->impl->ambiguityId = rhs.ambiguityId(); this->dof_ = rhs.dof(); + this->feasiblePolytopeR1_ = rhs.feasiblePolytopeR1(); + this->feasiblePolytopeR2_ = rhs.feasiblePolytopeR2(); + this->tvm_polytopeR1_ = std::move(rhs.tvm_polytopeR1_); + this->tvm_polytopeR2_ = std::move(rhs.tvm_polytopeR2_); return *this; } @@ -346,6 +357,7 @@ sva::PTransformd Contact::compute_X_r2s_r1s(const mc_rbdyn::Robots & robots) con { sva::PTransformd X_0_r1 = impl->r1Surface->X_0_s(robots.robot(impl->r1Index)); sva::PTransformd X_0_r2 = impl->r2Surface->X_0_s(robots.robot(impl->r2Index)); + impl->X_r2s_r1s = X_0_r1 * X_0_r2.inv(); return X_0_r1 * X_0_r2.inv(); } @@ -433,7 +445,9 @@ std::string Contact::toStr() const bool Contact::operator==(const Contact & rhs) const { - return (*(this->r1Surface()) == *(rhs.r1Surface())) && (*(this->r2Surface()) == *(rhs.r2Surface())); + // FIXME checking contact equality by also checking same robots (surface == only checks names) + return (*(this->r1Surface()) == *(rhs.r1Surface())) && (*(this->r2Surface()) == *(rhs.r2Surface())) + && (this->r1Index() == rhs.r1Index()) && (this->r2Index() == rhs.r2Index()); } bool Contact::operator!=(const Contact & rhs) const @@ -451,6 +465,52 @@ void Contact::friction(double friction) impl->friction = friction; } +void Contact::feasiblePolytopeR1(const mc_rbdyn::FeasiblePolytope & polytope) +{ + std::lock_guard lock(contactMutex_); + feasiblePolytopeR1_ = polytope; +} + +void Contact::feasiblePolytopeR2(const mc_rbdyn::FeasiblePolytope & polytope) +{ + std::lock_guard lock(contactMutex_); + feasiblePolytopeR2_ = polytope; +} + +const std::optional & Contact::feasiblePolytopeR1() const noexcept +{ + std::lock_guard lock(contactMutex_); + return feasiblePolytopeR1_; +} + +const std::optional & Contact::feasiblePolytopeR2() const noexcept +{ + std::lock_guard lock(contactMutex_); + return feasiblePolytopeR2_; +} + +mc_tvm::FeasiblePolytope & Contact::tvmPolytopeR1() const +{ + if(!tvm_polytopeR1_) + { + mc_rtc::log::warning("Creating a new feasible polytope tvm object"); + tvm_polytopeR1_.reset( + new mc_tvm::FeasiblePolytope(mc_tvm::FeasiblePolytope::NewPolytopeToken{}, *this, this->r1Index())); + } + return *tvm_polytopeR1_; +} + +mc_tvm::FeasiblePolytope & Contact::tvmPolytopeR2() const +{ + if(!tvm_polytopeR2_) + { + mc_rtc::log::warning("Creating a new feasible polytope tvm object"); + tvm_polytopeR2_.reset( + new mc_tvm::FeasiblePolytope(mc_tvm::FeasiblePolytope::NewPolytopeToken{}, *this, this->r2Index())); + } + return *tvm_polytopeR2_; +} + Contact Contact::swap(const mc_rbdyn::Robots & robots) const { const auto & c = *this; diff --git a/src/mc_rbdyn/Robot.cpp b/src/mc_rbdyn/Robot.cpp index 2c70f320e5..911d1d3acc 100644 --- a/src/mc_rbdyn/Robot.cpp +++ b/src/mc_rbdyn/Robot.cpp @@ -427,11 +427,19 @@ Robot::Robot(NewRobotToken, } } + // Add frames for all bodies for(const auto & b : mb().bodies()) { frames_[b.name()] = std::make_shared(RobotFrame::NewRobotFrameToken{}, b.name(), *this, b.name()); } + // Add frames for all force sensors + for(auto & fs : forceSensors()) + { + frames_[fs.name()] = std::make_shared(RobotFrame::NewRobotFrameToken{}, fs.name(), frame(fs.parent()), + fs.X_p_s(), false); + } + if(loadFiles) { if(fs::exists(module_.rsdf_dir)) { loadRSDFFromDir(module_.rsdf_dir); } diff --git a/src/mc_rbdyn/Surface.cpp b/src/mc_rbdyn/Surface.cpp index f831279d6b..0918ff68e3 100644 --- a/src/mc_rbdyn/Surface.cpp +++ b/src/mc_rbdyn/Surface.cpp @@ -107,6 +107,7 @@ std::vector & Surface::points() return impl->points; } +// FIXME this does not ensure equality !! the same surface name can be present on different robots bool Surface::operator==(const Surface & rhs) { return this->name() == rhs.name(); diff --git a/src/mc_solver/DynamicsConstraint.cpp b/src/mc_solver/DynamicsConstraint.cpp index 09b82a5845..76722b89df 100644 --- a/src/mc_solver/DynamicsConstraint.cpp +++ b/src/mc_solver/DynamicsConstraint.cpp @@ -127,7 +127,11 @@ void DynamicsConstraint::addToSolverImpl(QPSolver & solver) auto dyn = problem.add(dyn_fn == 0., tvm::task_dynamics::None(), {tvm::requirements::PriorityLevel(0)}); constraints_.push_back(dyn); auto cstr = problem.constraint(*dyn); - problem.add(tvm::hint::Substitution(cstr, tvm_robot.tau())); + // FIXME [BUG] the substitutions lose some relations between variables if tasks are added or removed, + // but without it some solver events are not triggered correctly. For now we add and remove it + // immediately when the constraint is added to the solver. + // - substitutions fail in conjunction with ProblemDefinitionEvent::TaskAddVariable + // problem.add(tvm::hint::Substitution(cstr, tvm_robot.tau())); break; } default: @@ -150,7 +154,9 @@ void DynamicsConstraint::removeFromSolverImpl(QPSolver & solver) { auto & constr = *static_cast(constraint_.get()); auto & problem = tvm_solver(solver).problem(); - problem.removeSubstitutionFor(*problem.constraint(*constr.constraints_.back())); + // FIXME: the substitutions lose some relations between variables if tasks are added or removed, + // commenting it for now + // problem.removeSubstitutionFor(*problem.constraint(*constr.constraints_.back())); KinematicsConstraint::removeFromSolverImpl(solver); break; } diff --git a/src/mc_solver/KinematicsConstraint.cpp b/src/mc_solver/KinematicsConstraint.cpp index 7e80b5b76e..b3e07c4446 100644 --- a/src/mc_solver/KinematicsConstraint.cpp +++ b/src/mc_solver/KinematicsConstraint.cpp @@ -39,39 +39,43 @@ void TVMKinematicsConstraint::addToSolver(mc_solver::TVMQPSolver & solver) /** Joint limits */ int startParam = tvm_robot.qFloatingBase()->size(); auto nParams = tvm_robot.qJoints()->size(); - auto ql = tvm_robot.limits().ql.segment(startParam, nParams); - auto qu = tvm_robot.limits().qu.segment(startParam, nParams); - Eigen::VectorXd di = damper_[0] * (qu - ql); - Eigen::VectorXd ds = damper_[1] * (qu - ql); - for(int i = 0; i < nParams; ++i) + // Only build damper and joint limits if there are actuated joints + if(nParams != 0) { - if(std::isinf(di(i))) + auto ql = tvm_robot.limits().ql.segment(startParam, nParams); + auto qu = tvm_robot.limits().qu.segment(startParam, nParams); + Eigen::VectorXd di = damper_[0] * (qu - ql); + Eigen::VectorXd ds = damper_[1] * (qu - ql); + for(int i = 0; i < nParams; ++i) { - di(i) = 0.01; - ds(i) = 0.005; + if(std::isinf(di(i))) + { + di(i) = 0.01; + ds(i) = 0.005; + } } + auto jl = solver.problem().add( + ql <= tvm_robot.qJoints() <= qu, + tvm::task_dynamics::VelocityDamper(solver.dt(), {di, ds, Eigen::VectorXd::Constant(nParams, 1, 0), + Eigen::VectorXd::Constant(nParams, 1, damper_[2])}), + {tvm::requirements::PriorityLevel(0)}); + constraints_.push_back(jl); + /** Velocity limits */ + int startDof = tvm_robot.qFloatingBase()->space().tSize(); + auto nDof = tvm_robot.qJoints()->space().tSize(); + auto vl = tvm_robot.limits().vl.segment(startDof, nDof) * velocityPercent_; + auto vu = tvm_robot.limits().vu.segment(startDof, nDof) * velocityPercent_; + auto vL = + solver.problem().add(vl <= tvm::dot(tvm_robot.qJoints()) <= vu, + tvm::task_dynamics::Proportional(1 / solver.dt()), {tvm::requirements::PriorityLevel(0)}); + constraints_.push_back(vL); + /** Acceleration limits */ + auto al = tvm_robot.limits().al.segment(startDof, nDof); + auto au = tvm_robot.limits().au.segment(startDof, nDof); + auto aL = solver.problem().add(al <= tvm::dot(tvm_robot.qJoints(), 2) <= au, tvm::task_dynamics::None{}, + {tvm::requirements::PriorityLevel(0)}); + constraints_.push_back(aL); } - auto jl = solver.problem().add( - ql <= tvm_robot.qJoints() <= qu, - tvm::task_dynamics::VelocityDamper(solver.dt(), {di, ds, Eigen::VectorXd::Constant(nParams, 1, 0), - Eigen::VectorXd::Constant(nParams, 1, damper_[2])}), - {tvm::requirements::PriorityLevel(0)}); - constraints_.push_back(jl); - /** Velocity limits */ - int startDof = tvm_robot.qFloatingBase()->space().tSize(); - auto nDof = tvm_robot.qJoints()->space().tSize(); - auto vl = tvm_robot.limits().vl.segment(startDof, nDof) * velocityPercent_; - auto vu = tvm_robot.limits().vu.segment(startDof, nDof) * velocityPercent_; - auto vL = - solver.problem().add(vl <= tvm::dot(tvm_robot.qJoints()) <= vu, tvm::task_dynamics::Proportional(1 / solver.dt()), - {tvm::requirements::PriorityLevel(0)}); - constraints_.push_back(vL); - /** Acceleration limits */ - auto al = tvm_robot.limits().al.segment(startDof, nDof); - auto au = tvm_robot.limits().au.segment(startDof, nDof); - auto aL = solver.problem().add(al <= tvm::dot(tvm_robot.qJoints(), 2) <= au, tvm::task_dynamics::None{}, - {tvm::requirements::PriorityLevel(0)}); - constraints_.push_back(aL); /** Mimic constraints */ for(const auto & m : tvm_robot.mimics()) { @@ -84,8 +88,10 @@ void TVMKinematicsConstraint::addToSolver(mc_solver::TVMQPSolver & solver) Eigen::MatrixXd A = mimicMult.segment(startIdx, f->size()); auto mimic = solver.problem().add(A * tvm::dot(leader, 2) - tvm::dot(f, 2) == 0., tvm::task_dynamics::None{}, {tvm::requirements::PriorityLevel(0)}); - solver.problem().add(tvm::hint::Substitution(solver.problem().constraint(*mimic), tvm::dot(f, 2), - tvm::constant::fullRank, tvm::hint::internal::DiagonalCalculator{})); + // FIXME substitutions are broken in tvm for now + // solver.problem().add(tvm::hint::Substitution(solver.problem().constraint(*mimic), tvm::dot(f, 2), + // tvm::constant::fullRank, + // tvm::hint::internal::DiagonalCalculator{})); mimics_constraints_.push_back(mimic); startIdx += f->size(); } @@ -97,7 +103,8 @@ void TVMKinematicsConstraint::removeFromSolver(mc_solver::TVMQPSolver & solver) { for(auto & c : mimics_constraints_) { - solver.problem().removeSubstitutionFor(*solver.problem().constraint(*c)); + // FIXME substitutions are broken in tvm for now + // solver.problem().removeSubstitutionFor(*solver.problem().constraint(*c)); solver.problem().remove(*c); } for(auto & c : constraints_) { solver.problem().remove(*c); } diff --git a/src/mc_solver/TVMQPSolver.cpp b/src/mc_solver/TVMQPSolver.cpp index 7d0cf4236f..9d025a36e8 100644 --- a/src/mc_solver/TVMQPSolver.cpp +++ b/src/mc_solver/TVMQPSolver.cpp @@ -2,6 +2,7 @@ * Copyright 2015-2022 CNRS-UM LIRMM, CNRS-AIST JRL */ +#include #include #include @@ -10,6 +11,8 @@ #include #include +#include +#include #include #include @@ -17,11 +20,15 @@ #include #include +#include +namespace fs = std::filesystem; + namespace mc_solver { inline static Eigen::MatrixXd discretizedFrictionCone(double muI) { + // 4 faces, 3 forces Eigen::MatrixXd C(4, 3); double mu = muI / std::sqrt(2); C << Eigen::Matrix2d::Identity(), Eigen::Vector2d::Constant(mu), -Eigen::Matrix2d::Identity(), @@ -32,15 +39,26 @@ inline static Eigen::MatrixXd discretizedFrictionCone(double muI) TVMQPSolver::TVMQPSolver(mc_rbdyn::RobotsPtr robots, double dt) : QPSolver(robots, dt, Backend::TVM), solver_(tvm::solver::DefaultLSSolverOptions{}) { + tvm::graph::internal::Logger::logger().enable(); } TVMQPSolver::TVMQPSolver(double dt) : QPSolver(dt, Backend::TVM), solver_(tvm::solver::DefaultLSSolverOptions{}) {} +void TVMQPSolver::gui(std::shared_ptr gui) +{ + QPSolver::gui(gui); + + gui->removeElements(this); + + gui_->addElement(this, {"Solver", "TVM"}, + mc_rtc::gui::Button("Generate Graph dot file (graphviz)", [this]() { this->saveGraphDotFile(); })); +} + size_t TVMQPSolver::getContactIdx(const mc_rbdyn::Contact & contact) { for(size_t i = 0; i < contacts_.size(); ++i) { - if(contacts_[i] == contact) { return i; } + if(*contacts_[i] == contact) { return i; } } return contacts_.size(); } @@ -52,14 +70,20 @@ void TVMQPSolver::setContacts(ControllerToken, const std::vectorname(); - const std::string & r2 = robots().robot(c.r2Index()).name(); - const std::string & r2S = c.r2Surface()->name(); + const std::string & r1 = robots().robot(c->r1Index()).name(); + const std::string & r1S = c->r1Surface()->name(); + const std::string & r2 = robots().robot(c->r2Index()).name(); + const std::string & r2S = c->r2Surface()->name(); logger_->removeLogEntry("contact_" + r1 + "::" + r1S + "_" + r2 + "::" + r2S); - if(gui_) { gui_->removeElement({"Contacts", "Forces"}, fmt::format("{}::{}/{}::{}", r1, r1S, r2, r2S)); } + logger_->removeLogEntry("contact_" + r2 + "::" + r2S + "_" + r1 + "::" + r1S); + if(gui_) + { + gui_->removeElement({"Contacts", "Forces"}, fmt::format("{}::{}/{}::{}", r1, r1S, r2, r2S)); + gui_->removeElement({"Contacts", "Forces"}, fmt::format("{}::{}/{}::{}", r2, r2S, r1, r1S)); + } it = removeContact(i); } else @@ -73,11 +97,16 @@ void TVMQPSolver::setContacts(ControllerToken, const std::vectorsecond->dynamicFunction().contactForce(r1.frame(id.r1Surface()->name())); } + auto it = dynamics_.find(r1.name()); + if(it != dynamics_.end()) { return it->second->dynamicFunction().contactForce(r1.frame(id.r1Surface()->name())); } + return sva::ForceVecd::Zero(); +} + +const sva::ForceVecd TVMQPSolver::desiredContactForce2(const mc_rbdyn::Contact & id) const +{ const auto & r2 = robot(id.r2Index()); - auto it2 = dynamics_.find(r2.name()); - if(it2 != dynamics_.end()) { return it2->second->dynamicFunction().contactForce(r2.frame(id.r2Surface()->name())); } + auto it = dynamics_.find(r2.name()); + if(it != dynamics_.end()) { return it->second->dynamicFunction().contactForce(r2.frame(id.r2Surface()->name())); } return sva::ForceVecd::Zero(); } @@ -266,21 +295,23 @@ void TVMQPSolver::addDynamicsConstraint(mc_solver::DynamicsConstraint * dyn) { const auto & contact = contacts_[i]; auto & data = contactsData_[i]; - bool isR1 = contact.r1Index() == dyn->robotIndex(); - bool isR2 = contact.r2Index() == dyn->robotIndex(); + bool isR1 = contact->r1Index() == dyn->robotIndex(); + bool isR2 = contact->r2Index() == dyn->robotIndex(); if(isR1 || isR2) { - const auto & r1 = robot(contact.r1Index()); - const auto & r2 = robot(contact.r2Index()); - const auto & s1 = *contact.r1Surface(); - const auto & s2 = *contact.r2Surface(); + const auto & r1 = robot(contact->r1Index()); + const auto & r2 = robot(contact->r2Index()); + const auto & s1 = *contact->r1Surface(); + const auto & s2 = *contact->r2Surface(); const auto & f1 = r1.frame(s1.name()); const auto & f2 = r2.frame(s2.name()); - const auto C = discretizedFrictionCone(contact.friction()); // FIXME Debug mc_rbdyn::intersection // auto s1Points = mc_rbdyn::intersection(s1, s2); const auto & s1Points = s1.points(); - if(isR1) { addContactToDynamics(r1.name(), f1, s1Points, data.f1_, data.f1Constraints_, C, 1.0); } + if(isR1) + { + addContactToDynamics(r1.name(), f1, s1Points, data.f1_, data.f1Constraints_, data.f1Targets_, *contact, 1.0); + } if(isR2) { std::vector s2Points; @@ -288,7 +319,7 @@ void TVMQPSolver::addDynamicsConstraint(mc_solver::DynamicsConstraint * dyn) auto X_b2_b1 = r1.mbc().bodyPosW[r1.bodyIndexByName(f1.body())] * r2.mbc().bodyPosW[r2.bodyIndexByName(f2.body())].inv(); for(const auto & X_b1_p : s1Points) { s2Points.push_back(X_b1_p * X_b2_b1); } - addContactToDynamics(r2.name(), f2, s2Points, data.f2_, data.f2Constraints_, C, -1.0); + addContactToDynamics(r2.name(), f2, s2Points, data.f2_, data.f2Constraints_, data.f2Targets_, *contact, -1.0); } } } @@ -314,69 +345,148 @@ void TVMQPSolver::removeDynamicsConstraint(mc_solver::DynamicsConstraint * dyn) const auto & contact = contacts_[i]; auto & data = contactsData_[i]; auto clearContacts = [&](const std::string & robot, tvm::VariableVector & forces, - std::vector & constraints) + std::vector & constraints, + std::vector & targets) { if(robot != r.name()) { return; } for(auto & c : constraints) { problem_.remove(*c); } constraints.clear(); + for(auto & t : targets) { problem_.remove(*t); } + targets.clear(); forces = tvm::VariableVector(); }; - const auto & r1 = robot(contact.r1Index()); - clearContacts(r1.name(), data.f1_, data.f1Constraints_); - const auto & r2 = robot(contact.r2Index()); - clearContacts(r2.name(), data.f2_, data.f2Constraints_); + const auto & r1 = robot(contact->r1Index()); + clearContacts(r1.name(), data.f1_, data.f1Constraints_, data.f1Targets_); + const auto & r2 = robot(contact->r2Index()); + clearContacts(r2.name(), data.f2_, data.f2Constraints_, data.f2Targets_); } } +// FIXME remove dir argument after confirming new logic handles it correctly void TVMQPSolver::addContactToDynamics(const std::string & robot, const mc_rbdyn::RobotFrame & frame, const std::vector & points, tvm::VariableVector & forces, std::vector & constraints, - const Eigen::MatrixXd & frictionCone, + std::vector & targets, + mc_rbdyn::Contact & contact, double dir) { auto it = dynamics_.find(robot); + bool hasForceVar; + // If this robot does not have a dynamics constraint, nothing to add on this side, just return if(it == dynamics_.end()) { return; } - if(constraints.size()) + if(constraints.size() || targets.size()) { // FIXME Instead of this we should be able to change C + // FIXME We keep this as a safety but in practice this is not called anymore (no hasWork if friction changed) and is + // now handled by the feasible polytope constraint automatically + mc_rtc::log::critical("[SHOULD NOT APPEAR] removing already existing contact constraint"); for(const auto & c : constraints) { problem_.remove(*c); } constraints.clear(); + for(const auto & t : targets) { problem_.remove(*t); } + targets.clear(); } else { - it->second->removeFromSolverImpl(*this); auto & dyn = it->second->dynamicFunction(); - forces = dyn.addContact(frame, points, dir); - it->second->addToSolverImpl(*this); + + /* Now we consider the force decision variable for the contact: + If this is a completely new contact we want to create the force variable, but if the other side of the contact + has already been created by the other robot's dynamic function we want to reuse it with the opposite direction + */ + // XXX this is only handled by 6d contact var, otherwise the total of the force variables must be opposite instead + // of just the variables, much more complex logic + tvm::VariableVector existingVariables; + // Iterate on all dynamics functions in the solver + for(const auto & [_, d] : dynamics_) + { + // check if a dynamics function already handles this contact's other frame + // i.e. the force decision variable already exists and we just need to reuse it in direction -1 + // instead of creating a new one + + if(frame.robot().robotIndex() == contact.r1Index()) + { + // This means the other robot in the contact is r2 + if(d->robotIndex() == contact.r2Index()) + { + // We found a dynamics function involving the other robot of this contact, check if it has this contact. + // The robot frame would be r2surface of this contact + existingVariables = d->dynamicFunction().getForceVariables(contact.r2Surface()->name()); + } + } + else + { + // This means the other robot in the contact is r1 + if(d->robotIndex() == contact.r1Index()) + { + // We found a dynamics function involving the other robot of this contact, check if it has this contact. + // The robot frame would be r1surface of this contact + existingVariables = d->dynamicFunction().getForceVariables(contact.r1Surface()->name()); + } + } + } + + // FIXME need to check what happens to var if first dyn function is destroyed + if(existingVariables.numberOfVariables() != 0) + { + // There were pre existing force variables for the other side of this contact + hasForceVar = false; + forces.add(existingVariables); + dyn.addContact6d(frame, existingVariables[0], contact); + } + else + { + // Create decision variables + hasForceVar = true; + // forces = dyn.addContact3d(frame, points, dir); + forces.add(dyn.addContact6d(frame, contact)); + } } + for(int i = 0; i < forces.numberOfVariables(); ++i) { auto & f = forces[i]; - constraints.push_back(problem_.add(dir * frictionCone * f >= 0.0, {tvm::requirements::PriorityLevel(0)})); + auto polyFunction = + std::make_shared(contact, f, robots().robotIndex(robot), hasForceVar); + // We want the force to stay inside of the polytope so the distance value should stay negative + constraints.push_back( + problem_.add(polyFunction <= 0., tvm::task_dynamics::None(), {tvm::requirements::PriorityLevel(0)})); + // Add a minimization on the force variable with a low weight + // TODO maybe find a way to parametrize a desired anisotropic weight from the mc_rbdyn::Contact ? + // TODO write a linear function to target zero (minimize) or a specific target + // targets.push_back(problem_.add(f == 0.0, {tvm::requirements::PriorityLevel(1), + // tvm::requirements::Weight(0.0001)})); } } auto TVMQPSolver::addVirtualContactImpl(const mc_rbdyn::Contact & contact) -> std::tuple { + // FIXME handle swapping of contact bool hasWork = false; auto idx = getContactIdx(contact); if(idx < contacts_.size()) { - const auto & oldContact = contacts_[idx]; + // This contact already exists in the solver + const auto & oldContact = *contacts_[idx]; if(oldContact.dof() == contact.dof() && oldContact.friction() == contact.friction()) { + // dof and friction stayed, no copy or work to do return std::make_tuple(idx, hasWork); } - hasWork = contact.friction() != oldContact.friction(); - contacts_[idx] = contact; + // Update internal contact map if any change + *contacts_[idx] = contact; } else { + // New contact so need to do everything hasWork = true; - contacts_.push_back(contact); + contacts_.emplace_back(std::make_shared(contact)); } + + const auto storedContact = contacts_[idx]; + + // Get the contactData element for this contact or create it if new contact auto & data = idx < contactsData_.size() ? contactsData_[idx] : contactsData_.emplace_back(); const auto & r1 = robot(contact.r1Index()); const auto & r2 = robot(contact.r2Index()); @@ -384,6 +494,7 @@ auto TVMQPSolver::addVirtualContactImpl(const mc_rbdyn::Contact & contact) -> st const auto & f2 = r2.frame(contact.r2Surface()->name()); if(!data.contactConstraint_) // New contact { + // Create a new contact function, add it to the problem and keep it in the contactData element auto contact_fn = std::make_shared(f1, f2, contact.dof()); // Check if a contact constraint exists in the QP for(const auto & constraint : constraints()) @@ -430,82 +541,163 @@ auto TVMQPSolver::addVirtualContactImpl(const mc_rbdyn::Contact & contact) -> st } logger_->addLogEntry(fmt::format("contact_{}::{}_{}::{}", r1.name(), f1.name(), r2.name(), f2.name()), - [this, contact]() { return desiredContactForce(contact); }); - gui_->addElement({"Contacts", "Forces"}, - mc_rtc::gui::Force( - fmt::format("{}::{}/{}::{}", r1.name(), f1.name(), r2.name(), f2.name()), [this, contact]() - { return desiredContactForce(contact); }, [&f1]() { return f1.position(); })); + [this, storedContact]() { return desiredContactForce(*storedContact); }); + logger_->addLogEntry(fmt::format("contact_{}::{}_{}::{}", r2.name(), f2.name(), r1.name(), f1.name()), + [this, storedContact]() { return desiredContactForce2(*storedContact); }); + gui_->addElement( + {"Contacts", "Forces"}, + mc_rtc::gui::Force( + fmt::format("{}::{}/{}::{}", r1.name(), f1.name(), r2.name(), f2.name()), + [this, storedContact]() { return desiredContactForce(*storedContact); }, [&f1]() { return f1.position(); }), + mc_rtc::gui::Force( + fmt::format("{}::{}/{}::{}", r2.name(), f2.name(), r1.name(), f1.name()), [this, storedContact]() + { return desiredContactForce2(*storedContact); }, [&f2]() { return f2.position(); })); } else { + // The contact function already exists, just update the contact dof auto contact_fn = std::static_pointer_cast(data.contactConstraint_->task.function()); contact_fn->dof(contact.dof()); } return std::make_tuple(idx, hasWork); } -void TVMQPSolver::addContact(const mc_rbdyn::Contact & contact) +void TVMQPSolver::addContact(const mc_rbdyn::Contact & contactTmp) { - size_t idx = contacts_.size(); - bool hasWork = false; - std::tie(idx, hasWork) = addVirtualContactImpl(contact); + // Add geometric contact constraint if it is not already present + // hasWork becomes true if new contact, in this case dynamics function must be updated + // dofs or friction changing do not influence the dynamics so does not matter here + // WARNING: copies the contactTmp passed as argument into contact_[idx] + // Any object storing a reference to this contact must use contact_[idx] + const auto [idx, hasWork] = addVirtualContactImpl(contactTmp); + // If !hasWork, then the contact already existed and is already in the dynamics + // addVirtual function updated it if needed, just return now if(!hasWork) { return; } auto & data = contactsData_[idx]; - const auto & r1 = robot(contact.r1Index()); - const auto & r2 = robot(contact.r2Index()); - const auto & s1 = *contact.r1Surface(); - const auto & s2 = *contact.r2Surface(); + const auto & r1 = robot(contactTmp.r1Index()); + const auto & r2 = robot(contactTmp.r2Index()); + const auto & s1 = *contactTmp.r1Surface(); + const auto & s2 = *contactTmp.r2Surface(); const auto & f1 = r1.frame(s1.name()); const auto & f2 = r2.frame(s2.name()); - // FIXME Let the user decide how much the friction cone should be discretized - auto C = discretizedFrictionCone(contact.friction()); - auto addContactForce = [&](const std::string & robot, const mc_rbdyn::RobotFrame & frame, - const std::vector & points, tvm::VariableVector & forces, - std::vector & constraints, double dir) - { addContactToDynamics(robot, frame, points, forces, constraints, C, dir); }; + + auto & addedContact = *contacts_[idx]; + + auto addContactForce = [&addedContact, this](const std::string & robot, const mc_rbdyn::RobotFrame & frame, + const std::vector & points, + tvm::VariableVector & forces, + std::vector & constraints, + std::vector & targets, double dir) + { addContactToDynamics(robot, frame, points, forces, constraints, targets, addedContact, dir); }; + // FIXME These points computation are a waste of time if they are not needed // FIXME Debug mc_rbdyn::intersection // auto s1Points = mc_rbdyn::intersection(s1, s2); auto s1Points = s1.points(); - addContactForce(r1.name(), f1, s1Points, data.f1_, data.f1Constraints_, 1.0); + mc_rtc::log::info("Dynamics change: adding contact force direction 1"); + addContactForce(r1.name(), f1, s1Points, data.f1_, data.f1Constraints_, data.f1Targets_, 1.0); std::vector s2Points; s2Points.reserve(s1Points.size()); auto X_b2_b1 = r1.mbc().bodyPosW[r1.bodyIndexByName(f1.body())] * r2.mbc().bodyPosW[r2.bodyIndexByName(f2.body())].inv(); std::transform(s1Points.begin(), s1Points.end(), std::back_inserter(s2Points), [&](const auto & X_b1_p) { return X_b1_p * X_b2_b1; }); - addContactForce(r2.name(), f2, s2Points, data.f2_, data.f2Constraints_, -1.0); + mc_rtc::log::info("Dynamics change: adding contact force direction -1"); + addContactForce(r2.name(), f2, s2Points, data.f2_, data.f2Constraints_, data.f2Targets_, -1.0); } auto TVMQPSolver::removeContact(size_t idx) -> ContactIterator { - auto & contact = contacts_[idx]; + auto & contact = *contacts_[idx]; auto & data = contactsData_[idx]; const auto & r1 = robot(contact.r1Index()); auto r1DynamicsIt = dynamics_.find(r1.name()); if(r1DynamicsIt != dynamics_.end()) { - r1DynamicsIt->second->removeFromSolverImpl(*this); r1DynamicsIt->second->dynamicFunction().removeContact(r1.frame(contact.r1Surface()->name())); - r1DynamicsIt->second->addToSolverImpl(*this); } const auto & r2 = robot(contact.r2Index()); auto r2DynamicsIt = dynamics_.find(r2.name()); if(r2DynamicsIt != dynamics_.end()) { - r2DynamicsIt->second->removeFromSolverImpl(*this); r2DynamicsIt->second->dynamicFunction().removeContact(r2.frame(contact.r2Surface()->name())); - r2DynamicsIt->second->addToSolverImpl(*this); } for(const auto & c : data.f1Constraints_) { problem_.remove(*c); } for(const auto & c : data.f2Constraints_) { problem_.remove(*c); } + for(const auto & t : data.f1Targets_) { problem_.remove(*t); } + for(const auto & t : data.f2Targets_) { problem_.remove(*t); } if(data.contactConstraint_) { problem_.remove(*data.contactConstraint_); data.contactConstraint_.reset(); } contactsData_.erase(contactsData_.begin() + static_cast(idx)); + return contacts_.erase(contacts_.begin() + static_cast(idx)); } +bool TVMQPSolver::saveGraphDotFile() const +{ + constexpr auto prefix = "mc_rtc_tvm_graph"; + auto get_path = [&]() + { + std::stringstream ss; + auto t = std::time(nullptr); + auto tm = std::localtime(&t); + // clang-format off + ss << prefix + << "-" << (1900 + tm->tm_year) + << "-" << std::setw(2) << std::setfill('0') << (1 + tm->tm_mon) + << "-" << std::setw(2) << std::setfill('0') << tm->tm_mday + << "-" << std::setw(2) << std::setfill('0') << tm->tm_hour + << "-" << std::setw(2) << std::setfill('0') << tm->tm_min + << "-" << std::setw(2) << std::setfill('0') << tm->tm_sec + << ".dot"; + // clang-format on + auto directory = fs::temp_directory_path(); + auto log_path = directory / fs::path(ss.str().c_str()); + return std::pair{directory, log_path}; + }; + + const auto [directory, log_path] = get_path(); + bool ret = saveGraphDotFile(log_path.string()); + if(ret) + { // Generate symlink to the latest saved graph + std::stringstream ss_sym; + ss_sym << prefix << "-latest.dot"; + fs::path log_sym_path = directory / fs::path(ss_sym.str().c_str()); + if(fs::is_symlink(log_sym_path)) { fs::remove(log_sym_path); } + if(!fs::exists(log_sym_path)) + { + std::error_code ec; + fs::create_symlink(log_path, log_sym_path); + if(!ec) { mc_rtc::log::info("Updated latest graph symlink: {}", log_sym_path.string()); } + else + { + mc_rtc::log::warning("Failed to create latest graph symlink: {}", ec.message()); + } + } + } + return ret; +} + +bool TVMQPSolver::saveGraphDotFile(const std::string & filename) const +{ + try + { + auto graphDot = tvm::graph::internal::Logger::logger().log().generateDot(&problem_.updateGraph()); + std::ofstream myfile; + myfile.open(filename); + myfile << graphDot << std::endl; + myfile.close(); + mc_rtc::log::info("[TVMQPSolver] Saved dot graph to {}", filename); + return true; + } + catch(std::exception & e) + { + mc_rtc::log::error("[TVMQPSolver] Failed to print graph: {}", e.what()); + } + return false; +} + } // namespace mc_solver diff --git a/src/mc_solver/TasksQPSolver.cpp b/src/mc_solver/TasksQPSolver.cpp index dc8644a295..2e9fac5ebe 100644 --- a/src/mc_solver/TasksQPSolver.cpp +++ b/src/mc_solver/TasksQPSolver.cpp @@ -49,43 +49,57 @@ void TasksQPSolver::setContacts(ControllerToken, const std::vectorname(); - const std::string & r2 = robots().robot(contact.r2Index()).name(); - const std::string & r2S = contact.r2Surface()->name(); - if(logger_) { logger_->removeLogEntry("contact_" + r1 + "::" + r1S + "_" + r2 + "::" + r2S); } - if(gui_) { gui_->removeElement({"Contacts", "Forces"}, fmt::format("{}::{}/{}::{}", r1, r1S, r2, r2S)); } + for(const auto & contact : contacts_) + { + const std::string & r1 = robots().robot(contact->r1Index()).name(); + const std::string & r1S = contact->r1Surface()->name(); + const std::string & r2 = robots().robot(contact->r2Index()).name(); + const std::string & r2S = contact->r2Surface()->name(); + logger_->removeLogEntry("contact_" + r1 + "::" + r1S + "_" + r2 + "::" + r2S); + } } - contacts_ = contacts; - for(auto it = contacts_.begin(); it != contacts_.end();) + if(gui_) { - auto & c = *it; - const auto & r1 = robots().robot(c.r1Index()); - const auto & r2 = robots().robot(c.r2Index()); - if(r1.mb().nrDof() == 0) + for(const auto & contact : contacts_) { - if(r2.mb().nrDof() != 0) { c = c.swap(robots()); } - else - { - it = contacts_.erase(it); - continue; - } + const std::string & r1 = robots().robot(contact->r1Index()).name(); + const std::string & r1S = contact->r1Surface()->name(); + const std::string & r2 = robots().robot(contact->r2Index()).name(); + const std::string & r2S = contact->r2Surface()->name(); + gui_->removeElement({"Contacts", "Forces"}, fmt::format("{}::{}/{}::{}", r1, r1S, r2, r2S)); } - ++it; } - for(const auto & contact : contacts_) + + contacts_.clear(); + for(auto & c : contacts) { contacts_.emplace_back(std::make_shared(c)); } + + for(auto & c : contacts_) + { + const auto & r1 = robots().robot(c->r1Index()); + if(r1.mb().nrDof() == 0) { *c = c->swap(robots()); } + } + if(logger_) { - const std::string & r1 = robots().robot(contact.r1Index()).name(); - const std::string & r1S = contact.r1Surface()->name(); - const std::string & r2 = robots().robot(contact.r2Index()).name(); - const std::string & r2S = contact.r2Surface()->name(); - if(logger_) + for(const auto & c : contacts_) { + const auto & contact = *c; + const std::string & r1 = robots().robot(contact.r1Index()).name(); + const std::string & r1S = contact.r1Surface()->name(); + const std::string & r2 = robots().robot(contact.r2Index()).name(); + const std::string & r2S = contact.r2Surface()->name(); logger_->addLogEntry("contact_" + r1 + "::" + r1S + "_" + r2 + "::" + r2S, [this, &contact]() { return desiredContactForce(contact); }); } - if(gui_) + } + if(gui_) + { + for(const auto & c : contacts_) { + const auto & contact = *c; + const std::string & r1 = robots().robot(contact.r1Index()).name(); + const std::string & r1S = contact.r1Surface()->name(); + const std::string & r2 = robots().robot(contact.r2Index()).name(); + const std::string & r2S = contact.r2Surface()->name(); gui_->addElement({"Contacts", "Forces"}, mc_rtc::gui::Force( fmt::format("{}::{}/{}::{}", r1, r1S, r2, r2S), @@ -96,9 +110,9 @@ void TasksQPSolver::setContacts(ControllerToken, const std::vectortaskContact(*robots_p); if(qcptr.unilateralContact) { uniContacts_.push_back(tasks::qp::UnilateralContact(*qcptr.unilateralContact)); diff --git a/src/mc_tasks/MomentumTask.cpp b/src/mc_tasks/MomentumTask.cpp index 58ef464871..35945e9e48 100644 --- a/src/mc_tasks/MomentumTask.cpp +++ b/src/mc_tasks/MomentumTask.cpp @@ -98,6 +98,8 @@ void MomentumTask::addToLogger(mc_rtc::Logger & logger) case Backend::TVM: logger.addLogEntry(name_ + "_momentum", this, [this]() -> const sva::ForceVecd & { return tvm_error(errorT)->algo().momentum(); }); + logger.addLogEntry(name_ + "_momentum dot", this, + [this]() -> const sva::ForceVecd & { return tvm_error(errorT)->algo().velocity(); }); break; default: break; diff --git a/src/mc_tvm/DynamicFunction.cpp b/src/mc_tvm/DynamicFunction.cpp index 9023bd0922..924e43e019 100644 --- a/src/mc_tvm/DynamicFunction.cpp +++ b/src/mc_tvm/DynamicFunction.cpp @@ -4,6 +4,7 @@ #include +#include #include #include @@ -39,6 +40,9 @@ DynamicFunction::ForceContact::ForceContact(const mc_rbdyn::RobotFrame & frame, void DynamicFunction::ForceContact::updateJacobians(DynamicFunction & parent) { + /* Note: in this original ForceContact formulation the jacobian chosen for the force is the bodyJacobian + This means the force variables are chosen to be in body frame, and they are translated for each point in body frame. + */ const auto & robot = frame_->robot(); const auto & bodyJac = jac_.bodyJacobian(robot.mb(), robot.mbc()); for(int i = 0; i < forces_.numberOfVariables(); ++i) @@ -64,35 +68,185 @@ sva::ForceVecd DynamicFunction::ForceContact::force() const return ret; } -const tvm::VariableVector & DynamicFunction::addContact(const mc_rbdyn::RobotFrame & frame, - std::vector points, - double dir) +DynamicFunction::WrenchContact::WrenchContact(const mc_rbdyn::RobotFrame & frame, mc_rbdyn::Contact & contact) +: frame_(frame), contact_(&contact), hasVariable_(true), jac_(frame.tvm_frame().rbdJacobian()), + blocks_(jac_.compactPath(frame.robot().mb())), full_jac_(6, frame.robot().mb().nrDof()) +{ + wrench_ = tvm::Space(6).createVariable("wrench " + frame.name()); + wrench_->setZero(); +} + +DynamicFunction::WrenchContact::WrenchContact(const mc_rbdyn::RobotFrame & frame, + const tvm::VariablePtr & wrench, + mc_rbdyn::Contact & contact) +: frame_(frame), wrench_(wrench), contact_(&contact), hasVariable_(false), jac_(frame.tvm_frame().rbdJacobian()), + blocks_(jac_.compactPath(frame.robot().mb())), full_jac_(6, frame.robot().mb().nrDof()) +{ +} + +void DynamicFunction::WrenchContact::updateWrenchJacobian(DynamicFunction & parent) +{ + const auto & robot = frame_->robot(); + + /* IMPORTANT: in the original ForceContact formulation the jacobian chosen for the force is the bodyJacobian + This means the force variables are chosen to be in body frame, and they are translated for each point in body frame. + Here we would like the variables in contact frame so that they can be directly reused easily (for example + in the CoMWrenchTransforms_) so we use the jacobian at the contact frame instead. + */ + + const auto X_0_contact = frame_->position(); + const auto & contactJac = jac_.jacobian(robot.mb(), robot.mbc(), X_0_contact); + full_jac_.setZero(); + // In surface contact representation, the wrench variable's jacobian is just the contact frame full jac transposed + jac_.addFullJacobian(blocks_, contactJac, full_jac_); + + if(hasVariable_) + { + parent.jacobian_[wrench_.get()].noalias() = -full_jac_.block(0, 0, 6, robot.mb().nrDof()).transpose(); + // Update transform map + // FIXME make sure the frame AND the com are up to date and use the tvm versions + auto X_0_f = frame_->position(); + auto X_0_C = sva::PTransformd(frame_->robot().com()); + auto X_f_C = (X_0_C * X_0_f.inv()); + parent.CoMWrenchTransforms_[wrench_] = X_f_C; + } + else + { + // We don't own the var so this is the second robot with a dynamics function in the contact. + // The jacobian to the wrench var must be transformed from the other frame to this one, and negated + // This way the wrench applied in the second contact frame results in - the first wrench if expressed in the + // first contact frame + + // Getting the right transform: if this robot is r1, the var is in frame r2 so we need X_r2_r1 + const auto X_var_f = + contact_->r1Surface()->name() == frame_->name() ? contact_->X_r2s_r1s() : contact_->X_r2s_r1s().inv(); + + parent.jacobian_[wrench_.get()].noalias() = + -full_jac_.block(0, 0, 6, robot.mb().nrDof()).transpose() * -X_var_f.dualMatrix(); + // Then we simply emplace the transform between the variable frame and this robot's CoM, + // taking the opposite into account + // FIXME make sure the frame AND the com are up to date and use the tvm versions + auto X_0_f = frame_->position(); + auto X_0_C = sva::PTransformd(frame_->robot().com()); + // This transforms to the negative frame + auto X_negative = sva::PTransformd(-Eigen::Matrix3d::Identity(), Eigen::Vector3d::Zero()); + + auto X_var_C = X_negative * X_0_C * X_0_f.inv() * X_var_f; + parent.CoMWrenchTransforms_[wrench_] = X_var_C; + } + // mc_rtc::log::critical("updated wrench jacobian to {}", parent.jacobian_[wrench_.get()]); +} + +sva::ForceVecd DynamicFunction::WrenchContact::wrench() const +{ + auto ret = sva::ForceVecd(wrench_->value()); + return ret; +} + +const tvm::VariableVector & DynamicFunction::addContact3d(const mc_rbdyn::RobotFrame & frame, + std::vector points, + double dir) { if(frame.robot().name() != robot_.name()) { mc_rtc::log::error_and_throw( "Attempted to add a contact for {} to dynamic function belonging to {}", frame.robot().name(), robot_.name()); } - auto & fc = contacts_.emplace_back(frame, std::move(points), dir); + // Constructs a ForceContact emplaced in contactForces_ (creates force variables tvm::Space(3)) + auto & fc = contactForces_.emplace_back(frame, std::move(points), dir); for(const auto & var : fc.forces_) { addVariable(var, true); } + // Adds dep to call a jacobian update on this function each time the frame jacobian is updated addInputDependency(Update::Jacobian, frame.tvm_frame(), mc_tvm::RobotFrame::Output::Jacobian); return fc.forces_; } +const tvm::VariablePtr & DynamicFunction::addContact6d(const mc_rbdyn::RobotFrame & frame, mc_rbdyn::Contact & contact) +{ + if(frame.robot().name() != robot_.name()) + { + mc_rtc::log::error_and_throw( + "Attempted to add a contact for {} to dynamic function belonging to {}", frame.robot().name(), robot_.name()); + } + // Constructs a WrenchContact emplaced in contactWrenches_ (creates wrench variable tvm::Space(6)) + auto & wc = contactWrenches_.emplace_back(frame, contact); + addVariable(wc.wrench_, true); + // add this variable to the dual wrenches map (it was created on this side) + auto X_0_f = frame.position(); + auto X_0_C = sva::PTransformd(robot_.tvmRobot().comAlgo().com()); + auto X_f_C = (X_0_C * X_0_f.inv()); + CoMWrenchTransforms_.emplace(wc.wrench_, X_f_C); + // Adds dep to call a jacobian update on this function each time the frame jacobian is updated + // FIXME Add a dependency on the contact instead of the frame + addInputDependency(Update::Jacobian, frame.tvm_frame(), mc_tvm::RobotFrame::Output::Jacobian); + return wc.wrench_; +} + +void DynamicFunction::addContact6d(const mc_rbdyn::RobotFrame & frame, + const tvm::VariablePtr & variable, + mc_rbdyn::Contact & contact) +{ + if(frame.robot().name() != robot_.name()) + { + mc_rtc::log::error_and_throw( + "Attempted to add a contact for {} to dynamic function belonging to {}", frame.robot().name(), robot_.name()); + } + // Constructs a WrenchContact emplaced in contactWrenches_ and set its variable to the given one + auto & wc = contactWrenches_.emplace_back(frame, variable, contact); + addVariable(wc.wrench_, true); + // add this variable to the dual wrenches map (it was created by the other frame of the contact) + auto X_0_f = frame.position(); + auto X_0_C = sva::PTransformd(robot_.tvmRobot().comAlgo().com()); + // Getting the transform from variable to this robot's frame: if this robot is r1, + // the var is in frame r2 so we need X_r2_r1, otherwise X_r1_r2 + // After this, since the given variable is opposite to this one (Newton) we negate the wrench value in the transform + auto X_negative = sva::PTransformd(-Eigen::Matrix3d::Identity(), Eigen::Vector3d::Zero()); + const auto X_var_f = contact.r1Surface()->name() == frame.name() ? contact.X_r2s_r1s() : contact.X_r2s_r1s().inv(); + auto X_var_C = X_0_C * X_0_f.inv() * X_negative * X_var_f; + CoMWrenchTransforms_.emplace(wc.wrench_, X_var_C); + // Adds dep to call a jacobian update on this function each time the frame jacobian is updated + // FIXME Add a dependency on the contact instead of the frame, this way if the second frame is updated, this jacobian + // will be too + addInputDependency(Update::Jacobian, frame.tvm_frame(), mc_tvm::RobotFrame::Output::Jacobian); +} + void DynamicFunction::removeContact(const mc_rbdyn::RobotFrame & frame) { - auto it = findContact(frame); - if(it != contacts_.end()) + // Find if this contact corresponds to a 3d or 6d var and remove it + auto it = findContactForce(frame); + if(it != contactForces_.end()) { for(const auto & var : it->forces_) { removeVariable(var); } - contacts_.erase(it); + contactForces_.erase(it); + } + + auto it2 = findContactWrench(frame); + if(it2 != contactWrenches_.end()) + { + CoMWrenchTransforms_.erase(it2->wrench_); + removeVariable(it2->wrench_); + contactWrenches_.erase(it2); } } sva::ForceVecd DynamicFunction::contactForce(const mc_rbdyn::RobotFrame & frame) const { - auto it = findContact(frame); - if(it != contacts_.end()) { return (*it).force(); } + auto it = findContactForce(frame); + auto it2 = findContactWrench(frame); + if(it != contactForces_.end()) { return (*it).force(); } + else if(it2 != contactWrenches_.end()) + { + // Check if need to return the wrench var as is or transform var (other side) + if(it2->hasVariable_) { return (*it2).wrench(); } + else + { + // getting the right transform: if this robot is r1, the var is in frame r2 so we need X_r2_r1 + // if this is r2, variable is w1 so we need X_r1_r2 + const auto dualMatrix = it2->contact_->r1Surface()->name() == it2->frame_->name() + ? it2->contact_->X_r2s_r1s().dualMatrix() + : it2->contact_->X_r2s_r1s().inv().dualMatrix(); + return sva::ForceVecd(-dualMatrix * it2->wrench().vector()); + } + } else { mc_rtc::log::error("No contact at frame {} in dynamic function for {}", frame.name(), robot_.name()); @@ -100,6 +254,25 @@ sva::ForceVecd DynamicFunction::contactForce(const mc_rbdyn::RobotFrame & frame) } } +const tvm::VariableVector DynamicFunction::getForceVariables(const std::string & contactFrameName) +{ + tvm::VariableVector variables; + auto it = findContactForce(robot_.frame(contactFrameName)); + if(it != contactForces_.end()) + { + // We found this frame in the active contacts for this robot + variables = (*it).forces_; + } + auto it2 = findContactWrench(robot_.frame(contactFrameName)); + if(it2 != contactWrenches_.end()) + { + // We found this frame in the active contacts for this robot + variables.add((*it2).wrench_); + } + + return variables; +} + void DynamicFunction::updateb() { b_ = robot_.tvmRobot().C(); @@ -109,12 +282,23 @@ void DynamicFunction::updateJacobian() { const auto & robot = robot_.tvmRobot(); splitJacobian(robot.H(), robot.alphaD()); - for(auto & c : contacts_) { c.updateJacobians(*this); } + // update jacobians for every 3D contact and every 6D contact + for(auto & c : contactForces_) { c.updateJacobians(*this); } + for(auto & c : contactWrenches_) { c.updateWrenchJacobian(*this); } +} + +auto DynamicFunction::findContactForce(const mc_rbdyn::RobotFrame & frame) const + -> std::vector::const_iterator +{ + return std::find_if(contactForces_.begin(), contactForces_.end(), + [&](const auto & c) { return c.frame_.get() == &frame; }); } -auto DynamicFunction::findContact(const mc_rbdyn::RobotFrame & frame) const -> std::vector::const_iterator +auto DynamicFunction::findContactWrench(const mc_rbdyn::RobotFrame & frame) const + -> std::vector::const_iterator { - return std::find_if(contacts_.begin(), contacts_.end(), [&](const auto & c) { return c.frame_.get() == &frame; }); + return std::find_if(contactWrenches_.begin(), contactWrenches_.end(), + [&](const auto & c) { return c.frame_.get() == &frame; }); } } // namespace mc_tvm diff --git a/src/mc_tvm/FeasiblePolytope.cpp b/src/mc_tvm/FeasiblePolytope.cpp new file mode 100644 index 0000000000..f5203483c1 --- /dev/null +++ b/src/mc_tvm/FeasiblePolytope.cpp @@ -0,0 +1,107 @@ +/* + * Copyright 2015-2022 CNRS-UM LIRMM, CNRS-AIST JRL + */ + +#include + +namespace mc_tvm +{ + +FeasiblePolytope::FeasiblePolytope(NewPolytopeToken, const mc_rbdyn::Contact & contact, const int & rIndex) +: contact_(contact), rIndex_(rIndex) +{ + registerUpdates(Update::Polytope, &FeasiblePolytope::updatePolytope); + + // This makes it so that to update the polytope value, it will fetch the rbdyn contact polytope + addOutputDependency(Output::Polytope, Update::Polytope); + // addInputDependency(Update::Value, tvm_frame, Frame::Output::Position); + + // Updating values from the start + updatePolytope(); +} + +void FeasiblePolytope::updatePolytope() +{ + // Use given robot index to know if this poly corresponds to r1 or r2 of the contact + bool isR1 = contact_.r1Index() == rIndex_; + std::optional feasiblePolytope; + if(isR1) { feasiblePolytope = contact_.feasiblePolytopeR1(); } + else + { + feasiblePolytope = contact_.feasiblePolytopeR2(); + } + + if(feasiblePolytope) + { + // If there is a feasible polytope, build the contact wrench polytope from the wrench face matrix and + // the feasible polytope + // The moments are the 3 first columns, the forces the 3 next + // Nb of lines: force poly constraints + 12 surface constraints (4 CoP, 8 yaw torque) + // Note: the 12 surface constraints assume a rectangular surface ! + int nbPolyConstraints = feasiblePolytope->planeConstants.size(); + int nbSurfaceConstraints = 12; + + normals_ = Eigen::MatrixXd::Zero(nbPolyConstraints + nbSurfaceConstraints, 6); + offsets_ = Eigen::VectorXd::Zero(nbPolyConstraints + nbSurfaceConstraints); + + // Fill polytope in force part only if it's a force polytope, in full matrix if it's a wrench polytope + if(feasiblePolytope->planeNormals.cols() == 3) + { + normals_.block(0, 3, nbPolyConstraints, 3) = feasiblePolytope->planeNormals; + } + else if(feasiblePolytope->planeNormals.cols() == 6) + { + normals_.block(0, 0, nbPolyConstraints, 6) = feasiblePolytope->planeNormals; + } + else + { + mc_rtc::log::error("[TVM Feasible Polytope]: Invalid polytope for contact frame {}", + isR1 ? contact_.r1Surface()->name() : contact_.r2Surface()->name()); + } + + // For surface constraints use the correct robot's surface + if(isR1) + { + normals_.block(nbPolyConstraints, 0, nbSurfaceConstraints, 6) = + computeSurfaceTorqueConstraint(*contact_.r1Surface(), contact_.friction()); + } + else + { + normals_.block(nbPolyConstraints, 0, nbSurfaceConstraints, 6) = + computeSurfaceTorqueConstraint(*contact_.r2Surface(), contact_.friction()); + } + + offsets_.segment(0, nbPolyConstraints) = feasiblePolytope->planeConstants; + // Offsets for wrench face matrix are just zero, nothing else to do + // mc_rtc::log::warning("Feasible Poly: feasible detected, built as:\n{}\n and {}", normals_, offsets_.transpose()); + } + else + { + // There is no feasible polytope, build a default one with just friction cone and surface matrix + // Note: the 12 surface constraints assume a rectangular surface ! + int nbFrictionSides = 5; + int nbSurfaceConstraints = 12; + + normals_ = Eigen::MatrixXd::Zero(nbFrictionSides + nbSurfaceConstraints, 6); + offsets_ = Eigen::VectorXd::Zero(nbFrictionSides + nbSurfaceConstraints); + // Translational friction cones + normals_.block(0, 3, nbFrictionSides, 3) = + generatePolyhedralConeHRep(nbFrictionSides, Eigen::Matrix3d::Identity(), contact_.friction()); + // Surface constraints + if(isR1) // Use correct robot's surface + { + normals_.block(nbFrictionSides, 0, nbSurfaceConstraints, 6) = + computeSurfaceTorqueConstraint(*contact_.r1Surface(), contact_.friction()); + } + else + { + normals_.block(nbFrictionSides, 0, nbSurfaceConstraints, 6) = + computeSurfaceTorqueConstraint(*contact_.r2Surface(), contact_.friction()); + } + + // Offsets are all zero in this default case (no second member if unbound polyhedral cone) + // mc_rtc::log::warning("Feasible Poly: no feasible detected, built default as:\n{}", normals_); + } +} + +} // namespace mc_tvm diff --git a/src/mc_tvm/ForceInPolytopeFunction.cpp b/src/mc_tvm/ForceInPolytopeFunction.cpp new file mode 100644 index 0000000000..1a687a2fac --- /dev/null +++ b/src/mc_tvm/ForceInPolytopeFunction.cpp @@ -0,0 +1,119 @@ +/* + * Copyright 2015-2022 CNRS-UM LIRMM, CNRS-AIST JRL + */ + +#include + +#include + +namespace mc_tvm +{ + +ForceInPolytopeFunction::ForceInPolytopeFunction(const mc_rbdyn::Contact & contact, + const tvm::VariableVector & forceVars, + const int & rIndex, + const bool & hasForceVar) +: tvm::function::abstract::LinearFunction(0), contact_(contact), rIndex_(rIndex), hasForceVar_(hasForceVar), + tvmPoly_(rIndex == contact.r1Index() ? contact.tvmPolytopeR1() : contact.tvmPolytopeR2()), forceVars_(forceVars), + constraintSizeChanged_(false) +{ + registerUpdates(Update::Jacobian, &ForceInPolytopeFunction::updateJacobian); + registerUpdates(Update::B, &ForceInPolytopeFunction::updateb); + registerUpdates(Update::Resize, &ForceInPolytopeFunction::resizeToPoly); + + addOutputDependency(Output::Jacobian, Update::Jacobian); + addOutputDependency(Output::B, Update::B); + + // Adding all variables in the vector + addVariable(forceVars, true); + + // Make sure the function has the right dimension + addInternalDependency(Update::Jacobian, Update::Resize); + addInternalDependency(Update::B, Update::Resize); + + // Updating Jacobian and B (and resizing) depends on updating the polytope + addInputDependency(Update::Resize, tvmPoly_, FeasiblePolytope::Output::Polytope); + + // Resizing dimension of the function to number of polytope planes (must be done every new polytope) + tvmPoly_.updatePolytope(); + resize(tvmPoly_.offsets().size()); + updateJacobian(); + updateb(); +} + +void ForceInPolytopeFunction::updateJacobian() +{ + // The function must bound the total of forces acting on this contact in the contact polytope + // If the force vars dimensions are 3d, then we bound in the force part of the polytope + // If the var dimension is 6d, we use the full wrench polytope + for(const auto & forceVar : forceVars_) + { + if(forceVar->space().size() == 3) + { + // The force only normals are the right 3 columns, minus bottom 12 rows (CoP + yaw torque) + int nbOfForceConstraints = tvmPoly_.offsets().size() - 12; + // FIXME Not handling other side variable in force case (very different logic, + // all force vars should be associated to another etc) + jacobian_[forceVar.get()] = tvmPoly_.normals().block(0, 3, nbOfForceConstraints, 3); + } + else if(forceVar->space().size() == 6) + { + if(hasForceVar_) { jacobian_[forceVar.get()] = tvmPoly_.normals(); } + else + { + // if the force var was created for the other side we need to multiply the var by minus the + // dual plücker transform matrix (manipulating a force vec) to get the wrench for this side + + // getting the right transform: if this robot is r1, the var is in frame r2 so we need X_r2_r1 + const auto dualMat = + contact_.r1Index() == rIndex_ ? contact_.X_r2s_r1s().dualMatrix() : contact_.X_r2s_r1s().inv().dualMatrix(); + jacobian_[forceVar.get()] = tvmPoly_.normals() * -dualMat; + } + } + // Not handling other dimensions + // mc_rtc::log::critical("Jacobian task {} updated correctly to\n{}", forceVars_.indexOf(*forceVar.get()), + // jacobian_[forceVar.get()]); + } +} + +void ForceInPolytopeFunction::updateb() +{ + // Since the linear function expects format of Ax + b /operator/ rhs + // the b member is minus the offsets + + if(forceVars_[0]->space().size() == 3) // check done on 1st var only, we assume all vars have same dim + { + // If the vars are force only, remove the last 12 elements of offsets (CoP + yaw torque) + int nbOfForceConstraints = tvmPoly_.offsets().size() - 12; + b_ = -tvmPoly_.offsets().segment(0, nbOfForceConstraints); + } + else if(forceVars_[0]->space().size() == 6) { b_ = -tvmPoly_.offsets(); } + // mc_rtc::log::critical("b task updated correctly to {}", b_.transpose()); +} + +void ForceInPolytopeFunction::resizeToPoly() +{ + // If polytope number of planes changed, update the task dimension + // The dimension is the number of constraints : it is both the jacobian rows and the output space + if(forceVars_[0]->space().size() == 3) + { + // If we manipulate forces only, we don't need the 6D related constraint (CoP, torsional friction) + if(imageSpace().size() != tvmPoly_.offsets().size() - 12) + { + // mc_rtc::log::warning("Image space changed from {} to {}", imageSpace().size(), tvmPoly_.offsets().size() - 12); + resize(tvmPoly_.offsets().size() - 12); + constraintSizeChanged_ = true; + } + } + else if(forceVars_[0]->space().size() == 6) + { + if(imageSpace().size() != tvmPoly_.offsets().size()) + { + // mc_rtc::log::warning("Image space changed from {} to {}", imageSpace().size(), tvmPoly_.offsets().size()); + resize(tvmPoly_.offsets().size()); + constraintSizeChanged_ = true; + } + } +} + +} // namespace mc_tvm diff --git a/src/mc_tvm/Momentum.cpp b/src/mc_tvm/Momentum.cpp index f98d91fd68..d1c12c3386 100644 --- a/src/mc_tvm/Momentum.cpp +++ b/src/mc_tvm/Momentum.cpp @@ -14,6 +14,7 @@ Momentum::Momentum(NewMomentumToken, CoM & com) : com_(com), mat_(robot().robot( // clang-format off registerUpdates( Update::Momentum, &Momentum::updateMomentum, + Update::Velocity, &Momentum::updateVelocity, Update::Jacobian, &Momentum::updateJacobian, Update::NormalAcceleration, &Momentum::updateNormalAcceleration, Update::JDot, &Momentum::updateJDot); @@ -23,6 +24,10 @@ Momentum::Momentum(NewMomentumToken, CoM & com) : com_(com), mat_(robot().robot( addInputDependency(Update::Momentum, robot(), Robot::Output::FK); addInputDependency(Update::Momentum, com_, CoM::Output::CoM); + addOutputDependency(Output::Velocity, Update::Velocity); + addInputDependency(Update::Velocity, com_, CoM::Output::CoM); + addInputDependency(Update::Velocity, com_, CoM::Output::Velocity); + addOutputDependency(Output::Jacobian, Update::Jacobian); addInputDependency(Update::Jacobian, robot(), Robot::Output::FV); addInputDependency(Update::Jacobian, com_, CoM::Output::CoM); @@ -49,6 +54,12 @@ void Momentum::updateMomentum() momentum_ = rbd::computeCentroidalMomentum(r.mb(), r.mbc(), com_.com()); } +void Momentum::updateVelocity() +{ + const auto & r = robot().robot(); + velocity_ = rbd::computeCentroidalMomentumDot(r.mb(), r.mbc(), com_.com(), com_.velocity()); +} + void Momentum::updateNormalAcceleration() { const auto & r = robot().robot(); diff --git a/src/mc_tvm/MomentumFunction.cpp b/src/mc_tvm/MomentumFunction.cpp index 1a2ba49c6f..ad4afa611e 100644 --- a/src/mc_tvm/MomentumFunction.cpp +++ b/src/mc_tvm/MomentumFunction.cpp @@ -28,8 +28,13 @@ MomentumFunction::MomentumFunction(const mc_rbdyn::Robot & robot) addOutputDependency(Output::Jacobian, Update::Jacobian); addOutputDependency(Output::NormalAcceleration, Update::NormalAcceleration); addOutputDependency(Output::JDot, Update::JDot); + // FIXME The variable for the momentum should be dot(q), but the way TVM task dynamics are implemented + // relies on the derivative order of the variables. If dot(q) is given, the dynamics will simply be + // a proportional. addVariable(robot.tvmRobot().q(), false); addInputDependency(Update::Value, momentumAlgo_, mc_tvm::Momentum::Output::Momentum); + // Still add momentum velocity dependency so that it is computed in the graph for logging + addInputDependency(Update::Velocity, momentumAlgo_, mc_tvm::Momentum::Output::Velocity); addInputDependency(Update::Jacobian, momentumAlgo_, mc_tvm::Momentum::Output::Jacobian); addInputDependency(Update::NormalAcceleration, momentumAlgo_, mc_tvm::Momentum::Output::NormalAcceleration); @@ -50,6 +55,8 @@ void MomentumFunction::updateValue() void MomentumFunction::updateVelocity() { + // Giving -refVel as velocity error and not momentumAlgo_.velocity() - refVel_ + // because it's a feedforward (momentum is already a velocity) velocity_ = -refVel_; } diff --git a/src/mc_tvm/PostureFunction.cpp b/src/mc_tvm/PostureFunction.cpp index 0738cdb6ee..8338b6c16f 100644 --- a/src/mc_tvm/PostureFunction.cpp +++ b/src/mc_tvm/PostureFunction.cpp @@ -15,6 +15,8 @@ PostureFunction::PostureFunction(const mc_rbdyn::Robot & robot) j0_(robot_.mb().joint(0).type() == rbd::Joint::Free ? 1 : 0), refVel_(Eigen::VectorXd::Zero(size())), refAccel_(Eigen::VectorXd::Zero(size())) { + registerUpdates(Update::Value, &PostureFunction::updateValue_); + registerUpdates(Update::Velocity, &PostureFunction::updateVelocity_); // For mbc.jointConfig addInputDependency(Update::Value, robot.tvmRobot(), mc_tvm::Robot::Output::FK); diff --git a/tests/controllers/TestFSMStateOptions.cpp b/tests/controllers/TestFSMStateOptions.cpp index 3a9592b870..fb5067848c 100644 --- a/tests/controllers/TestFSMStateOptions.cpp +++ b/tests/controllers/TestFSMStateOptions.cpp @@ -58,8 +58,9 @@ struct MC_CONTROL_DLLAPI TestFSMStateOptionsController : public fsm::Controller { BOOST_REQUIRE(executor_.state() == "TestContactsManipulation2"); BOOST_REQUIRE(solver().contacts().size() == 2); - for(const auto & c : solver().contacts()) + for(const auto & contacts : solver().contacts()) { + const auto & c = *contacts; if(c.r1Surface()->name() == "RightFootCenter") { BOOST_REQUIRE(c.dof() == dof); } } }