effector_spline_rotation.h 11.1 KB
Newer Older
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23
/**
* \file exact_cubic.h
* \brief class allowing to create an Exact cubic spline.
* \author Steve T.
* \version 0.1
* \date 06/17/2013
*
* This file contains definitions for the ExactCubic class.
* Given a set of waypoints (x_i*) and timestep (t_i), it provides the unique set of
* cubic splines fulfulling those 4 restrictions :
* - x_i(t_i) = x_i* ; this means that the curve passes trough each waypoint
* - x_i(t_i+1) = x_i+1* ;
* - its derivative is continous at t_i+1
* - its acceleration is continous at t_i+1
* more details in paper "Task-Space Trajectories via Cubic Spline Optimization"
* By J. Zico Kolter and Andrew Y.ng (ICRA 2009)
*/


#ifndef _CLASS_EFFECTOR_SPLINE_ROTATION
#define _CLASS_EFFECTOR_SPLINE_ROTATION

#include "spline/helpers/effector_spline.h"
24
#include "spline/curve_abc.h"
25 26 27 28 29 30
#include <Eigen/Geometry>

namespace spline
{
namespace helpers
{
31

32 33 34 35
typedef Eigen::Matrix<Numeric, 4, 1> quat_t;
typedef Eigen::Ref<quat_t> quat_ref_t;
typedef const Eigen::Ref<const quat_t> quat_ref_const_t;
typedef Eigen::Matrix<Numeric, 7, 1> config_t;
36 37 38
typedef curve_abc<Time, Numeric, 4, false, quat_t> curve_abc_quat_t;
typedef std::pair<Numeric, quat_t > waypoint_quat_t;
typedef std::vector<waypoint_quat_t> t_waypoint_quat_t;
39 40 41 42 43 44
typedef Eigen::Matrix<Numeric, 1, 1> point_one_dim_t;
typedef spline_deriv_constraint <Numeric, Numeric, 1, false, point_one_dim_t> spline_deriv_constraint_one_dim;
typedef std::pair<Numeric, point_one_dim_t > waypoint_one_dim_t;
typedef std::vector<waypoint_one_dim_t> t_waypoint_one_dim_t;


45
class rotation_spline: public curve_abc_quat_t
46 47
{
    public:
48 49 50 51 52 53
     rotation_spline (quat_ref_const_t quat_from=quat_t(0,0,0,1), quat_ref_const_t quat_to=quat_t(0,0,0,1),
                               const double min = 0., const double max = 1.)
         : curve_abc_quat_t()
         , quat_from_(quat_from.data()), quat_to_(quat_to.data()), min_(min), max_(max)
         , time_reparam_(computeWayPoints())
     {}
54

55 56 57 58 59 60 61 62 63
    ~rotation_spline() {}

    /* Copy Constructors / operator=*/
    rotation_spline& operator=(const rotation_spline& from)
    {
        quat_from_ = from.quat_from_;
        quat_to_ = from.quat_to_;
        min_ =from.min_; max_ = from.max_;
        time_reparam_ = spline_deriv_constraint_one_dim(from.time_reparam_);
stevet's avatar
stevet committed
64
        return *this;
65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81
    }
    /* Copy Constructors / operator=*/

    quat_t operator()(const Numeric t) const
    {
        if(t<=min()) return quat_from_.coeffs();
        if(t>=max()) return quat_to_.coeffs();
        //normalize u
        Numeric u = (t - min()) /(max() - min());
        // reparametrize u
        return quat_from_.slerp(time_reparam_(u)[0], quat_to_).coeffs();
    }

    virtual quat_t derivate(time_t /*t*/, std::size_t /*order*/) const
    {
        throw std::runtime_error("TODO quaternion spline does not implement derivate");
    }
82 83 84 85 86 87 88 89 90

    spline_deriv_constraint_one_dim computeWayPoints() const
    {
        // initializing time reparametrization for spline
        t_waypoint_one_dim_t waypoints;
        waypoints.push_back(std::make_pair(0,point_one_dim_t::Zero()));
        waypoints.push_back(std::make_pair(1,point_one_dim_t::Ones()));
        return spline_deriv_constraint_one_dim(waypoints.begin(), waypoints.end());
    }
91 92 93 94 95 96 97 98 99 100

    virtual time_t min() const{return min_;}
    virtual time_t max() const{return max_;}

    public:
    Eigen::Quaterniond quat_from_; //const
    Eigen::Quaterniond quat_to_;   //const
    double min_;                   //const
    double max_;                   //const
    spline_deriv_constraint_one_dim time_reparam_; //const
101
};
102

103 104 105 106

typedef exact_cubic<Time, Numeric, 4, false, quat_t, std::vector<quat_t,Eigen::aligned_allocator<quat_t> >, rotation_spline> exact_cubic_quat_t;


107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143
/// \class effector_spline_rotation
/// \brief Represents a trajectory for and end effector
/// uses the method effector_spline to create a spline trajectory.
/// Additionally, handles the rotation of the effector as follows:
/// does not rotate during the take off and landing phase,
/// then uses a SLERP algorithm to interpolate the rotation in the quaternion
/// space.
class effector_spline_rotation
{
    /* Constructors - destructors */
    public:
    /// \brief Constructor.
    /// Given a set of waypoints, and the normal vector of the start and
    /// ending positions, automatically create the spline such that:
    /// + init and end velocities / accelerations are 0.
    /// + the effector lifts and lands exactly in the direction of the specified normals
    /// \param wayPointsBegin   : an iterator pointing to the first element of a waypoint container
    /// \param wayPointsEnd     : an iterator pointing to the end           of a waypoint container
    /// \param to_quat          : 4D vector, quaternion indicating rotation at take off(x, y, z, w)
    /// \param land_quat        : 4D vector, quaternion indicating rotation at landing (x, y, z, w)
    /// \param lift_normal      : normal to be followed by end effector at take-off
    /// \param land_normal      : normal to be followed by end effector at landing
    /// \param lift_offset      : length of the straight line along normal at take-off
    /// \param land_offset      : length of the straight line along normal at landing
    /// \param lift_offset_duration : time travelled along straight line at take-off
    /// \param land_offset_duration : time travelled along straight line at landing
    ///
    template<typename In>
    effector_spline_rotation(In wayPointsBegin, In wayPointsEnd,
            quat_ref_const_t& to_quat=quat_t(0,0,0,1), quat_ref_const_t& land_quat=quat_t(0,0,0,1),
            const Point& lift_normal=Eigen::Vector3d::UnitZ(), const Point& land_normal=Eigen::Vector3d::UnitZ(),
            const Numeric lift_offset=0.02, const Numeric land_offset=0.02,
            const Time lift_offset_duration=0.02, const Time land_offset_duration=0.02)
        : spline_(effector_spline(wayPointsBegin, wayPointsEnd, lift_normal, land_normal, lift_offset, land_offset, lift_offset_duration, land_offset_duration))
        , to_quat_(to_quat.data()), land_quat_(land_quat.data())
        , time_lift_offset_(spline_->min()+lift_offset_duration)
        , time_land_offset_(spline_->max()-land_offset_duration)
144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 175 176 177
        , quat_spline_(simple_quat_spline())
    {
        // NOTHING
    }

