Skip to content

GitLab

Commit 79a1d351 authored by Pierre Fernbach's avatar Pierre Fernbach
Browse files

Add a new parameter : kinodynamic/forceYawOrientation to only adjust the... 

Add a new parameter : kinodynamic/forceYawOrientation to only adjust the orientation along the z axis
parent 1c29a3ae
Hide whitespace changes
Inline Side-by-side
include/hpp/core/kinodynamic-oriented-path.hh
View file @ 79a1d351
......@@ -39,9 +39,9 @@ namespace hpp{
static KinodynamicOrientedPathPtr_t create (const DevicePtr_t& device,
ConfigurationIn_t init,
ConfigurationIn_t end,
value_type length,ConfigurationIn_t a1,ConfigurationIn_t t0,ConfigurationIn_t t1,ConfigurationIn_t tv,ConfigurationIn_t t2,ConfigurationIn_t vLim)
value_type length,ConfigurationIn_t a1,ConfigurationIn_t t0,ConfigurationIn_t t1,ConfigurationIn_t tv,ConfigurationIn_t t2,ConfigurationIn_t vLim, bool ignoreZValue = false)
{
KinodynamicOrientedPath* ptr = new KinodynamicOrientedPath (device, init, end, length,a1,t0,t1,tv,t2,vLim);
KinodynamicOrientedPath* ptr = new KinodynamicOrientedPath (device, init, end, length,a1,t0,t1,tv,t2,vLim,ignoreZValue);
KinodynamicOrientedPathPtr_t shPtr (ptr);
ptr->init (shPtr);
ptr->checkPath ();
......@@ -57,10 +57,10 @@ namespace hpp{
ConfigurationIn_t init,
ConfigurationIn_t end,
value_type length,ConfigurationIn_t a1,ConfigurationIn_t t0,ConfigurationIn_t t1,ConfigurationIn_t tv,ConfigurationIn_t t2,ConfigurationIn_t vLim,
ConstraintSetPtr_t constraints)
ConstraintSetPtr_t constraints, bool ignoreZValue = false)
{
KinodynamicOrientedPath* ptr = new KinodynamicOrientedPath (device, init, end, length,a1,t0,t1,tv,t2,vLim,
constraints);
constraints,ignoreZValue);
KinodynamicOrientedPathPtr_t shPtr (ptr);
ptr->init (shPtr);
ptr->checkPath ();
......@@ -78,9 +78,9 @@ namespace hpp{
return shPtr;
}
static KinodynamicOrientedPathPtr_t create (const KinodynamicPathPtr_t& path)
static KinodynamicOrientedPathPtr_t create (const KinodynamicPathPtr_t& path,bool ignoreZValue = false)
{
KinodynamicOrientedPath* ptr = new KinodynamicOrientedPath (*path);
KinodynamicOrientedPath* ptr = new KinodynamicOrientedPath (*path,ignoreZValue);
KinodynamicOrientedPathPtr_t shPtr (ptr);
ptr->init (shPtr);
ptr->checkPath ();
......@@ -118,6 +118,8 @@ namespace hpp{
return createCopy (weak_.lock (), constraints);
}
bool ignoreZValue() const {return ignoreZValue_;}
void ignoreZValue(bool ignoreZValue){ignoreZValue_ = ignoreZValue;}
protected:
/// Print path in a stream
......@@ -135,19 +137,19 @@ namespace hpp{
/// Constructor
KinodynamicOrientedPath (const DevicePtr_t& robot, ConfigurationIn_t init,
ConfigurationIn_t end, value_type length, ConfigurationIn_t a1,ConfigurationIn_t t0, ConfigurationIn_t t1, ConfigurationIn_t tv, ConfigurationIn_t t2, ConfigurationIn_t vLim);
ConfigurationIn_t end, value_type length, ConfigurationIn_t a1, ConfigurationIn_t t0, ConfigurationIn_t t1, ConfigurationIn_t tv, ConfigurationIn_t t2, ConfigurationIn_t vLim,bool ignoreZValue );
/// Constructor with constraints
KinodynamicOrientedPath (const DevicePtr_t& robot, ConfigurationIn_t init,
ConfigurationIn_t end, value_type length,ConfigurationIn_t a1,ConfigurationIn_t t0,ConfigurationIn_t t1,ConfigurationIn_t tv,ConfigurationIn_t t2,ConfigurationIn_t vLim,
ConstraintSetPtr_t constraints);
ConstraintSetPtr_t constraints,bool ignoreZValue);
/// Copy constructor
KinodynamicOrientedPath (const KinodynamicOrientedPath& path);
/// constructor from KinodynamicPath
KinodynamicOrientedPath (const KinodynamicPath& path);
KinodynamicOrientedPath (const KinodynamicPath& path,bool ignoreZValue);
/// Copy constructor with constraints
KinodynamicOrientedPath (const KinodynamicOrientedPath& path,
......@@ -168,6 +170,7 @@ namespace hpp{
private:
KinodynamicOrientedPathWkPtr_t weak_;
bool ignoreZValue_;
};//class kinodynamic oriented path
}//namespace core
}//namespace hpp
......
include/hpp/core/steering-method/steering-kinodynamic.hh
View file @ 79a1d351
......@@ -129,7 +129,7 @@ namespace hpp {
double aMaxFixed_Z_;
bool synchronizeVerticalAxis_;
bool orientedPath_;
bool orientationIgnoreZValue_;
private:
DeviceWkPtr_t device_;
KinodynamicWkPtr_t weak_;
......
src/kinodynamic-oriented-path.cc
View file @ 79a1d351
......@@ -26,6 +26,11 @@ namespace hpp {
size_type configSize = device->configSize() - device->extraConfigSpace().dimension ();
Eigen::Vector3d vi(initial_[configSize],initial_[configSize+1],initial_[configSize+2]);
Eigen::Vector3d ve(end_[configSize],end_[configSize+1],end_[configSize+2]);
hppDout(notice,"OrienteInitAndGoal: ignoreZValue = "<<ignoreZValue_);
if(ignoreZValue_){
vi[2] = 0.;
ve[2] = 0.;
}
if(vi.norm() > 0){ // if velocity in the state
Eigen::Quaterniond quat = Eigen::Quaterniond::FromTwoVectors(Eigen::Vector3d::UnitX(),vi);
initial_[6]=quat.w();
......@@ -45,8 +50,8 @@ namespace hpp {
KinodynamicOrientedPath::KinodynamicOrientedPath (const DevicePtr_t& device,
ConfigurationIn_t init,
ConfigurationIn_t end,
value_type length, ConfigurationIn_t a1, ConfigurationIn_t t0, ConfigurationIn_t t1, ConfigurationIn_t tv, ConfigurationIn_t t2, ConfigurationIn_t vLim) :
parent_t (device,init,end,length,a1,t0,t1,tv,t2,vLim)
value_type length, ConfigurationIn_t a1, ConfigurationIn_t t0, ConfigurationIn_t t1, ConfigurationIn_t tv, ConfigurationIn_t t2, ConfigurationIn_t vLim, bool ignoreZValue) :
parent_t (device,init,end,length,a1,t0,t1,tv,t2,vLim),ignoreZValue_(ignoreZValue)
{
orienteInitAndGoal(device);
}
......@@ -54,29 +59,30 @@ namespace hpp {
KinodynamicOrientedPath::KinodynamicOrientedPath (const DevicePtr_t& device,
ConfigurationIn_t init,
ConfigurationIn_t end,
value_type length, ConfigurationIn_t a1, ConfigurationIn_t t0,ConfigurationIn_t t1, ConfigurationIn_t tv, ConfigurationIn_t t2, ConfigurationIn_t vLim,
ConstraintSetPtr_t constraints) :
parent_t (device,init,end,length,a1,t0,t1,tv,t2,vLim,constraints)
value_type length, ConfigurationIn_t a1, ConfigurationIn_t t0, ConfigurationIn_t t1, ConfigurationIn_t tv, ConfigurationIn_t t2, ConfigurationIn_t vLim,
ConstraintSetPtr_t constraints, bool ignoreZValue) :
parent_t (device,init,end,length,a1,t0,t1,tv,t2,vLim,constraints),ignoreZValue_(ignoreZValue)
{
orienteInitAndGoal(device);
}
KinodynamicOrientedPath::KinodynamicOrientedPath (const KinodynamicOrientedPath& path) :
parent_t (path)
parent_t (path),ignoreZValue_(path.ignoreZValue())
{
hppDout(notice,"Create copy called, ignoreZValue = "<<ignoreZValue_);
orienteInitAndGoal(path.device());
}
KinodynamicOrientedPath::KinodynamicOrientedPath (const KinodynamicPath& path) :
parent_t (path)
KinodynamicOrientedPath::KinodynamicOrientedPath (const KinodynamicPath& path,bool ignoreZValue) :
parent_t (path),ignoreZValue_(ignoreZValue)
{
orienteInitAndGoal(path.device());
}
KinodynamicOrientedPath::KinodynamicOrientedPath (const KinodynamicOrientedPath& path,
const ConstraintSetPtr_t& constraints) :
parent_t (path, constraints)
parent_t (path, constraints),ignoreZValue_(path.ignoreZValue())
{
orienteInitAndGoal(path.device());
}
......@@ -89,6 +95,8 @@ namespace hpp {
// FIX ME : assume first joint is freeflyer
size_type configSize = device()->configSize() - device()->extraConfigSpace().dimension ();
Eigen::Vector3d v(result[configSize],result[configSize+1],result[configSize+2]);
if(ignoreZValue_)
v[2] = 0;
if(v.norm() > 0){ // if velocity in the state
Eigen::Quaterniond quat = Eigen::Quaterniond::FromTwoVectors(Eigen::Vector3d::UnitX(),v);
result[6]=quat.w();
......@@ -105,7 +113,7 @@ namespace hpp {
PathPtr_t path = parent_t::impl_extract(subInterval);
KinodynamicPathPtr_t kinoPath = boost::dynamic_pointer_cast<KinodynamicPath>(path);
if(kinoPath)
return KinodynamicOrientedPath::create(kinoPath);
return KinodynamicOrientedPath::create(kinoPath,ignoreZValue_);
else{
hppDout(error,"Error while casting path in KinodynamicOrientedPath::extract");
return PathPtr_t();
......
src/steering-method/steering-kinodynamic.cc
View file @ 79a1d351
......@@ -157,7 +157,7 @@ namespace hpp {
KinodynamicPathPtr_t path = KinodynamicPath::create (device_.lock (), q1, q2,length,a1_t,t0_t,t1_t,tv_t,t2_t,vLim_t);
if(orientedPath_){
KinodynamicOrientedPathPtr_t orientedPath = KinodynamicOrientedPath::create(path);
KinodynamicOrientedPathPtr_t orientedPath = KinodynamicOrientedPath::create(path,orientationIgnoreZValue_);
return orientedPath;
}else{
return path;
......@@ -188,13 +188,15 @@ namespace hpp {
synchronizeVerticalAxis_ = problem_.getParameter(std::string("Kinodynamic/synchronizeVerticalAxis")).boolValue();
hppDout(notice,"synchronizeVerticalAxis in steering method = "<<synchronizeVerticalAxis_);
orientedPath_ = problem_.getParameter(std::string("Kinodynamic/forceAllOrientation")).boolValue();
orientedPath_ = problem_.getParameter(std::string("Kinodynamic/forceAllOrientation")).boolValue() || problem_.getParameter(std::string("Kinodynamic/forceYawOrientation")).boolValue();
orientationIgnoreZValue_ = problem_.getParameter(std::string("Kinodynamic/forceYawOrientation")).boolValue() && !problem_.getParameter(std::string("Kinodynamic/forceAllOrientation")).boolValue() ;
hppDout(notice,"oriented path : "<<orientedPath_);
hppDout(notice,"oriented path only constraint yaw (ignore z value) : "<<orientationIgnoreZValue_);
}
/// Copy constructor
Kinodynamic::Kinodynamic (const Kinodynamic& other) :
SteeringMethod (other),aMax_(other.aMax_),vMax_(other.vMax_),synchronizeVerticalAxis_(other.synchronizeVerticalAxis_),orientedPath_(other.orientedPath_),device_ (other.device_)
SteeringMethod (other),aMax_(other.aMax_),vMax_(other.vMax_),synchronizeVerticalAxis_(other.synchronizeVerticalAxis_),orientedPath_(other.orientedPath_),orientationIgnoreZValue_(other.orientationIgnoreZValue_),device_ (other.device_)
{
}
......@@ -533,6 +535,10 @@ namespace hpp {
"Kinodynamic/forceAllOrientation",
"If true, the orientation of the root is always aligned with the velocity",
Parameter(false)));
Problem::declareParameter(ParameterDescription (Parameter::BOOL,
"Kinodynamic/forceYawOrientation",
"If true, the yaw orientation (along z axis) of the root is always aligned with the velocity",
Parameter(false)));
Problem::declareParameter(ParameterDescription (Parameter::BOOL,
"Kinodynamic/synchronizeVerticalAxis",
"If false, the acceleration along the vertical axis is not synchronized wwith the other axis. This result in a greater vertical acceleration with a longer phase at constant velocity, resulting in a motion with less displacement along the vertical axis",
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment