cartesian.hh

Go to the documentation of this file.

// Copyright (c) 2026, LAAS-CNRS
// Authors: Florent Lamiraux
//
 
// 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.
 
#ifndef HPP_MANIPULATION_STEERING_METHOD_CARTESIAN_HH
#define HPP_MANIPULATION_STEERING_METHOD_CARTESIAN_HH
 
#include <hpp/manipulation/fwd.hh>
 
namespace hpp {
namespace manipulation {
namespace steeringMethod {
 
HPP_PREDEF_CLASS(Cartesian);
typedef shared_ptr<Cartesian> CartesianPtr_t;

class Cartesian {
 public:
  typedef constraints::ImplicitPtr_t ImplicitPtr_t;
  typedef core::ConfigurationIn_t ConfigurationIn_t;
  typedef core::interval_t interval_t;
  typedef core::PathPtr_t PathPtr_t;
 
  static CartesianPtr_t create(const core::ProblemConstPtr_t& problem);

  static PathPtr_t makePiecewiseLinearTrajectory(matrixIn_t points,
                                                 vectorIn_t weights);

  void maxIterations(size_type iterations);
  size_type maxIterations() const;

  void errorThreshold(const value_type& threshold);
  value_type errorThreshold() const;
  void trajectoryConstraint(const ImplicitPtr_t& ic);
  const ImplicitPtr_t& trajectoryConstraint() { return trajConstraint_; }
  void rightHandSide(const PathPtr_t& rhs, bool se3Output);

  void rightHandSide(const DifferentiableFunctionPtr_t& rhs,
                     const interval_t& timeRange);

  const DifferentiableFunctionPtr_t& rightHandSide() const { return rhs_; }

  const interval_t& timeRange() const { return timeRange_; }

  size_type nDiscreteSteps() const { return nDiscreteSteps_; }
 
  void nDiscreteSteps(size_type n) {
    assert(n > 0);
    nDiscreteSteps_ = n;
  }

  bool planPath(ConfigurationIn_t q_init, PathPtr_t& result);
 
 protected:
  Cartesian(const core::ProblemConstPtr_t& problem);
  PathPtr_t projectedPath(vectorIn_t times, matrixIn_t configs) const;
  void checkProblem(const std::string& method);
 
 private:
  pinocchio::DevicePtr_t robot_;
  core::ConstraintSetPtr_t constraints_;
  ImplicitPtr_t trajConstraint_;
  DifferentiableFunctionPtr_t rhs_;
  interval_t timeRange_;
  size_type nDiscreteSteps_;
 
};  // class Cartesian


 
}  // namespace steeringMethod
}  // namespace manipulation
}  // namespace hpp
 
#endif  // HPP_MANIPULATION_STEERING_METHOD_CARTESIAN_HH