robot.hpp

Go to the documentation of this file.

#ifndef ROBOTOC_ROBOT_HPP_
#define ROBOTOC_ROBOT_HPP_
 
#include <string>
#include <vector>
#include <utility>
#include <iostream>
 
#include "Eigen/Core"
#include "pinocchio/multibody/model.hpp"
#include "pinocchio/multibody/data.hpp"
#include "pinocchio/container/aligned-vector.hpp"
#include "pinocchio/spatial/force.hpp"
 
#include "robotoc/robot/robot_model_info.hpp"
#include "robotoc/robot/se3.hpp"
#include "robotoc/robot/point_contact.hpp"
#include "robotoc/robot/surface_contact.hpp"
#include "robotoc/robot/contact_status.hpp"
#include "robotoc/robot/impact_status.hpp"
#include "robotoc/robot/robot_properties.hpp"
#include "robotoc/utils/aligned_vector.hpp"
 
 
namespace robotoc {
 
class Robot {
public:
  using Vector6d = Eigen::Matrix<double, 6, 1>;
 
  Robot(const RobotModelInfo& robot_model_info);
 
  Robot();
 
  ~Robot() = default;
 
  Robot(const Robot&) = default;
 
  Robot& operator=(const Robot&) = default;
 
  Robot(Robot&&) noexcept = default;
 
  Robot& operator=(Robot&&) noexcept = default;
 
  template <typename TangentVectorType, typename ConfigVectorType>
  void integrateConfiguration(
      const Eigen::MatrixBase<TangentVectorType>& v, 
      const double integration_length, 
      const Eigen::MatrixBase<ConfigVectorType>& q) const;
 
  template <typename ConfigVectorType1, typename TangentVectorType,  
            typename ConfigVectorType2>
  void integrateConfiguration(
      const Eigen::MatrixBase<ConfigVectorType1>& q, 
      const Eigen::MatrixBase<TangentVectorType>& v, 
      const double integration_length, 
      const Eigen::MatrixBase<ConfigVectorType2>& q_integrated) const;
 
  template <typename ConfigVectorType, typename TangentVectorType, 
            typename MatrixType1, typename MatrixType2>
  void dIntegrateTransport_dq(
      const Eigen::MatrixBase<ConfigVectorType>& q,
      const Eigen::MatrixBase<TangentVectorType>& v,
      const Eigen::MatrixBase<MatrixType1>& Jin,
      const Eigen::MatrixBase<MatrixType2>& Jout) const;
 
  template <typename ConfigVectorType, typename TangentVectorType, 
            typename MatrixType1, typename MatrixType2>
  void dIntegrateTransport_dv(
      const Eigen::MatrixBase<ConfigVectorType>& q,
      const Eigen::MatrixBase<TangentVectorType>& v,
      const Eigen::MatrixBase<MatrixType1>& Jin,
      const Eigen::MatrixBase<MatrixType2>& Jout) const;
 
  template <typename ConfigVectorType1, typename ConfigVectorType2, 
            typename TangentVectorType>
  void subtractConfiguration(
      const Eigen::MatrixBase<ConfigVectorType1>& qf, 
      const Eigen::MatrixBase<ConfigVectorType2>& q0,
      const Eigen::MatrixBase<TangentVectorType>& qdiff) const;
 
  template <typename ConfigVectorType1, typename ConfigVectorType2, 
            typename MatrixType>
  void dSubtractConfiguration_dqf(
      const Eigen::MatrixBase<ConfigVectorType1>& qf,
      const Eigen::MatrixBase<ConfigVectorType2>& q0,
      const Eigen::MatrixBase<MatrixType>& dqdiff_dqf) const;
 
  template <typename ConfigVectorType1, typename ConfigVectorType2, 
            typename MatrixType>
  void dSubtractConfiguration_dq0(
      const Eigen::MatrixBase<ConfigVectorType1>& qf,
      const Eigen::MatrixBase<ConfigVectorType2>& q0,
      const Eigen::MatrixBase<MatrixType>& dqdiff_dq0) const;
 
