1 |
|
|
// |
2 |
|
|
// Copyright (c) 2023 MIPT |
3 |
|
|
// |
4 |
|
|
// This file is part of tsid |
5 |
|
|
// tsid is free software: you can redistribute it |
6 |
|
|
// and/or modify it under the terms of the GNU Lesser General Public |
7 |
|
|
// License as published by the Free Software Foundation, either version |
8 |
|
|
// 3 of the License, or (at your option) any later version. |
9 |
|
|
// tsid is distributed in the hope that it will be |
10 |
|
|
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty |
11 |
|
|
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU |
12 |
|
|
// General Lesser Public License for more details. You should have |
13 |
|
|
// received a copy of the GNU Lesser General Public License along with |
14 |
|
|
// tsid If not, see |
15 |
|
|
// <http://www.gnu.org/licenses/>. |
16 |
|
|
// |
17 |
|
|
|
18 |
|
|
#ifndef __invdyn_task_frames_equality_hpp__ |
19 |
|
|
#define __invdyn_task_frames_equality_hpp__ |
20 |
|
|
|
21 |
|
|
#include "tsid/tasks/task-motion.hpp" |
22 |
|
|
#include "tsid/trajectories/trajectory-base.hpp" |
23 |
|
|
#include "tsid/math/constraint-equality.hpp" |
24 |
|
|
|
25 |
|
|
#include <pinocchio/multibody/model.hpp> |
26 |
|
|
#include <pinocchio/multibody/data.hpp> |
27 |
|
|
|
28 |
|
|
namespace tsid { |
29 |
|
|
namespace tasks { |
30 |
|
|
|
31 |
|
|
class TaskTwoFramesEquality : public TaskMotion { |
32 |
|
|
public: |
33 |
|
|
EIGEN_MAKE_ALIGNED_OPERATOR_NEW |
34 |
|
|
|
35 |
|
|
typedef math::Index Index; |
36 |
|
|
typedef trajectories::TrajectorySample TrajectorySample; |
37 |
|
|
typedef math::Vector Vector; |
38 |
|
|
typedef math::ConstraintEquality ConstraintEquality; |
39 |
|
|
typedef pinocchio::Data Data; |
40 |
|
|
typedef pinocchio::Data::Matrix6x Matrix6x; |
41 |
|
|
typedef pinocchio::Motion Motion; |
42 |
|
|
typedef pinocchio::SE3 SE3; |
43 |
|
|
|
44 |
|
|
TaskTwoFramesEquality(const std::string& name, RobotWrapper& robot, |
45 |
|
|
const std::string& frameName1, |
46 |
|
|
const std::string& frameName2); |
47 |
|
|
|
48 |
|
|
int dim() const; |
49 |
|
|
|
50 |
|
|
const ConstraintBase& compute(const double t, ConstRefVector q, |
51 |
|
|
ConstRefVector v, Data& data); |
52 |
|
|
|
53 |
|
|
const ConstraintBase& getConstraint() const; |
54 |
|
|
|
55 |
|
|
/** Return the desired task acceleration (after applying the specified mask). |
56 |
|
|
* The value is expressed in local frame is the local_frame flag is true, |
57 |
|
|
* otherwise it is expressed in a local world-oriented frame. |
58 |
|
|
*/ |
59 |
|
|
const Vector& getDesiredAcceleration() const; |
60 |
|
|
|
61 |
|
|
/** Return the task acceleration (after applying the specified mask). |
62 |
|
|
* The value is expressed in local frame is the local_frame flag is true, |
63 |
|
|
* otherwise it is expressed in a local world-oriented frame. |
64 |
|
|
*/ |
65 |
|
|
Vector getAcceleration(ConstRefVector dv) const; |
66 |
|
|
|
67 |
|
|
virtual void setMask(math::ConstRefVector mask); |
68 |
|
|
|
69 |
|
|
/** Return the position tracking error (after applying the specified mask). |
70 |
|
|
* The error is expressed in local frame is the local_frame flag is true, |
71 |
|
|
* otherwise it is expressed in a local world-oriented frame. |
72 |
|
|
*/ |
73 |
|
|
const Vector& position_error() const; |
74 |
|
|
|
75 |
|
|
/** Return the velocity tracking error (after applying the specified mask). |
76 |
|
|
* The error is expressed in local frame is the local_frame flag is true, |
77 |
|
|
* otherwise it is expressed in a local world-oriented frame. |
78 |
|
|
*/ |
79 |
|
|
const Vector& velocity_error() const; |
80 |
|
|
|
81 |
|
|
const Vector& Kp() const; |
82 |
|
|
const Vector& Kd() const; |
83 |
|
|
void Kp(ConstRefVector Kp); |
84 |
|
|
void Kd(ConstRefVector Kp); |
85 |
|
|
|
86 |
|
|
Index frame_id1() const; |
87 |
|
|
Index frame_id2() const; |
88 |
|
|
|
89 |
|
|
/** |
90 |
|
|
* @brief Specifies if the jacobian and desired acceloration should be |
91 |
|
|
* expressed in the local frame or the local world-oriented frame. |
92 |
|
|
* |
93 |
|
|
* @param local_frame If true, represent jacobian and acceloration in the |
94 |
|
|
* local frame. If false, represent them in the local world-oriented frame. |
95 |
|
|
*/ |
96 |
|
|
void useLocalFrame(bool local_frame); |
97 |
|
|
|
98 |
|
|
protected: |
99 |
|
|
std::string m_frame_name1; |
100 |
|
|
std::string m_frame_name2; |
101 |
|
|
Index m_frame_id1; |
102 |
|
|
Index m_frame_id2; |
103 |
|
|
Motion m_p_error, m_v_error; |
104 |
|
|
Vector m_p_error_vec, m_v_error_vec; |
105 |
|
|
Vector m_p_error_masked_vec, m_v_error_masked_vec; |
106 |
|
|
Vector m_p, m_v; |
107 |
|
|
Vector m_p_ref, m_v_ref_vec; |
108 |
|
|
Motion m_v_ref, m_a_ref; |
109 |
|
|
SE3 m_wMl1, m_wMl2; |
110 |
|
|
Vector m_Kp; |
111 |
|
|
Vector m_Kd; |
112 |
|
|
Vector m_a_des, m_a_des_masked; |
113 |
|
|
Motion m_drift; |
114 |
|
|
Vector m_drift_masked; |
115 |
|
|
Matrix6x m_J1, m_J2; |
116 |
|
|
Matrix6x m_J1_rotated, m_J2_rotated; |
117 |
|
|
ConstraintEquality m_constraint; |
118 |
|
|
}; |
119 |
|
|
|
120 |
|
|
} // namespace tasks |
121 |
|
|
} // namespace tsid |
122 |
|
|
|
123 |
|
|
#endif // ifndef __invdyn_task_frames_equality_hpp__ |