Consider kinematics closed loop

src/CMakeLists.txt

@@ -13,6 +13,7 @@ set(SOURCES
     GenQPSolver.cpp
     QPContactConstr.cpp
     QLDQPSolver.cpp
+    QPCoincidenceConstr.cpp
 )
 set(HEADERS
     Tasks/Tasks.h
@@ -25,6 +26,7 @@ set(HEADERS
     Tasks/GenQPSolver.h
     Tasks/Bounds.h
     Tasks/QPContactConstr.h
+    Tasks/QPCoincidenceConstr.h
 )
 set(PRIVATE_HEADERS utils.h GenQPUtils.h QLDQPSolver.h)
 

src/QPCoincidenceConstr.cpp

@@ -0,0 +1,193 @@
+#include "Tasks/QPCoincidenceConstr.h"
+
+// RBDyn
+#include <RBDyn/Jacobian.h>
+#include <RBDyn/MultiBody.h>
+#include <RBDyn/MultiBodyConfig.h>
+
+// Tasks
+#include "utils.h"
+
+namespace tasks
+{
+namespace qp
+{
+
+const Eigen::MatrixXd & CoincidenceConstr::AEq() const
+{
+  return A_;
+}
+
+const Eigen::VectorXd & CoincidenceConstr::bEq() const
+{
+  return b_;
+}
+
+int CoincidenceConstr::maxEq() const
+{
+  return int(A_.rows());
+}
+
+void CoincidenceConstr::setJointSelector(const Eigen::VectorXd & selector)
+{
+  jointSelector_ = selector;
+}
+
+// =================== FixedCoincidenceConstr ===================
+
+FixedCoincidenceConstr::FixedCoincidenceConstr(int robotIndex,
+                                               const std::string & body1Name,
+                                               const std::string & body2Name,
+                                               const Eigen::Vector3d & point1,
+                                               const Eigen::Vector3d & point2,
+                                               const Eigen::VectorXd & jointSelector)
+: CoincidenceConstr(robotIndex, body1Name, body2Name, jointSelector), point1_(point1), point2_(point2)
+{
+  A_.resize(0, 0);
+  b_.resize(0);
+}
+
+std::string FixedCoincidenceConstr::nameEq() const
+{
+  return "FixedCoincidenceConstr";
+}
+
+std::string FixedCoincidenceConstr::descEq(const std::vector<rbd::MultiBody> & mbs, int i)
+{
+  return "FixedCoincidenceConstr between " + body1Name_ + " and " + body2Name_ + ".";
+}
+
+void FixedCoincidenceConstr::updateNrVars(const std::vector<rbd::MultiBody> & mbs, const SolverData & data)
+{
+  const rbd::MultiBody & mb = mbs[robotIndex_];
+  body1Index_ = mb.bodyIndexByName(body1Name_);
+  body2Index_ = mb.bodyIndexByName(body2Name_);
+  if(body1Index_ == -1 || body2Index_ == -1)
+  {
+    // Error: Invalid body indices in FixedCoincidenceConstr::update
+    return;
+  }
+
+  A_.setZero(6, data.nrVars());
+  b_.setZero(6);
+}
+
+void FixedCoincidenceConstr::update(const std::vector<rbd::MultiBody> & mbs,
+                                    const std::vector<rbd::MultiBodyConfig> & mbcs,
+                                    const SolverData & data)
+{
+  using namespace Eigen;
+  const rbd::MultiBody & mb = mbs[robotIndex_];
+  const rbd::MultiBodyConfig & mbc = mbcs[robotIndex_];
+  if(body1Index_ == -1 || body2Index_ == -1)
+  {
+    // Error: Invalid body indices in FixedCoincidenceConstr::update
+    return;
+  }
+
+  int alphaDBegin = data.alphaDBegin(robotIndex_);
+  int nrDof = mb.nrDof();
+
+  rbd::Jacobian jac1(mb, body1Name_);
+  Eigen::MatrixXd jacMat1 = jac1.jacobian(mb, mbc);
+  Eigen::MatrixXd fulljacobian1(6, nrDof);
+  jac1.fullJacobian(mb, jacMat1.block(0, 0, 6, nrDof), fulljacobian1);
+
+  rbd::Jacobian jac2(mb, body2Name_);
+  Eigen::MatrixXd jacMat2 = jac2.jacobian(mb, mbc);
+  Eigen::MatrixXd fulljacobian2(6, nrDof);
+  jac2.fullJacobian(mb, jacMat2.block(0, 0, 6, nrDof), fulljacobian2);
+
+  A_.block(0, alphaDBegin, 6, nrDof) = (fulljacobian1 - fulljacobian2) * jointSelector_.asDiagonal();
+
+  const std::vector<sva::MotionVecd> & normalAccB = data.normalAccB(robotIndex_);
+  Vector6d normalAcc1 = jac1.normalAcceleration(mb, mbc, normalAccB).vector();
+  Vector6d normalAcc2 = jac2.normalAcceleration(mb, mbc, normalAccB).vector();
+
+  double Kp = 1;
+  double Kd = 2;
+  Eigen::VectorXd errorp = mbc.bodyPosW[body1Index_].translation() - mbc.bodyPosW[body2Index_].translation();
+  Eigen::VectorXd errord = mbc.bodyVelW[body1Index_].linear() - mbc.bodyVelW[body2Index_].linear();
+
+  b_.segment(0, 6) = -(normalAcc1.tail(6) - normalAcc2.tail(6)) - Kp * errorp - Kd * errord;
+}
+
+// =============== RotationalCoincidenceConstr ===============
+
+RotationalCoincidenceConstr::RotationalCoincidenceConstr(int robotIndex,
+                                                         const std::string & body1Name,
+                                                         const std::string & body2Name,
+                                                         const Eigen::VectorXd & jointSelector)
+: CoincidenceConstr(robotIndex, body1Name, body2Name, jointSelector)
+{
+  A_.resize(0, 0);
+  b_.resize(0);
+}
+
+std::string RotationalCoincidenceConstr::nameEq() const
+{
+  return "RotationalCoincidenceConstr";
+}
+
+std::string RotationalCoincidenceConstr::descEq(const std::vector<rbd::MultiBody> & mbs, int i)
+{
+  return "RotationalCoincidenceConstr between " + body1Name_ + " and " + body2Name_ + ".";
+}
+
+void RotationalCoincidenceConstr::updateNrVars(const std::vector<rbd::MultiBody> & mbs, const SolverData & data)
+{
+  const rbd::MultiBody & mb = mbs[robotIndex_];
+  body1Index_ = mb.bodyIndexByName(body1Name_);
+  body2Index_ = mb.bodyIndexByName(body2Name_);
+  if(body1Index_ == -1 || body2Index_ == -1)
+  {
+    // Error: Invalid body indices in RotationalCoincidenceConstr::update
+    return;
+  }
+
+  A_.setZero(3, data.nrVars());
+  b_.setZero(3);
+}
+
+void RotationalCoincidenceConstr::update(const std::vector<rbd::MultiBody> & mbs,
+                                         const std::vector<rbd::MultiBodyConfig> & mbcs,
+                                         const SolverData & data)
+{
+  using namespace Eigen;
+  const rbd::MultiBody & mb = mbs[robotIndex_];
+  const rbd::MultiBodyConfig & mbc = mbcs[robotIndex_];
+  if(body1Index_ == -1 || body2Index_ == -1)
+  {
+    // Error: Invalid body indices in RotationalCoincidenceConstr::update
+    return;
+  }
+
+  int alphaDBegin = data.alphaDBegin(robotIndex_);
+  int nrDof = mb.nrDof();
+
+  rbd::Jacobian jac1(mb, body1Name_);
+  Eigen::MatrixXd jacMat1 = jac1.jacobian(mb, mbc);
+  Eigen::MatrixXd fulljacobian1(3, nrDof);
+  jac1.fullJacobian(mb, jacMat1.block(3, 0, 3, nrDof), fulljacobian1);
+
+  rbd::Jacobian jac2(mb, body2Name_);
+  Eigen::MatrixXd jacMat2 = jac2.jacobian(mb, mbc);
+  Eigen::MatrixXd fulljacobian2(3, nrDof);
+  jac2.fullJacobian(mb, jacMat2.block(3, 0, 3, nrDof), fulljacobian2);
+
+  A_.block(0, alphaDBegin, 3, nrDof) = (fulljacobian1 - fulljacobian2) * jointSelector_.asDiagonal();
+
+  const std::vector<sva::MotionVecd> & normalAccB = data.normalAccB(robotIndex_);
+  Vector6d normalAcc1 = jac1.normalAcceleration(mb, mbc, normalAccB).vector();
+  Vector6d normalAcc2 = jac2.normalAcceleration(mb, mbc, normalAccB).vector();
+
+  double Kp = 1;
+  double Kd = 2;
+  Eigen::VectorXd errorp = mbc.bodyPosW[body1Index_].translation() - mbc.bodyPosW[body2Index_].translation();
+  Eigen::VectorXd errord = mbc.bodyVelW[body1Index_].linear() - mbc.bodyVelW[body2Index_].linear();
+
+  b_.segment(0, 3) = -(normalAcc1.tail(3) - normalAcc2.tail(3)) - Kp * errorp - Kd * errord;
+}
+
+} // namespace qp
+} // namespace tasks

src/Tasks/QPCoincidenceConstr.h

@@ -0,0 +1,154 @@
+/*
+ * Copyright 2012-2019 CNRS-UM LIRMM, CNRS-AIST JRL
+ */
+
+#pragma once
+
+// includes
+// std
+#include <vector>
+
+// Eigen
+#include <Eigen/Core>
+
+// Tasks
+#include "QPSolver.h"
+
+namespace tasks
+{
+
+namespace qp
+{
+
+/**
+ * Base class for coincidence equality constraints.
+ *
+ * This constraint enforces acceleration-level coincidence between two bodies
+ * of the same robot using their Jacobians and normal accelerations.
+ *
+ * A proportional-derivative (PD) stabilization term is added in the update step
+ * to reduce position and velocity errors between the bodies.
+ *
+ * Derived classes define whether the coincidence applies to translation,
+ * rotation, or both.
+ */
+class TASKS_DLLAPI CoincidenceConstr : public ConstraintFunction<Equality>
+{
+public:
+  /**
+   * @param robotIndex Constrained robot Index in mbs.
+   * @param body1Name Name of the first body.
+   * @param body2Name Name of the second body.
+   * @param jointSelector Joint selection vector.
+   */
+  CoincidenceConstr(int robotIndex,
+                    const std::string & body1Name,
+                    const std::string & body2Name,
+                    const Eigen::VectorXd & jointSelector)
+  : robotIndex_(robotIndex), body1Name_(body1Name), body2Name_(body2Name), body1Index_(-1), body2Index_(-1),
+    jointSelector_(jointSelector)
+  {
+  }
+
+  // Equality constraint
+  virtual int maxEq() const override;
+
+  virtual const Eigen::MatrixXd & AEq() const override;
+
+  virtual const Eigen::VectorXd & bEq() const override;
+
+  /**
+   * Set the joint selector.
+   * @param selector Joint selection vector.
+   */
+  void setJointSelector(const Eigen::VectorXd & selector);
+
+protected:
+  Eigen::MatrixXd A_;
+  Eigen::VectorXd b_;
+  int robotIndex_;
+  std::string body1Name_, body2Name_;
+  int body1Index_, body2Index_;
+  Eigen::VectorXd jointSelector_;
+};
+
+/**
+ * Enforce translational coincidence between two bodies.
+ *
+ * This constraint enforces equality of linear accelerations between two bodies
+ * (6D formulation), using the difference of their full Jacobians.
+ *
+ * A PD stabilization term is added based on the difference of body world
+ * positions and linear velocities.
+ */
+class TASKS_DLLAPI FixedCoincidenceConstr : public CoincidenceConstr
+{
+public:
+  /**
+   * @param robotIndex Constrained robot Index in mbs.
+   * @param body1Name Name of the first body.
+   * @param body2Name Name of the second body.
+   * @param point1 Unused (kept for API compatibility).
+   * @param point2 Unused (kept for API compatibility).
+   * @param jointSelector Joint selection vector.
+   */
+  FixedCoincidenceConstr(int robotIndex = 0,
+                         const std::string & body1Name = "",
+                         const std::string & body2Name = "",
+                         const Eigen::Vector3d & point1 = Eigen::Vector3d::Zero(),
+                         const Eigen::Vector3d & point2 = Eigen::Vector3d::Zero(),
+                         const Eigen::VectorXd & jointSelector = Eigen::VectorXd());
+
+  // Equality constraint
+  virtual std::string nameEq() const override;
+
+  virtual std::string descEq(const std::vector<rbd::MultiBody> & mbs, int line) override;
+
+  virtual void updateNrVars(const std::vector<rbd::MultiBody> & mbs, const SolverData & data) override;
+
+  virtual void update(const std::vector<rbd::MultiBody> & mbs,
+                      const std::vector<rbd::MultiBodyConfig> & mbcs,
+                      const SolverData & data) override;
+
+private:
+  Eigen::Vector3d point1_, point2_;
+};
+
+/**
+ * Enforce rotational coincidence between two bodies.
+ *
+ * This constraint enforces equality of angular accelerations between two bodies
+ * (3D formulation), using the angular part of their Jacobians.
+ *
+ * A PD stabilization term is added based on the difference of body world
+ * orientations and angular velocities.
+ */
+class TASKS_DLLAPI RotationalCoincidenceConstr : public CoincidenceConstr
+{
+public:
+  /**
+   * @param robotIndex Constrained robot Index in mbs.
+   * @param body1Name Name of the first body.
+   * @param body2Name Name of the second body.
+   * @param jointSelector Joint selection vector.
+   */
+  RotationalCoincidenceConstr(int robotIndex = 0,
+                              const std::string & body1Name = "",
+                              const std::string & body2Name = "",
+                              const Eigen::VectorXd & jointSelector = Eigen::VectorXd());
+
+  // Equality constraint
+  virtual std::string nameEq() const override;
+
+  virtual std::string descEq(const std::vector<rbd::MultiBody> & mbs, int line) override;
+
+  virtual void updateNrVars(const std::vector<rbd::MultiBody> & mbs, const SolverData & data) override;
+
+  virtual void update(const std::vector<rbd::MultiBody> & mbs,
+                      const std::vector<rbd::MultiBodyConfig> & mbcs,
+                      const SolverData & data) override;
+};
+
+} // namespace qp
+
+} // namespace tasks