  template <typename ConfigVectorType1, typename ConfigVectorType2, 
            typename ConfigVectorType3>
  void interpolateConfiguration(
      const Eigen::MatrixBase<ConfigVectorType1>& q1, 
      const Eigen::MatrixBase<ConfigVectorType2>& q2, const double t,
      const Eigen::MatrixBase<ConfigVectorType3>& qout) const;
 
  template <typename ConfigVectorType, typename MatrixType>
  void integrateCoeffWiseJacobian(const Eigen::MatrixBase<ConfigVectorType>& q, 
                                  const Eigen::MatrixBase<MatrixType>& J) const;
 
  template <typename ConfigVectorType, typename TangentVectorType1, 
            typename TangentVectorType2>
  void updateKinematics(const Eigen::MatrixBase<ConfigVectorType>& q, 
                        const Eigen::MatrixBase<TangentVectorType1>& v, 
                        const Eigen::MatrixBase<TangentVectorType2>& a);
 
  template <typename ConfigVectorType, typename TangentVectorType>
  void updateKinematics(const Eigen::MatrixBase<ConfigVectorType>& q, 
                        const Eigen::MatrixBase<TangentVectorType>& v);
 
  template <typename ConfigVectorType>
  void updateKinematics(const Eigen::MatrixBase<ConfigVectorType>& q);
 
  template <typename ConfigVectorType, typename TangentVectorType1, 
            typename TangentVectorType2>
  void updateFrameKinematics(const Eigen::MatrixBase<ConfigVectorType>& q, 
                             const Eigen::MatrixBase<TangentVectorType1>& v, 
                             const Eigen::MatrixBase<TangentVectorType2>& a);
 
  template <typename ConfigVectorType, typename TangentVectorType>
  void updateFrameKinematics(const Eigen::MatrixBase<ConfigVectorType>& q, 
                             const Eigen::MatrixBase<TangentVectorType>& v);
 
  template <typename ConfigVectorType>
  void updateFrameKinematics(const Eigen::MatrixBase<ConfigVectorType>& q);
 
  const Eigen::Vector3d& framePosition(const int frame_id) const;
 
  const Eigen::Vector3d& framePosition(const std::string& frame_name) const;
 
  const Eigen::Matrix3d& frameRotation(const int frame_id) const;
 
  const Eigen::Matrix3d& frameRotation(const std::string& frame_name) const;
 
  const SE3& framePlacement(const int frame_id) const;
 
  const SE3& framePlacement(const std::string& frame_name) const;
 
  const Eigen::Vector3d& CoM() const;
 
  Eigen::Vector3d frameLinearVelocity(
      const int frame_id, 
      const pinocchio::ReferenceFrame reference_frame=pinocchio::LOCAL_WORLD_ALIGNED) const;
 
  Eigen::Vector3d frameLinearVelocity(
      const std::string& frame_name, 
      const pinocchio::ReferenceFrame reference_frame=pinocchio::LOCAL_WORLD_ALIGNED) const;
 
  Eigen::Vector3d frameAngularVelocity(
      const int frame_id, 
      const pinocchio::ReferenceFrame reference_frame=pinocchio::LOCAL_WORLD_ALIGNED) const;
 
  Eigen::Vector3d frameAngularVelocity(
      const std::string& frame_name, 
      const pinocchio::ReferenceFrame reference_frame=pinocchio::LOCAL_WORLD_ALIGNED) const;
 
  Vector6d frameSpatialVelocity(
      const int frame_id, 
      const pinocchio::ReferenceFrame reference_frame=pinocchio::LOCAL_WORLD_ALIGNED) const;
 
  Vector6d frameSpatialVelocity(
      const std::string& frame_name, 
      const pinocchio::ReferenceFrame reference_frame=pinocchio::LOCAL_WORLD_ALIGNED) const;
 
  const Eigen::Vector3d& CoMVelocity() const;
 
  template <typename MatrixType>
  void getFrameJacobian(const int frame_id, 
                        const Eigen::MatrixBase<MatrixType>& J);
 
