Commit 1032380e authored by Florent Lamiraux's avatar Florent Lamiraux
Browse files

[ReedsAndShepp] Refactor steering method and distance

  - radius and wheel joints are now provided by parameters through class
    Problem,
  - distance does not need steering method anymore.
parent 25bc3015
......@@ -107,7 +107,6 @@ SET(${PROJECT_NAME}_HEADERS
include/hpp/core/path/spline.hh
include/hpp/core/path/hermite.hh
include/hpp/core/path/math.hh
include/hpp/core/reeds-shepp-path.hh
include/hpp/core/kinodynamic-path.hh
include/hpp/core/kinodynamic-oriented-path.hh
include/hpp/core/plan-and-optimize.hh
......
......@@ -38,15 +38,17 @@ namespace hpp {
virtual DistancePtr_t clone () const;
static ReedsSheppPtr_t create (const ProblemConstPtr_t& problem);
static ReedsSheppPtr_t create (const ProblemConstPtr_t& problem,
const value_type& turningRadius,
JointPtr_t xyJoint, JointPtr_t rzJoint);
const value_type& turningRadius,
JointPtr_t xyJoint, JointPtr_t rzJoint,
std::vector <JointPtr_t> wheels);
static ReedsSheppPtr_t createCopy (const ReedsSheppPtr_t& distance);
protected:
ReedsShepp (const ProblemConstPtr_t& problem);
ReedsShepp (const ProblemConstPtr_t& problem,
const value_type& turningRadius,
JointPtr_t xyJoint, JointPtr_t rzJoint);
JointPtr_t xyJoint, JointPtr_t rzJoint,
std::vector <JointPtr_t> wheels);
ReedsShepp (const ReedsShepp& distance);
/// Derived class should implement this function
......@@ -54,7 +56,12 @@ namespace hpp {
ConfigurationIn_t q2) const;
void init (const ReedsSheppWkPtr_t& weak);
private:
steeringMethod::ReedsSheppPtr_t sm_;
WeighedDistancePtr_t weighedDistance_;
DeviceWkPtr_t device_;
value_type rho_;
JointPtr_t xy_, rz_;
size_type xyId_, rzId_;
std::vector<JointPtr_t> wheels_;
ReedsSheppWkPtr_t weak_;
ReedsShepp() {};
......
......@@ -46,6 +46,18 @@ namespace hpp {
return shPtr;
}
/// Create instance and return shared pointer
static PathVectorPtr_t create (size_type outputSize,
size_type outputDerivativeSize,
const ConstraintSetPtr_t& constraint)
{
PathVector* ptr = new PathVector (outputSize, outputDerivativeSize,
constraint);
PathVectorPtr_t shPtr (ptr);
ptr->init (shPtr);
return shPtr;
}
/// Create instance and return shared pointer
static PathVectorPtr_t createCopy (const PathVectorPtr_t& original)
{
......@@ -148,6 +160,14 @@ namespace hpp {
paths_ ()
{
}
/// Constructor
PathVector (std::size_t outputSize, std::size_t outputDerivativeSize,
const ConstraintSetPtr_t& constraint) :
parent_t (std::make_pair (0, 0), outputSize, outputDerivativeSize,
constraint),
paths_ ()
{
}
///Copy constructor
PathVector (const PathVector& path) : parent_t (path),
paths_ ()
......
//
// Copyright (c) 2016 CNRS
// Authors: Joseph Mirabel
//
// This file is part of hpp-core
// hpp-core is free software: you can redistribute it
// and/or modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation, either version
// 3 of the License, or (at your option) any later version.
//
// hpp-core is distributed in the hope that it will be
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
// General Lesser Public License for more details. You should have
// received a copy of the GNU Lesser General Public License along with
// hpp-core If not, see
// <http://www.gnu.org/licenses/>.
#ifndef HPP_CORE_REEDS_SHEPP_PATH_HH
# define HPP_CORE_REEDS_SHEPP_PATH_HH
# include <hpp/pinocchio/device.hh>
# include <hpp/core/fwd.hh>
# include <hpp/core/config.hh>
# include <hpp/core/path-vector.hh>
namespace hpp {
namespace core {
/// \addtogroup path
/// \{
/// Car like motion
///
/// Implement a Reeds and Shepp motion generation on the base joint.
/// Degrees of freedom are interpolated depending on the type of
/// \link hpp::pinocchio::Joint joint \endlink
/// they parameterize:
/// The following interpolation is made:
/// \li Reeds and Shepp interpolation for the base_joint_xy and
/// base_joint_rz
/// \li If the wheel joints are passed using setWheelJoints,
/// the configuration parameter of those joints are computed so that
/// the wheel is aligned with the velocity.
/// \li linear interpolation for the other joints
class HPP_CORE_DLLAPI ReedsSheppPath : public PathVector
{
public:
typedef core::PathVector parent_t;
/// Destructor
virtual ~ReedsSheppPath () {}
/// Create instance and return shared pointer
/// \param device Robot corresponding to configurations
/// \param init, end Start and end configurations of the path
/// \param rho The radius of a turn.
/// \param extraLength the length of the path due to the non RS DoF
static ReedsSheppPathPtr_t create (const pinocchio::DevicePtr_t& device,
ConfigurationIn_t init,
ConfigurationIn_t end,
value_type extraLength,
value_type rho,
size_type xyId, size_type rzId,
const std::vector<JointPtr_t> wheels);
/// Create instance and return shared pointer
/// \param device Robot corresponding to configurations
/// \param init, end Start and end configurations of the path
/// \param rho The radius of a turn.
/// \param extraLength the length of the path due to the non RS DoF
/// \param constraints the path is subject to
static ReedsSheppPathPtr_t create (const DevicePtr_t& device,
ConfigurationIn_t init,
ConfigurationIn_t end,
value_type extraLength,
value_type rho,
size_type xyId, size_type rzId,
const std::vector<JointPtr_t> wheels,
ConstraintSetPtr_t constraints);
/// Create copy and return shared pointer
/// \param path path to copy
static ReedsSheppPathPtr_t createCopy (const ReedsSheppPathPtr_t& path)
{
ReedsSheppPath* ptr = new ReedsSheppPath (*path);
ReedsSheppPathPtr_t shPtr (ptr);
ptr->init (shPtr);
return shPtr;
}
/// Create copy and return shared pointer
/// \param path path to copy
/// \param constraints the path is subject to
static ReedsSheppPathPtr_t createCopy
(const ReedsSheppPathPtr_t& path, const ConstraintSetPtr_t& constraints)
{
ReedsSheppPath* ptr = new ReedsSheppPath (*path, constraints);
ReedsSheppPathPtr_t shPtr (ptr);
ptr->init (shPtr);
return shPtr;
}
/// Return a shared pointer to a copy of this object
virtual PathPtr_t copy () const
{
return createCopy (weak_.lock ());
}
/// Return a shared pointer to a copy of this and set constraints
///
/// \param constraints constraints to apply to the copy
/// \pre *this should not have constraints.
virtual PathPtr_t copy (const ConstraintSetPtr_t& constraints) const
{
return createCopy (weak_.lock (), constraints);
}
/// Return the internal robot.
inline DevicePtr_t device () const
{
return device_;
}
/// Get the initial configuration
inline Configuration_t initial () const
{
return initial_;
}
/// Get the final configuration
inline Configuration_t end () const
{
return end_;
}
protected:
/// Print path in a stream
virtual std::ostream& print (std::ostream &os) const;
/// Constructor
ReedsSheppPath (const DevicePtr_t& robot, ConfigurationIn_t init,
ConfigurationIn_t end, value_type extraLength,
value_type rho,
size_type xyId, size_type rzId,
const std::vector<JointPtr_t> wheels);
/// Constructor with constraints
ReedsSheppPath (const DevicePtr_t& robot, ConfigurationIn_t init,
ConfigurationIn_t end, value_type extraLength,
value_type rho, size_type xyId, size_type rzId,
const std::vector<JointPtr_t> wheels,
ConstraintSetPtr_t constraints);
/// Copy constructor
ReedsSheppPath (const ReedsSheppPath& path);
/// Copy constructor with constraints
ReedsSheppPath (const ReedsSheppPath& path,
const ConstraintSetPtr_t& constraints);
void init (ReedsSheppPathPtr_t self);
/// Virtual implementation of derivative
virtual void impl_derivative (vectorOut_t result, const value_type& t,
size_type order) const;
/// For serialization only.
ReedsSheppPath() : xyId_(0), rzId_(0) {}
private:
typedef Eigen::Matrix<value_type, 5, 1> Lengths_t;
// Compute path
void buildReedsShepp (const JointPtr_t rz,
const std::vector<JointPtr_t> wheels);
// Setup path
void setupPath (const std::size_t& typeId, double t, double u=0.,
double v=0., double w=0., double x=0.);
void CSC (const vector2_t& xy, const vector2_t& csPhi, const double& phi);
void CCC (const vector2_t& xy, const vector2_t& csPhi, const double& phi);
void CCCC (const vector2_t& xy, const vector2_t& csPhi, const double& phi);
void CCSC (const vector2_t& xy, const vector2_t& csPhi, const double& phi);
void CCSCC(const vector2_t& xy, const vector2_t& csPhi, const double& phi);
DevicePtr_t device_;
Configuration_t initial_;
Configuration_t end_;
const size_type xyId_,rzId_;
size_type dxyId_,drzId_;
std::size_t typeId_;
Lengths_t lengths_;
// Distance traveled by the center of the rear wheel axis
value_type rsLength_;
// Extra length to take into account the motion of other joints:
// Length of path = rsLength_ + extraLength_
value_type extraLength_;
// Minimal radius of curvature
value_type rho_;
ReedsSheppPathWkPtr_t weak_;
HPP_SERIALIZABLE();
}; // class ReedsSheppPath
/// \}
} // namespace core
} // namespace hpp
BOOST_CLASS_EXPORT_KEY(hpp::core::ReedsSheppPath)
#endif // HPP_CORE_REEDS_SHEPP_PATH_HH
......@@ -48,16 +48,6 @@ namespace hpp {
wheels_ = wheels;
}
/// Compute the turning radius.
///
/// The turning radius is the maximum of the turning radius of each
/// wheel. The turning radius of a wheel is the radius of the circle
/// defined by:
/// - its center is on the plane x = 0 in the frame of joint RZ,
/// - the origin of joint RZ is on the circle,
/// - the bounds of the joint wheel are saturated.
void computeRadius ();
protected:
/// Constructor
CarLike (const ProblemConstPtr_t& problem);
......@@ -84,10 +74,11 @@ namespace hpp {
size_type xyId_, rzId_;
std::vector<JointPtr_t> wheels_;
private:
value_type computeAngle(const JointPtr_t wheel) const;
void guessWheels();
CarLikeWkPtr_t weak_;
}; // CarLike
std::vector <JointPtr_t> getWheelsFromeParameter
(const ProblemConstPtr_t& problem, const JointPtr_t& rz);
/// \}
} // namespace steeringMethod
} // namespace core
......
......@@ -112,7 +112,21 @@ namespace hpp {
private:
WeighedDistancePtr_t weighedDistance_;
ReedsSheppWkPtr_t weak_;
}; // ReedsShepp
}; // class ReedsShepp
/// Create a Reeds and Shepp path and return shared pointer
/// \param device Robot corresponding to configurations,
/// \param init, end start and end configurations of the path,
/// \param extraLength the length of the path due to the non RS DoF,
/// \param rho The radius of a turn,
/// \param xyId, rzId indices in configuration vector of the joints
/// corresponding to the translation and rotation of the car.
PathVectorPtr_t reedsSheppPathOrDistance(const DevicePtr_t& device,
ConfigurationIn_t init, ConfigurationIn_t end,
value_type extraLength, value_type rho, size_type xyId, size_type rzId,
const std::vector<JointPtr_t> wheels, ConstraintSetPtr_t constraints,
bool computeDistance, value_type& distance);
/// \}
} // namespace steeringMethod
} // namespace core
......
......@@ -16,8 +16,16 @@
// hpp-core If not, see
// <http://www.gnu.org/licenses/>.
#include <pinocchio/multibody/joint/joint-generic.hpp>
#include <hpp/pinocchio/device.hh>
#include <hpp/pinocchio/joint.hh>
#include <hpp/pinocchio/joint-collection.hh>
#include <hpp/core/distance/reeds-shepp.hh>
#include <hpp/core/problem.hh>
#include <hpp/core/steering-method/reeds-shepp.hh>
#include <hpp/core/weighed-distance.hh>
namespace hpp {
namespace core {
......@@ -37,10 +45,11 @@ namespace hpp {
}
ReedsSheppPtr_t ReedsShepp::create
(const ProblemConstPtr_t& problem, const value_type& turningRadius,
JointPtr_t xyJoint, JointPtr_t rzJoint)
JointPtr_t xyJoint, JointPtr_t rzJoint,
std::vector <JointPtr_t> wheels)
{
ReedsShepp* ptr (new ReedsShepp (problem, turningRadius,
xyJoint, rzJoint));
xyJoint, rzJoint, wheels));
ReedsSheppPtr_t shPtr (ptr);
ptr->init (shPtr);
return shPtr;
......@@ -55,27 +64,67 @@ namespace hpp {
}
ReedsShepp::ReedsShepp (const ProblemConstPtr_t& problem) :
Distance (), sm_ (steeringMethod::ReedsShepp::createWithGuess (problem))
Distance (),
weighedDistance_(WeighedDistance::create(problem->robot())),
device_(problem->robot()), xyId_(0), rzId_(2)
{
DevicePtr_t d (device_.lock());
xy_ = d->getJointAtConfigRank(xyId_);
rz_ = d->getJointAtConfigRank(rzId_);
wheels_ = steeringMethod::getWheelsFromeParameter(problem, rz_);
rho_ = problem->getParameter("SteeringMethod/Carlike/turningRadius").
floatValue();
}
ReedsShepp::ReedsShepp (const ProblemConstPtr_t& problem,
const value_type& turningRadius,
JointPtr_t xyJoint, JointPtr_t rzJoint) :
Distance (), sm_ (steeringMethod::ReedsShepp::create
(problem, turningRadius, xyJoint, rzJoint))
JointPtr_t xyJoint, JointPtr_t rzJoint,
std::vector <JointPtr_t> wheels) :
Distance (), weighedDistance_(WeighedDistance::create
(problem->robot())),
device_ (problem->robot ()), rho_ (turningRadius), xy_ (xyJoint),
rz_ (rzJoint), xyId_ (xy_->rankInConfiguration ()), wheels_ (wheels),
weak_ ()
{
if (rz_->jointModel ().shortname () == "JointModelPlanar") {
rzId_ = rz_->rankInConfiguration () + 2;
} else {
rzId_ = rz_->rankInConfiguration ();
}
}
ReedsShepp::ReedsShepp (const ReedsShepp& distance) :
Distance (), sm_ (steeringMethod::ReedsShepp::createCopy (distance.sm_))
ReedsShepp::ReedsShepp (const ReedsShepp& other) :
Distance (other),
weighedDistance_(WeighedDistance::createCopy(other.weighedDistance_)),
device_(other.device_), rho_(other.rho_), xy_(other.xy_),
rz_(other.rz_), xyId_(other.xyId_), rzId_(other.rzId_),
wheels_(other.wheels_), weak_()
{
}
value_type ReedsShepp::impl_distance (ConfigurationIn_t q1,
ConfigurationIn_t q2) const
{
return (*sm_) (q1, q2)->length ();
// TODO this should not be done here.
// See todo in class ConstantCurvature
Configuration_t qEnd (q2);
qEnd.segment<2>(xyId_) = q1.segment<2>(xyId_);
qEnd.segment<2>(rzId_) = q1.segment<2>(rzId_);
// Do not take into account wheel joints in additional distance.
for (std::vector<JointPtr_t>::const_iterator it = wheels_.begin ();
it != wheels_.end (); ++it) {
size_type i = (*it)->rankInConfiguration ();
qEnd [i] = q1 [i];
}
// The length corresponding to the non RS DoF
value_type extraL = (*weighedDistance_) (q1, qEnd);
value_type distance;
PathVectorPtr_t path
(steeringMethod::reedsSheppPathOrDistance
(device_.lock (), q1, q2, extraL, rho_ , xyId_, rzId_, wheels_,
ConstraintSetPtr_t (), true, distance));
return distance;
}
void ReedsShepp::init (const ReedsSheppWkPtr_t& weak)
......
......@@ -1124,6 +1124,14 @@ namespace hpp {
"ConfigurationShooter/sampleExtraDOF",
"If false, the value of the random configuration extraDOF are set to 0.",
Parameter(true)));
Problem::declareParameter(ParameterDescription(Parameter::FLOAT,
"SteeringMethod/Carlike/turningRadius",
"Turning radius of a carlike robot for Reeds and Shepp and Dubins "
"distances and steering methods.", Parameter(1.)));
Problem::declareParameter(ParameterDescription(Parameter::STRING,
"SteeringMethod/Carlike/wheels",
"Names of revolute joints that hold directional wheels separated by "
"commas.", Parameter(std::string(""))));
HPP_END_PARAMETER_DECLARATION(ProblemSolver)
} // namespace core
} // namespace hpp
This diff is collapsed.
......@@ -14,6 +14,8 @@
// received a copy of the GNU Lesser General Public License along with
// hpp-core. If not, see <http://www.gnu.org/licenses/>.
#include <boost/algorithm/string.hpp>
#include <hpp/core/steering-method/car-like.hh>
#include <pinocchio/spatial/se3.hpp>
......@@ -35,8 +37,9 @@ namespace hpp {
DevicePtr_t d (device_.lock());
xy_ = d->getJointAtConfigRank(0);
rz_ = d->getJointAtConfigRank(2);
guessWheels();
computeRadius();
wheels_ = getWheelsFromeParameter(problem, rz_);
rho_ = problem->getParameter("SteeringMethod/Carlike/turningRadius").
floatValue();
}
CarLike::CarLike (const ProblemConstPtr_t& problem,
......@@ -62,44 +65,38 @@ namespace hpp {
{
}
void CarLike::computeRadius ()
{
rho_ = 1;
if (wheels_.empty()) return;
rho_ = 0;
for (std::vector<JointPtr_t>::const_iterator _wheels = wheels_.begin();
_wheels != wheels_.end(); ++_wheels) {
rho_ = std::max (rho_, computeAngle(*_wheels));
}
hppDout (info, "rho_ = " << rho_);
}
inline value_type CarLike::computeAngle(const JointPtr_t wheel) const
std::vector <JointPtr_t> getWheelsFromeParameter
(const ProblemConstPtr_t& problem, const JointPtr_t& rz)
{
// Compute wheel position in joint RZ
const Transform3f zt (rz_->currentTransformation ());
const Transform3f wt (zt.inverse () * wheel->currentTransformation ());
const vector3_t& wp (wt.translation ());
std::vector <JointPtr_t> wheels;
std::string p(problem->getParameter("SteeringMethod/Carlike/wheels").
stringValue());
std::vector<std::string> wheelNames;
boost::split(wheelNames, p, [](char c){return c == ',';});
// Assume the non turning wheels are on the plane z = 0 of
// in joint rz.
const value_type alpha = (wheel->upperBound(0) - wheel->lowerBound(0)) / 2;
const value_type delta = std::abs(wp[1]);
const value_type beta = std::abs(wp[0]);
return delta + beta / std::tan(alpha);
}
inline void CarLike::guessWheels()
{
wheels_.clear();
for (std::size_t i = 0; i < rz_->numberChildJoints(); ++i) {
JointPtr_t j = rz_->childJoint(i);
if (j->configSize() != 1) continue;
if (!j->isBounded(0)) continue;
if (j->name().find("wheel") == std::string::npos) continue;
wheels_.push_back(j);
}
wheels.clear();
for(const std::string& name : wheelNames) {
if (name == "") continue;
bool found(false);
for (std::size_t i = 0; i < rz->numberChildJoints(); ++i) {
JointPtr_t j = rz->childJoint(i);
if (j->name() == name) {
found = true;
if (j->configSize() != 1) {
throw std::runtime_error
("Carlike: wheel joint should be of dimension 1.");
}
wheels.push_back(j);
hppDout(info, "wheel: " << name);
}
}
if (!found) {
std::ostringstream os;
os << "CarLike: no joint with name \"" << name << "\".";
throw std::runtime_error(os.str());
}
}
return wheels;
}
} // namespace steeringMethod
......
......@@ -22,8 +22,8 @@
#include <hpp/pinocchio/joint.hh>
#include <hpp/core/distance.hh>
#include <hpp/core/path-vector.hh>
#include <hpp/core/problem.hh>
#include <hpp/core/reeds-shepp-path.hh>
#include <hpp/core/weighed-distance.hh>
namespace hpp {
......@@ -46,9 +46,11 @@ namespace hpp {
// The length corresponding to the non RS DoF
value_type extraL = (*weighedDistance_) (q1, qEnd);
ReedsSheppPathPtr_t path =
ReedsSheppPath::create (device_.lock (), q1, q2, extraL,
rho_ , xyId_, rzId_, wheels_, constraints ());
value_type distance;
PathVectorPtr_t path
(reedsSheppPathOrDistance(device_.lock (), q1, q2, extraL,
rho_ , xyId_, rzId_, wheels_, constraints (),
false, distance));
return path;
}
......
......@@ -76,3 +76,4 @@ ADD_SUBDIRECTORY(plugin-test)
CONFIG_FILES (plugin.cc)
ADD_TESTCASE (plugin TRUE)
ADD_DEPENDENCIES (plugin example)
ADD_TESTCASE(reeds-and-shepp FALSE)
// Copyright (c) 2020, CNRS
// Authors: Florent Lamiraux
//
// This file is part of hpp-core.
// hpp-core is free software: you can redistribute it
// and/or modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation, either version
// 3 of the License, or (at your option) any later version.
//
// hpp-core is distributed in the hope that it will be
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
<