    /// \brief Constructor.
    /// Given a set of waypoints, and the normal vector of the start and
    /// ending positions, automatically create the spline such that:
    /// + init and end velocities / accelerations are 0.
    /// + the effector lifts and lands exactly in the direction of the specified normals
    /// \param wayPointsBegin       : an iterator pointing to the first element of a waypoint container
    /// \param wayPointsEnd         : an iterator pointing to the end           of a waypoint container
    /// \param quatWayPointsBegin   : en iterator pointing to the first element of a 4D vector (x, y, z, w) container of
    ///  quaternions indicating rotation at specific time steps.
    /// \param quatWayPointsEnd     : en iterator pointing to the end           of a 4D vector (x, y, z, w) container of
    ///  quaternions indicating rotation at specific time steps.
    /// \param lift_normal          : normal to be followed by end effector at take-off
    /// \param land_normal          : normal to be followed by end effector at landing
    /// \param lift_offset          : length of the straight line along normal at take-off
    /// \param land_offset          : length of the straight line along normal at landing
    /// \param lift_offset_duration : time travelled along straight line at take-off
    /// \param land_offset_duration : time travelled along straight line at landing
    ///
    template<typename In, typename InQuat>
    effector_spline_rotation(In wayPointsBegin, In wayPointsEnd,
            InQuat quatWayPointsBegin, InQuat quatWayPointsEnd,
            const Point& lift_normal=Eigen::Vector3d::UnitZ(), const Point& land_normal=Eigen::Vector3d::UnitZ(),
            const Numeric lift_offset=0.02, const Numeric land_offset=0.02,
            const Time lift_offset_duration=0.02, const Time land_offset_duration=0.02)
        : spline_(effector_spline(wayPointsBegin, wayPointsEnd, lift_normal, land_normal, lift_offset, land_offset, lift_offset_duration, land_offset_duration))
        , to_quat_((quatWayPointsBegin->second).data()), land_quat_(((quatWayPointsEnd-1)->second).data())
        , time_lift_offset_(spline_->min()+lift_offset_duration)
        , time_land_offset_(spline_->max()-land_offset_duration)
        , quat_spline_(quat_spline(quatWayPointsBegin, quatWayPointsEnd))
178 179 180 181 182 183 184 185 186 187 188 189 190 191 192 193 194 195 196 197 198 199 200 201 202 203 204 205
    {
        // NOTHING
    }