  template <typename MatrixType>
  void getCoMJacobian(const Eigen::MatrixBase<MatrixType>& J) const;
 
  template <typename Vector3dType>
  void transformFromLocalToWorld(
      const int frame_id, const Eigen::Vector3d& vec_local, 
      const Eigen::MatrixBase<Vector3dType>& vec_world) const;
 
  template <typename Vector3dType, typename MatrixType>
  void getJacobianTransformFromLocalToWorld(
      const int frame_id, const Eigen::MatrixBase<Vector3dType>& vec_world,
      const Eigen::MatrixBase<MatrixType>& J);
 
  template <typename VectorType>
  void computeBaumgarteResidual(
      const ContactStatus& contact_status, 
      const Eigen::MatrixBase<VectorType>& baumgarte_residual);
 
  template <typename MatrixType1, typename MatrixType2, typename MatrixType3>
  void computeBaumgarteDerivatives(
      const ContactStatus& contact_status, 
      const Eigen::MatrixBase<MatrixType1>& baumgarte_partial_dq, 
      const Eigen::MatrixBase<MatrixType2>& baumgarte_partial_dv, 
      const Eigen::MatrixBase<MatrixType3>& baumgarte_partial_da);
 
  template <typename VectorType>
  void computeImpactVelocityResidual(
      const ImpactStatus& impact_status, 
      const Eigen::MatrixBase<VectorType>& velocity_residual) const;
 
  template <typename MatrixType1, typename MatrixType2>
  void computeImpactVelocityDerivatives(
      const ImpactStatus& impact_status, 
      const Eigen::MatrixBase<MatrixType1>& velocity_partial_dq, 
      const Eigen::MatrixBase<MatrixType2>& velocity_partial_dv);
 
  template <typename VectorType>
  void computeContactPositionResidual(
      const ImpactStatus& impact_status, 
      const Eigen::MatrixBase<VectorType>& position_residual);
 
  template <typename MatrixType>
  void computeContactPositionDerivative(
      const ImpactStatus& impact_status, 
      const Eigen::MatrixBase<MatrixType>& position_partial_dq);
 
  void setContactForces(const ContactStatus& contact_status, 
                        const std::vector<Vector6d>& f);
 
  void setImpactForces(const ImpactStatus& impact_status, 
                       const std::vector<Vector6d>& f);
 
  template <typename ConfigVectorType, typename TangentVectorType1, 
            typename TangentVectorType2, typename TangentVectorType3>
  void RNEA(const Eigen::MatrixBase<ConfigVectorType>& q, 
            const Eigen::MatrixBase<TangentVectorType1>& v, 
            const Eigen::MatrixBase<TangentVectorType2>& a, 
            const Eigen::MatrixBase<TangentVectorType3>& tau);
 
  template <typename ConfigVectorType, typename TangentVectorType1, 
            typename TangentVectorType2, typename MatrixType1, 
            typename MatrixType2, typename MatrixType3>
  void RNEADerivatives(const Eigen::MatrixBase<ConfigVectorType>& q, 
                       const Eigen::MatrixBase<TangentVectorType1>& v, 
                       const Eigen::MatrixBase<TangentVectorType2>& a,
                       const Eigen::MatrixBase<MatrixType1>& dRNEA_partial_dq, 
                       const Eigen::MatrixBase<MatrixType2>& dRNEA_partial_dv, 
                       const Eigen::MatrixBase<MatrixType3>& dRNEA_partial_da);
 
  template <typename ConfigVectorType, typename TangentVectorType1, 
            typename TangentVectorType2>
  void RNEAImpact(const Eigen::MatrixBase<ConfigVectorType>& q, 
                  const Eigen::MatrixBase<TangentVectorType1>& dv,
                  const Eigen::MatrixBase<TangentVectorType2>& res);
 
  template <typename ConfigVectorType, typename TangentVectorType, 
            typename MatrixType1, typename MatrixType2>
  void RNEAImpactDerivatives(
      const Eigen::MatrixBase<ConfigVectorType>& q, 
      const Eigen::MatrixBase<TangentVectorType>& dv, 
      const Eigen::MatrixBase<MatrixType1>& dRNEA_partial_dq, 
      const Eigen::MatrixBase<MatrixType2>& dRNEA_partial_ddv);
 
  template <typename MatrixType1, typename MatrixType2>
  void computeMinv(const Eigen::MatrixBase<MatrixType1>& M, 
                   const Eigen::MatrixBase<MatrixType2>& Minv);
 
  template <typename MatrixType1, typename MatrixType2, typename MatrixType3>
  void computeMJtJinv(const Eigen::MatrixBase<MatrixType1>& M, 
                      const Eigen::MatrixBase<MatrixType2>& J,
                      const Eigen::MatrixBase<MatrixType3>& MJtJinv);
 
  Eigen::VectorXd generateFeasibleConfiguration() const;
 
  template <typename ConfigVectorType>
  void normalizeConfiguration(
      const Eigen::MatrixBase<ConfigVectorType>& q) const;
 
  int frameId(const std::string& frame_name) const;
 
  std::string frameName(const int frame_id) const;
 
  double totalMass() const;
 
  double totalWeight() const;
 
  int dimq() const;
 
  int dimv() const;
 
  int dimu() const;
 
  int max_dimf() const;
 
  int dim_passive() const;
 
  bool hasFloatingBase() const;
 
  int maxNumContacts() const;
 
  int maxNumPointContacts() const;
 
  int maxNumSurfaceContacts() const;
 
  ContactType contactType(const int contact_index) const;
 
  std::vector<ContactType> contactTypes() const;
 
  std::vector<int> contactFrames() const;
 
  std::vector<std::string> contactFrameNames() const;
 
  std::vector<int> pointContactFrames() const;
 
  std::vector<std::string> pointContactFrameNames() const;
 
  std::vector<int> surfaceContactFrames() const;
 
  std::vector<std::string> surfaceContactFrameNames() const;
 
  ContactStatus createContactStatus() const;
 
  ImpactStatus createImpactStatus() const;
 
  void initializeJointLimits();
 
  Eigen::VectorXd jointEffortLimit() const;
 
  Eigen::VectorXd jointVelocityLimit() const;
 
  Eigen::VectorXd lowerJointPositionLimit() const;
 
  Eigen::VectorXd upperJointPositionLimit() const;
 
  void setJointEffortLimit(const Eigen::VectorXd& joint_effort_limit);
 
  void setJointVelocityLimit(const Eigen::VectorXd& joint_velocity_limit);
 
  void setLowerJointPositionLimit(
      const Eigen::VectorXd& lower_joint_position_limit);
 
  void setUpperJointPositionLimit(
      const Eigen::VectorXd& upper_joint_position_limit);
 
  const RobotModelInfo& robotModelInfo() const;
 
  const RobotProperties& robotProperties() const;
 
  void setRobotProperties(const RobotProperties& properties);
 
  void disp(std::ostream& os) const;
 
  friend std::ostream& operator<<(std::ostream& os, const Robot& robot);
 
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
private:
  // Robot model info
  RobotModelInfo info_;
  // Pinocchio models and datas, and runtime variables
  pinocchio::Model model_, impact_model_;
  pinocchio::Data data_, impact_data_;
  pinocchio::container::aligned_vector<pinocchio::Force> fjoint_;
  Eigen::MatrixXd dimpact_dv_; 
  // Contact models
  aligned_vector<PointContact> point_contacts_;
  aligned_vector<SurfaceContact> surface_contacts_;
  // Robot model variables
  int dimq_, dimv_, dimu_, dim_passive_, max_dimf_, max_num_contacts_;
  RobotProperties properties_;
  Eigen::VectorXd joint_effort_limit_, joint_velocity_limit_, 
                  lower_joint_position_limit_, upper_joint_position_limit_;
};
 
} // namespace robotoc
 
#include "robotoc/robot/robot.hxx"
 
#endif // ROBOTOC_ROBOT_HPP_