Leg.hpp

Go to the documentation of this file.

 
#pragma once
 
#include "spatial.hpp"
#include <cmath>
#include <iostream>
#include <numbers>
 
constexpr int njoints = 3;

class Leg {
public:
  Leg(Eigen::Vector<double, njoints + 1> &l,
      Eigen::Matrix<double, 6, njoints> &Slist, Eigen::Isometry3d &M,
      std::array<Eigen::Isometry3d, njoints + 1> &Mlist,
      std::array<Eigen::Matrix6d, njoints> &Glist,
      Eigen::Matrix<double, njoints, 2> &thetaRange,
      Eigen::Vector<double, njoints> &thetadMax,
      Eigen::Vector<double, njoints> &thetaddMax,
      Eigen::Vector<double, njoints> &tauMax)
      : l{l}, Slist{Slist}, M{M}, Mlist{Mlist}, Glist{Glist},
        thetaRange{thetaRange}, thetadMax{thetadMax}, thetaddMax{thetaddMax},
        tauMax{tauMax} {};

  bool fk(const Eigen::Vector3d &thetalist, Eigen::Vector3d &pf);

  bool ik(const Eigen::Vector3d &pf, Eigen::Vector3d &thetalist,
          Eigen::Matrix3d *Jinv = nullptr);

  bool ivk(const Eigen::Vector3d &vf, const Eigen::Matrix3d &Jinv,
           Eigen::Vector3d &thetadlist);

  bool id(Eigen::Vector<double, njoints> &taulist);
 
  // Initiate lift and set target
  void liftTo(const Eigen::Isometry3d &T);

  bool jointTrajectory(const Eigen::Isometry3d &Tstart,
                       const Eigen::Isometry3d &Tgoal, const double t0,
                       const double t, Eigen::Isometry3d &Tcurrent);

  void run();

  enum state_t {
    IDLE,
    HOMING,
    RUNNING,
    LIFT,
    PLACE
  } state = IDLE,
    statep = IDLE;
 
  // Foot space
  Eigen::Vector3d pf;  
  Eigen::Vector3d vf;  
  Eigen::Vector3d dvf; 
  Eigen::Vector3d ff;  
  Eigen::Vector3d g;   
  // Eigen::Vector3d liftpf; //< m
  // double llift = 0.02;    // m TODO: Set from config
 
  // Joint space (updated by motor controller)
  Eigen::Vector<double, njoints> thetalist;   // rad
  Eigen::Vector<double, njoints> thetadlist;  // rad/s
  Eigen::Vector<double, njoints> thetaddlist; // rad/s^2
  Eigen::Vector<double, njoints> taulist;     // N*m
 
private:
  /* Leg geometry & mass properties */
  const Eigen::Vector<double, njoints + 1> l;             // m
  const Eigen::Matrix<double, 6, njoints> Slist;          //
  const Eigen::Isometry3d M;                              // SE(3)
  const std::array<Eigen::Isometry3d, njoints + 1> Mlist; // SE(3)
  const std::array<Eigen::Matrix6d, njoints> Glist;       // kg, kg*m^2
 
  /* Joint limits */
  const Eigen::Matrix<double, njoints, 2> thetaRange; // rad
  const Eigen::Vector<double, njoints> thetadMax;     // rad/s
  const Eigen::Vector<double, njoints> thetaddMax;    // rad/s^2
  const Eigen::Vector<double, njoints> tauMax;        // N*m
 
  /* Joint directions */
  const Eigen::Vector<double, njoints> thetadir{Slist(0, 0) * Slist(1, 1),
                                                Slist(1, 1), Slist(1, 1)};
 
  /* Inverse velocity kinematics */
  Eigen::Matrix<double, njoints, 3> Jinv; // rad/m
 
  /* Inverse dynamics */
  Eigen::Isometry3d Mi;                          // SE(3)
  Eigen::Matrix<double, 6, njoints> Ai;          //
  Eigen::Matrix<double, 6, njoints + 1> Vi;      // rad/s, m/s
  Eigen::Matrix<double, 6, njoints + 1> dVi;     // rad/s^2, m/s^2
  std::array<Eigen::Matrix6d, njoints + 1> AdTi; //
  Eigen::Vector6d Fi;                            // N*m, N
};