Unverified Commit 79a0de83 authored by Joseph Mirabel's avatar Joseph Mirabel Committed by GitHub
Browse files

Merge pull request #157 from florent-lamiraux/carlike

Enhance carlike paths.
parents 7904e170 4d25c945
......@@ -23,7 +23,7 @@
# include <hpp/core/fwd.hh>
# include <hpp/core/config.hh>
# include <hpp/core/path.hh>
# include <hpp/core/path-vector.hh>
namespace hpp {
namespace core {
......@@ -43,10 +43,10 @@ namespace hpp {
/// 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 DubinsPath : public Path
class DubinsPath : public PathVector
{
public:
typedef core::Path parent_t;
typedef core::PathVector parent_t;
/// Destructor
virtual ~DubinsPath () throw () {}
......@@ -54,19 +54,24 @@ namespace hpp {
/// 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 Part of path length due to non Dubins degrees of
/// freedom,
/// \param rho The radius of a turn,
/// \param xyId, rzId indices in configuration vector of translation
/// and rotation of the car,
/// \param wheels vector of joints that represent turning wheels.
static DubinsPathPtr_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);
/// Create instance and return shared pointer
/// \param device Robot corresponding to configurations
/// \param init, end Start and end configurations of the path
/// \param extraLength Part of path length due to non Dubins degrees of
/// freedom,
/// \param rho The radius of a turn.
/// \param xyId, rzId indices in configuration vector of translation
/// and rotation of the car,
......@@ -75,6 +80,7 @@ namespace hpp {
static DubinsPathPtr_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,
......@@ -147,13 +153,13 @@ namespace hpp {
}
/// Constructor
DubinsPath (const DevicePtr_t& robot, ConfigurationIn_t init,
ConfigurationIn_t end, value_type rho,
ConfigurationIn_t end, value_type extraLength, value_type rho,
size_type xyId, size_type rzId,
const std::vector<JointPtr_t> wheels);
/// Constructor with constraints
DubinsPath (const DevicePtr_t& robot, ConfigurationIn_t init,
ConfigurationIn_t end, value_type rho,
ConfigurationIn_t end, value_type extraLength, value_type rho,
size_type xyId, size_type rzId,
const std::vector<JointPtr_t> wheels,
ConstraintSetPtr_t constraints);
......@@ -167,19 +173,7 @@ namespace hpp {
void init (DubinsPathPtr_t self);
virtual bool impl_compute (ConfigurationOut_t result,
value_type param) const;
/// Virtual implementation of derivative
virtual void impl_derivative (vectorOut_t result, const value_type& t,
size_type order) const;
private:
/// Set the wheel joints for a car-like vehicle.
///
/// \param rz joint from which the turning radius was computed.
/// \param wheels bounded rotation joints.
void setWheelJoints (const JointPtr_t rz,
const std::vector<JointPtr_t> wheels);
void dubins_init_normalised (double alpha, double beta, double d);
void dubins_init (vector3_t q0, vector3_t q1);
typedef Eigen::Matrix<value_type, 3, 1> Lengths_t;
......@@ -189,15 +183,10 @@ namespace hpp {
Configuration_t end_;
const size_type xyId_,rzId_;
size_type dxyId_,drzId_;
struct Wheels_t {
value_type L, R, S; // Left, Right and Straight turn
JointPtr_t j;
Wheels_t () : j() {}
};
std::vector<Wheels_t> wheels_;
std::vector<JointPtr_t> wheels_;
std::size_t typeId_;
Lengths_t lengths_;
value_type rho_;
value_type extraLength_, rho_;
vector3_t qi_; // the initial configuration
DubinsPathWkPtr_t weak_;
......
......@@ -169,8 +169,7 @@ namespace hpp {
// Compute path
void buildReedsShepp (const JointPtr_t rz,
const std::vector<JointPtr_t> wheels,
const ConstraintSetPtr_t constraints = ConstraintSetPtr_t());
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.);
......
......@@ -25,20 +25,25 @@
#include <hpp/pinocchio/device.hh>
#include <hpp/pinocchio/joint.hh>
#include <hpp/core/dubins-path.hh>
#include <hpp/core/steering-method/constant-curvature.hh>
#include <pinocchio/spatial/se3.hpp>
#include "dubins.hh"
namespace hpp {
namespace core {
using steeringMethod::ConstantCurvaturePtr_t;
using steeringMethod::ConstantCurvature;
DubinsPathPtr_t DubinsPath::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)
{
DubinsPath* ptr = new DubinsPath (device, init, end, rho, xyId, rzId,
wheels);
DubinsPath* ptr = new DubinsPath (device, init, end, extraLength, rho,
xyId, rzId, wheels);
DubinsPathPtr_t shPtr (ptr);
try {
ptr->init (shPtr);
......@@ -51,13 +56,14 @@ namespace hpp {
DubinsPathPtr_t DubinsPath::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)
{
DubinsPath* ptr = new DubinsPath (device, init, end, rho, xyId, rzId,
wheels, constraints);
DubinsPath* ptr = new DubinsPath (device, init, end, extraLength, rho,
xyId, rzId, wheels, constraints);
DubinsPathPtr_t shPtr (ptr);
ptr->init (shPtr);
return shPtr;
......@@ -74,37 +80,13 @@ namespace hpp {
return std::min(j->upperBound(i), std::max(j->lowerBound(i), v));
}
void DubinsPath::setWheelJoints (const JointPtr_t rz,
const std::vector<JointPtr_t> wheels)
{
Transform3f zt (rz->currentTransformation ().inverse());
wheels_.resize(wheels.size());
std::size_t rk = 0;
for (std::vector<JointPtr_t>::const_iterator _wheels = wheels.begin();
_wheels != wheels.end(); ++_wheels) {
wheels_[rk].j = *_wheels;
wheels_[rk].S = meanBounds(wheels_[rk].j, 0);
const vector3_t wheelPos = zt.act
(wheels_[rk].j->currentTransformation().translation());
const value_type left = std::atan(wheelPos[0] / ( rho_ - wheelPos[1]));
const value_type right = std::atan(wheelPos[0] / (-rho_ - wheelPos[1]));
wheels_[rk].L = saturate(wheels_[rk].S + left, *_wheels, 0);
wheels_[rk].R = saturate(wheels_[rk].S + right, *_wheels, 0);
++rk;
}
}
DubinsPath::DubinsPath (const DevicePtr_t& robot, ConfigurationIn_t init,
ConfigurationIn_t end, value_type rho,
size_type xyId, size_type rzId,
ConfigurationIn_t end, value_type extraLength,
value_type rho, size_type xyId, size_type rzId,
const std::vector<JointPtr_t> wheels) :
parent_t (interval_t (0, 1.), robot->configSize (),
robot->numberDof ()),
device_ (robot), initial_ (init), end_ (end),
xyId_ (xyId), rzId_ (rzId), typeId_ (0), rho_ (rho)
parent_t (robot->configSize (), robot->numberDof ()),
device_ (robot), initial_ (init), end_ (end), xyId_ (xyId), rzId_ (rzId),
wheels_ (wheels), typeId_ (0), extraLength_ (extraLength), rho_ (rho)
{
assert (robot);
assert (rho_ > 0);
......@@ -114,19 +96,18 @@ namespace hpp {
q0 [2] = atan2 (initial_ [rzId_ + 1], initial_ [rzId_ + 0]);
q1 [2] = atan2 (end_ [rzId_ + 1], end_ [rzId_ + 0]);
dubins_init (q0, q1);
setWheelJoints (robot->getJointAtConfigRank (rzId), wheels);
}
DubinsPath::DubinsPath (const DevicePtr_t& robot, ConfigurationIn_t init,
ConfigurationIn_t end, value_type rho,
size_type xyId, size_type rzId,
ConfigurationIn_t end, value_type extraLength,
value_type rho, size_type xyId, size_type rzId,
const std::vector<JointPtr_t> wheels,
ConstraintSetPtr_t constraints) :
parent_t (interval_t (0, 1.), robot->configSize (),
robot->numberDof (), constraints),
device_ (robot), initial_ (init), end_ (end),
xyId_ (xyId), rzId_ (rzId), typeId_ (0), rho_ (rho)
parent_t (robot->configSize (), robot->numberDof ()),
device_ (robot), initial_ (init), end_ (end), xyId_ (xyId), rzId_ (rzId),
wheels_ (wheels), typeId_ (0), extraLength_ (extraLength), rho_ (rho)
{
this->constraints (constraints);
assert (robot);
assert (rho_ > 0);
vector3_t q0, q1;
......@@ -135,27 +116,24 @@ namespace hpp {
q0 [2] = atan2 (initial_ [rzId_ + 1], initial_ [rzId_ + 0]);
q1 [2] = atan2 (end_ [rzId_ + 1], end_ [rzId_ + 0]);
dubins_init (q0, q1);
setWheelJoints (robot->getJointAtConfigRank (rzId), wheels);
}
DubinsPath::DubinsPath (const DubinsPath& path) :
parent_t (path), device_ (path.device_), initial_ (path.initial_),
end_ (path.end_), xyId_ (path.xyId_), rzId_ (path.rzId_),
dxyId_ (path.dxyId_), drzId_ (path.drzId_),
wheels_ (path.wheels_),
typeId_ (path.typeId_), lengths_(path.lengths_), rho_ (path.rho_),
qi_ (path.qi_)
dxyId_ (path.dxyId_), drzId_ (path.drzId_), wheels_ (path.wheels_),
typeId_ (path.typeId_), lengths_(path.lengths_),
extraLength_ (path.extraLength_), rho_ (path.rho_), qi_ (path.qi_)
{
}
DubinsPath::DubinsPath (const DubinsPath& path,
const ConstraintSetPtr_t& constraints) :
parent_t (path, constraints), device_ (path.device_),
initial_ (path.initial_), end_ (path.end_),
xyId_ (path.xyId_), rzId_ (path.rzId_),
wheels_ (path.wheels_),
typeId_ (path.typeId_), lengths_(path.lengths_), rho_ (path.rho_),
qi_ (path.qi_)
initial_ (path.initial_), end_ (path.end_), xyId_ (path.xyId_),
rzId_ (path.rzId_), wheels_ (path.wheels_), typeId_ (path.typeId_),
lengths_(path.lengths_), extraLength_ (path.extraLength_),
rho_ (path.rho_), qi_ (path.qi_)
{
}
......@@ -196,9 +174,6 @@ namespace hpp {
throw std::logic_error ("Failed to build Dubins path");
}
typeId_ = best_word;
interval_t tr = timeRange();
tr.second = rho_ * (lengths_ [0] + lengths_ [1] + lengths_ [2]);
timeRange (tr);
}
double fmodr( double x, double y)
......@@ -234,6 +209,51 @@ namespace hpp {
joint = device_->getJointAtConfigRank (rzId_);
offset = rzId_ - joint->rankInConfiguration ();
drzId_ = joint->rankInVelocity () + offset;
// Create constant curvature segments
std::vector <Configuration_t> q (4);
q [0] = initial_;
q [1].resize (device_->configSize ());
q [2].resize (device_->configSize ());
q [3] = end_;
std::vector <value_type> extraL (3);
value_type L (lengths_ [0] +lengths_ [1] +lengths_ [2]);
for (std::size_t i=0; i<3; ++i) {
extraL [i] = lengths_ [i] / L * extraLength_;
}
value_type l (lengths_ [0]);
pinocchio::interpolate
(device_, initial_.head (device_->configSize ()),
end_.head (device_->configSize ()), l/L, q[1]);
l += lengths_ [1];
pinocchio::interpolate
(device_, initial_.head (device_->configSize ()),
end_.head (device_->configSize ()), l/L, q[2]);
const int* types = DIRDATA [typeId_];
for (std::size_t i=0; i<3; ++i) {
value_type curvature;
switch (types [i]) {
case L_SEG:
curvature = 1./rho_;
break;
case S_SEG:
curvature = 0;
break;
case R_SEG:
curvature = -1./rho_;
break;
default:
assert (false && "Invalid Dubins segment type.");
}
ConstantCurvaturePtr_t path (ConstantCurvature::create
(device_, q [i], q [i+1],
rho_ * lengths_ [i],
rho_ * lengths_ [i] + extraL [i],
curvature, xyId_, rzId_,
device_->getJointAtConfigRank (rzId_),
wheels_, ConstraintSetPtr_t ()));
appendPath (path);
q [i+1] = path->end ();
}
}
void dubins_segment( double t, vector3_t qi, vector3_t& qt, int type)
......@@ -278,191 +298,5 @@ namespace hpp {
v [2] = 0;
}
}
bool DubinsPath::impl_compute (ConfigurationOut_t result,
value_type param) const
{
const value_type L = paramLength();
if (param <= paramRange ().first || L == 0) {
result = initial_;
return true;
}
if (param >= paramRange ().second) {
result = end_;
return true;
}
// Does a linear interpolation on all the joints.
const value_type u = (param - paramRange().first) / L;
pinocchio::interpolate (device_, initial_, end_, u, result);
// Compute the position of the car.
result.segment <2> (xyId_).setZero();
// tprime is the normalised variant of the parameter
double tprime = param / rho_;
// The computation is done in five stages.
//
// 1. translate the components of the initial configuration to the origin
// 2. generate the target configuration
// 3. transform the target configuration
// scale the target configuration
// translate the target configration back to the original starting
// point
// normalise the target configurations angular component
// The translated initial configuration
vector3_t qi; qi << 0, 0, qi_ [2];
// Generate the target configuration
const int* types = DIRDATA [typeId_];
int type;
double p1 = lengths_ [0];
double p2 = lengths_ [1];
vector3_t q1; // end-of segment 1
vector3_t q2; // end-of segment 2
vector3_t q;
dubins_segment( p1, qi, q1, types[0] );
dubins_segment( p2, q1, q2, types[1] );
if( tprime < p1 ) {
dubins_segment( tprime, qi, q, types[0] );
type = types [0];
}
else if( tprime < (p1+p2) ) {
dubins_segment( tprime-p1, q1, q, types[1] );
type = types [1];
}
else {
dubins_segment( tprime-p1-p2, q2, q, types[2] );
type = types [2];
}
// scale the target configuration, translate back to the original starting
// point
result [xyId_ + 0] = q[0] * rho_ + qi_ [0];
result [xyId_ + 1] = q[1] * rho_ + qi_ [1];
result [rzId_ + 0] = cos (q[2]);
result [rzId_ + 1] = sin (q[2]);
switch(type)
{
case L_SEG:
for (std::vector<Wheels_t>::const_iterator _w = wheels_.begin();
_w != wheels_.end(); ++_w)
result(_w->j->rankInConfiguration()) = _w->L;
break;
case R_SEG:
for (std::vector<Wheels_t>::const_iterator _w = wheels_.begin();
_w != wheels_.end(); ++_w)
result(_w->j->rankInConfiguration()) = _w->R;
break;
case S_SEG:
for (std::vector<Wheels_t>::const_iterator _w = wheels_.begin();
_w != wheels_.end(); ++_w)
result(_w->j->rankInConfiguration()) = _w->S;
break;
default:
assert (false && "No type in Dubins path.");
}
return true;
}
void DubinsPath::impl_derivative (vectorOut_t result, const value_type& t,
size_type order) const
{
value_type p (t);
if (order != 1) {
std::ostringstream oss;
oss << "derivative only implemented for order 1: got" << order;
HPP_THROW_EXCEPTION (hpp::Exception, oss.str ());
}
const value_type L = paramLength();
if (p <= paramRange ().first || L == 0) {
p = paramRange ().first;
}
if (p >= paramRange ().second) {
p = paramRange ().second;
}
// Does a linear interpolation on all the joints.
if (order > 1) {
result.setZero ();
}
else if (order == 1) {
pinocchio::difference (device_, end_, initial_, result);
result = (1/L) * result;
} else {
std::ostringstream oss;
oss << "order of derivative (" << order << ") should be positive.";
HPP_THROW_EXCEPTION (hpp::Exception, oss.str ());
}
// Compute the velocity of the car.
result.segment <2> (xyId_).setZero();
// tprime is the normalised variant of the parameter
double tprime = t / rho_;
// The computation is done in five stages.
//
// 1. translate the components of the initial configuration to the origin
// 2. generate the target configuration
// 3. transform the target configuration
// scale the target configuration
// translate the target configration back to the original starting
// point
// normalise the target configurations angular component
// The translated initial configuration
vector3_t qi; qi << 0, 0, qi_ [2];
// Generate the target configuration
const int* types = DIRDATA [typeId_];
int type;
double p1 = lengths_ [0];
double p2 = lengths_ [1];
vector3_t q1; // end-of segment 1
vector3_t q2; // end-of segment 2
vector3_t q;
dubins_segment (p1, qi, q1, types[0] );
dubins_segment (p2, q1, q2, types[1] );
if( tprime < p1 ) {
dubins_segment_velocity (tprime, qi, q, types[0] );
type = types [0];
}
else if( tprime < (p1+p2) ) {
dubins_segment_velocity (tprime-p1, q1, q, types[1] );
type = types [1];
}
else {
dubins_segment_velocity (tprime-p1-p2, q2, q, types[2] );
type = types [2];
}
// scale the target configuration, translate back to the original starting
// point
result [dxyId_ + 0] = q[0] * rho_ + qi_ [0];
result [dxyId_ + 1] = q[1] * rho_ + qi_ [1];
result [drzId_] = q[2];
switch(type)
{
case L_SEG:
for (std::vector<Wheels_t>::const_iterator _w = wheels_.begin();
_w != wheels_.end(); ++_w)
result(_w->j->rankInVelocity()) = 0;
break;
case R_SEG:
for (std::vector<Wheels_t>::const_iterator _w = wheels_.begin();
_w != wheels_.end(); ++_w)
result(_w->j->rankInVelocity()) = 0;
break;
case S_SEG:
for (std::vector<Wheels_t>::const_iterator _w = wheels_.begin();
_w != wheels_.end(); ++_w)
result(_w->j->rankInVelocity()) = 0;
break;
default:
assert (false && "No type in Dubins path.");
}
}
} // namespace hpp-core
} // namespace hpp
......@@ -586,7 +586,8 @@ namespace hpp {
assert (device);
assert (rho_ > 0);
lengths_.setZero ();
buildReedsShepp (device->getJointAtConfigRank (rzId), wheels, constraints);
buildReedsShepp (device->getJointAtConfigRank (rzId), wheels);
this->constraints (constraints);
assert (fabs (extraLength_ + currentLength_ - paramRange ().second) < 1e-8);
}
......@@ -625,8 +626,7 @@ namespace hpp {
}
void ReedsSheppPath::buildReedsShepp(const JointPtr_t rz,
const std::vector<JointPtr_t> wheels,
const ConstraintSetPtr_t constraints)
const std::vector<JointPtr_t> wheels)
{
// Find rank of translation and rotation in velocity vectors
// Hypothesis: degrees of freedom all belong to a planar joint or
......@@ -651,7 +651,8 @@ namespace hpp {
if (xy.squaredNorm () + phi*phi < 1e-8) {
ConstantCurvaturePtr_t segment
(ConstantCurvature::create (device_, qInit, end_, 0, extraLength_, 0,
xyId_, rzId_, rz, wheels, constraints));
xyId_, rzId_, rz, wheels,
ConstraintSetPtr_t ()));
appendPath (segment);
currentLength_ = 0;
return;
......@@ -690,7 +691,7 @@ namespace hpp {
rho_ * lengths_ [i] +
lengths_ [i] * extraLength_ / L,
curvature, xyId_, rzId_, rz, wheels,
constraints));
ConstraintSetPtr_t ()));
appendPath (segment);
qInit = segment->end ();
}
......
......@@ -19,6 +19,7 @@
# include <hpp/pinocchio/device.hh>
# include <hpp/pinocchio/joint.hh>
# include <hpp/core/distance.hh>
# include <hpp/core/problem.hh>
# include <hpp/core/dubins-path.hh>
......@@ -28,8 +29,20 @@ namespace hpp {
PathPtr_t Dubins::impl_compute (ConfigurationIn_t q1,
ConfigurationIn_t q2) const
{
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
DistancePtr_t d (problem().distance());
value_type extraL = (*d) (q1, qEnd);
DubinsPathPtr_t path =
DubinsPath::create (device_.lock (), q1, q2,
DubinsPath::create (device_.lock (), q1, q2, extraL,
rho_ , xyId_, rzId_, wheels_, constraints ());
return path;
}
......
......@@ -19,6 +19,7 @@
# include <hpp/pinocchio/device.hh>
# include <hpp/pinocchio/joint.hh>
# include <hpp/core/distance.hh>
# include <hpp/core/problem.hh>
# include <hpp/core/dubins-path.hh>
......@@ -28,8 +29,20 @@ namespace hpp {
PathPtr_t Snibud::impl_compute (ConfigurationIn_t q1,
ConfigurationIn_t q2) const
{
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 ();