Contact.h

Go to the documentation of this file.

/*
 * Copyright (c) 2018-2019, CNRS-UM LIRMM
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 * 1. Redistributions of source code must retain the above copyright notice,
 * this list of conditions and the following disclaimer.
 *
 * 2. Redistributions in binary form must reproduce the above copyright notice,
 * this list of conditions and the following disclaimer in the documentation
 * and/or other materials provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 */

#pragma once

#include <mc_rbdyn/Robot.h>
#include <mc_rtc/Configuration.h>
#include <mc_rtc/logging.h>

#include <SpaceVecAlg/SpaceVecAlg>

#include <cmath>
#include <lipm_walking/Sole.h>
#include <lipm_walking/utils/world.h>

namespace lipm_walking
{

using HrepXd = std::pair<Eigen::MatrixXd, Eigen::VectorXd>;

enum class ContactState
{
  DoubleSupport,
  LeftFoot,
  RightFoot
};

struct Contact
{
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  Contact() {}

  Contact(const sva::PTransformd & pose) : pose(pose) {}

  Eigen::Vector3d sagittal() const
  {
    return pose.rotation().row(0);
  }

  Eigen::Vector3d lateral() const
  {
    return pose.rotation().row(1);
  }

  Eigen::Vector3d normal() const
  {
    return pose.rotation().row(2);
  }

  const Eigen::Vector3d & position() const
  {
    return pose.translation();
  }

  const Eigen::Vector3d & p() const
  {
    return position();
  }

  Eigen::Vector3d anklePos(const Sole & sole) const
  {
    if(surfaceName == "LeftFootCenter")
    {
      return p() + sole.leftAnkleOffset.x() * sagittal() + sole.leftAnkleOffset.y() * lateral();
    }
    else if(surfaceName == "RightFootCenter")
    {
      return p() + sole.leftAnkleOffset.x() * sagittal() - sole.leftAnkleOffset.y() * lateral();
    }
    else
    {
      LOG_ERROR("Cannot compute anklePos for surface " << surfaceName);
      return p();
    }
  }

  sva::PTransformd anklePose(const Sole & sole) const
  {
    return {pose.rotation(), anklePos(sole)};
  }

  double x() const
  {
    return position()(0);
  }

  double y() const
  {
    return position()(1);
  }

  double z() const
  {
    return position()(2);
  }

  Eigen::Vector3d vertex0() const
  {
    return position() + halfLength * sagittal() + halfWidth * lateral();
  }

  Eigen::Vector3d vertex1() const
  {
    return position() + halfLength * sagittal() - halfWidth * lateral();
  }

  Eigen::Vector3d vertex2() const
  {
    return position() - halfLength * sagittal() - halfWidth * lateral();
  }

  Eigen::Vector3d vertex3() const
  {
    return position() - halfLength * sagittal() + halfWidth * lateral();
  }

  template<int i>
  double minCoord() const
  {
    return std::min(std::min(vertex0()(i), vertex1()(i)), std::min(vertex2()(i), vertex3()(i)));
  }

  template<int i>
  double maxCoord() const
  {
    return std::max(std::max(vertex0()(i), vertex1()(i)), std::max(vertex2()(i), vertex3()(i)));
  }

  double xmin() const
  {
    return minCoord<0>();
  }

  double xmax() const
  {
    return maxCoord<0>();
  }

  double ymin() const
  {
    return minCoord<1>();
  }

  double ymax() const
  {
    return maxCoord<1>();
  }

  double zmin() const
  {
    return minCoord<2>();
  }

  double zmax() const
  {
    return maxCoord<2>();
  }

  HrepXd hrep() const
  {
    Eigen::Matrix<double, 4, 2> localHrepMat, worldHrepMat;
    Eigen::Matrix<double, 4, 1> localHrepVec, worldHrepVec;
    // clang-format off
    localHrepMat <<
      +1, 0,
      -1, 0,
      0, +1,
      0, -1;
    localHrepVec <<
      halfLength,
      halfLength,
      halfWidth,
      halfWidth;
    // clang-format on
    if((normal() - world::vertical).norm() > 1e-3)
    {
      LOG_WARNING("Contact is not horizontal");
    }
    const sva::PTransformd & X_0_c = pose;
    worldHrepMat = localHrepMat * X_0_c.rotation().topLeftCorner<2, 2>();
    worldHrepVec = worldHrepMat * X_0_c.translation().head<2>() + localHrepVec;
    return HrepXd(worldHrepMat, worldHrepVec);
  }

  inline sva::PTransformd robotTransform(const mc_rbdyn::Robot & robot) const
  {
    const sva::PTransformd & X_0_c = pose;
    const sva::PTransformd & X_0_fb = robot.posW();
    sva::PTransformd X_s_0 = robot.surfacePose(surfaceName).inv();
    sva::PTransformd X_s_fb = X_0_fb * X_s_0;
    return X_s_fb * X_0_c;
  }

public:
  Eigen::Vector3d refVel =
      Eigen::Vector3d::Zero(); 
  double halfLength = 0.; 
  double halfWidth = 0.; 
  mc_rtc::Configuration
      swingConfig; 
  std::string surfaceName = ""; 
  sva::PTransformd pose; 
  unsigned id = 0; 
};

inline Contact operator*(const sva::PTransformd & X, const Contact & contact)
{
  Contact result = contact;
  result.pose = X * contact.pose;
  return result;
}
} // namespace lipm_walking

namespace mc_rtc
{
template<>
struct ConfigurationLoader<lipm_walking::Contact>
{
  static lipm_walking::Contact load(const mc_rtc::Configuration & config)
  {
    lipm_walking::Contact contact;
    contact.pose = config("pose");
    config("half_length", contact.halfLength);
    config("half_width", contact.halfWidth);
    config("ref_vel", contact.refVel);
    config("surface", contact.surfaceName);
    if(config.has("swing"))
    {
      contact.swingConfig = config("swing");
    }
    return contact;
  }

  static mc_rtc::Configuration save(const lipm_walking::Contact & contact)
  {
    mc_rtc::Configuration config;
    config.add("half_length", contact.halfLength);
    config.add("half_width", contact.halfWidth);
    config.add("pose", contact.pose);
    config.add("ref_vel", contact.refVel);
    config.add("surface", contact.surfaceName);
    if(!contact.swingConfig.empty())
    {
      config("swing") = contact.swingConfig;
    }
    return config;
  }
};

} // namespace mc_rtc