CartesianInterface/include/cartesian_interface/sdk/problem/Cartesian.h at 6d9705dba53a018795567838590b935ce187c9e7 · ADVRHumanoids/CartesianInterface · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
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
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
#ifndef __XBOT_CARTESIAN_PROBLEM_CARTESIAN_IMPL_H__
#define __XBOT_CARTESIAN_PROBLEM_CARTESIAN_IMPL_H__

#include <cartesian_interface/sdk/problem/Task.h>
#include <cartesian_interface/problem/Cartesian.h>

#include <cartesian_interface/Enum.h>
#include <cartesian_interface/Context.h>
#include <cartesian_interface/trajectory/Trajectory.h>

namespace Reflexxes {
namespace Utils
{
class TrajectoryGenerator;
}
}

namespace XBot { namespace Cartesian {

class CartesianTaskImpl :
    public TaskDescriptionImpl,
    public virtual CartesianTask
{

public:

    CARTESIO_DECLARE_SMART_PTR(CartesianTaskImpl)

    /* Constructors */
    CartesianTaskImpl() = default;

    CartesianTaskImpl(Context::ConstPtr context,
                      std::string name,
                      std::string distal_link,
                      std::string base_link = "world");

    CartesianTaskImpl(Context::ConstPtr context,
                      std::string name,
                      std::string type,
                      std::string distal_link,
                      std::string base_link);

    CartesianTaskImpl(YAML::Node node, Context::ConstPtr context);

    virtual bool validate() override;

    /* Parameters */
    bool isSubtaskLocal() const override;
    bool isVelocityLocal() const override;
    void setIsVelocityLocal(const bool is_velocity_local) override;

    /* Safety limits */
    virtual void getVelocityLimits(double& max_vel_lin,
                                   double& max_vel_ang) const;

    virtual void getAccelerationLimits(double& max_acc_lin,
                                       double& max_acc_ang) const;

    virtual void setVelocityLimits(double max_vel_lin,
                                   double max_vel_ang);

    virtual void setAccelerationLimits(double max_acc_lin,
                                       double max_acc_ang);

    /**
         * @brief enableOtg enables online trajectory generation,
         * which enforces safety limtis to the Cartesian reference signals
         */
    void enableOnlineTrajectoryGeneration() override;

    virtual State getTaskState() const;

    virtual const std::string& getBaseLink() const;
    virtual bool setBaseLink(const std::string& new_base_link);

    virtual const std::string& getDistalLink() const;

    ControlType getControlMode() const;
    bool setControlMode(const ControlType & value);

    virtual bool getCurrentPose(Eigen::Affine3d& base_T_ee) const; // ?

    virtual bool getPoseReference(Eigen::Affine3d& base_T_ref,
                                  Eigen::Vector6d * base_vel_ref = nullptr,
                                  Eigen::Vector6d * base_acc_ref = nullptr) const;

    virtual bool getPoseReferenceRaw(Eigen::Affine3d& base_T_ref,
                                     Eigen::Vector6d * base_vel_ref = nullptr,
                                     Eigen::Vector6d * base_acc_ref = nullptr) const;

    virtual bool setPoseReference(const Eigen::Affine3d& base_T_ref);

    virtual bool setPoseReferenceRaw(const Eigen::Affine3d& base_T_ref);

    virtual bool setVelocityReference(const Eigen::Vector6d& base_vel_ref);

    virtual bool setAccelerationReference(const Eigen::Vector6d& base_acc_ref);

    virtual bool getPoseTarget(Eigen::Affine3d& base_T_ref) const;

    virtual bool setPoseTarget(const Eigen::Affine3d& base_T_ref,
                               double time);

    virtual int getCurrentSegmentId() const;

    virtual bool setWayPoints(const Trajectory::WayPointVector& way_points);

    virtual void abort();

    void update(double time, double period) override;

    void reset() override;

    virtual void log(MatLogger2::Ptr logger,
                     bool init_logger = false,
                     int buf_size = 1e5) override;

    virtual void registerObserver(TaskObserver::WeakPtr observer);

private:

    ControlType _ctrl_mode;

    std::list<CartesianTaskObserver::WeakPtr> _observers;

    /**
         * @brief The task controls the relative motion of distal_link w.r.t base_link
         */
    std::string _base_link, _distal_link;

    /**
         * @brief Set Cartesian task in local (body) frame
         */
    bool _is_body_jacobian;

    bool _is_velocity_local;

    /**
         * @brief Parameter that weights orientation errors w.r.t. position errors.
         * For example, setting it to orientation_gain = 2.0 means that an error of
         * 2 rad is recovered in the same time as an error of 1 meter.
         */
    double _orientation_gain;

    /* Task implementation variables (TBD: pimpl) */

    typedef Eigen::Matrix<double, 7, 1> EigenVector7d;

    bool check_reach() const;
    void apply_otg();
    void reset_otg();
    Eigen::Affine3d get_pose_ref_otg() const;
    virtual Eigen::Affine3d get_current_pose() const;

    Eigen::Affine3d _T;
    Eigen::Vector6d _vel;
    Eigen::Vector6d _acc;

    EigenVector7d _otg_des, _otg_ref, _otg_vref;
    EigenVector7d _otg_maxvel, _otg_maxacc;

    State _state;
    double _vref_time_to_live;
    double _aref_time_to_live;

    Trajectory::Ptr _trajectory;
    std::shared_ptr<Reflexxes::Utils::TrajectoryGenerator> _otg;


};

} }


#endif