    ~effector_spline_rotation() {delete spline_;}
    /* Constructors - destructors */

    /*Helpers*/
    public:
    Numeric min() const{return spline_->min();}
    Numeric max() const{return spline_->max();}
    /*Helpers*/

    /*Operations*/
    public:
    ///  \brief Evaluation of the effector position and rotation at time t.
    ///  \param t : the time when to evaluate the spline
    ///  \param return : a 7D vector; The 3 first values are the 3D position, the 4 last the
    ///  quaternion describing the rotation
    ///
    config_t operator()(const Numeric t) const
    {
        config_t res;
        res.head<3>() = (*spline_)(t);
        res.tail<4>() = interpolate_quat(t);
        return res;
    }

206 207
    public:
    quat_t interpolate_quat(const Numeric t) const
208
    {
209 210
        if(t<=time_lift_offset_) return to_quat_.coeffs();
        if(t>=time_land_offset_) return land_quat_.coeffs();
211 212 213 214 215 216 217 218 219 220 221 222 223 224 225 226 227 228 229 230 231 232 233 234 235 236 237 238
        return quat_spline_(t);
    }

    private:
    exact_cubic_quat_t simple_quat_spline() const
    {
        std::vector<rotation_spline> splines;
        splines.push_back(rotation_spline(to_quat_.coeffs(),land_quat_.coeffs(),time_lift_offset_, time_land_offset_));
        return exact_cubic_quat_t(splines);
    }

    template <typename InQuat>
    exact_cubic_quat_t quat_spline(InQuat quatWayPointsBegin, InQuat quatWayPointsEnd) const
    {
        if(std::distance(quatWayPointsBegin, quatWayPointsEnd) < 1)
            return simple_quat_spline();
        exact_cubic_quat_t::t_spline_t splines;
        InQuat it(quatWayPointsBegin);
        Time init = time_lift_offset_;
        Eigen::Quaterniond current_quat = to_quat_;
        for(; it != quatWayPointsEnd; ++it)
        {
            splines.push_back(rotation_spline(current_quat.coeffs(), it->second, init, it->first));
            current_quat = it->second;
            init = it->first;
        }
        splines.push_back(rotation_spline(current_quat.coeffs(), land_quat_.coeffs(), init, time_land_offset_));
        return exact_cubic_quat_t(splines);
239
    }
240

241 242 243 244 245 246 247 248 249
    /*Operations*/

    /*Attributes*/
    public:
    const exact_cubic_t* spline_;
    const Eigen::Quaterniond to_quat_;
    const Eigen::Quaterniond land_quat_;
    const double time_lift_offset_;
    const double time_land_offset_;
250
    const exact_cubic_quat_t quat_spline_;
251 252 253 254 255 256 257
    /*Attributes*/
};

} // namespace helpers
} // namespace spline
#endif //_CLASS_EFFECTOR_SPLINE_ROTATION