Loading...
Searching...
No Matches
task-se3-equality.hpp
Go to the documentation of this file.
1//
2// Copyright (c) 2017 CNRS, NYU, MPI Tübingen
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_se3_equality_hpp__
19#define __invdyn_task_se3_equality_hpp__
20
24
25#include <pinocchio/multibody/model.hpp>
26#include <pinocchio/multibody/data.hpp>
27
28namespace tsid
29{
30 namespace tasks
31 {
32
34 {
35 public:
36 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
37
42 typedef pinocchio::Data Data;
43 typedef pinocchio::Data::Matrix6x Matrix6x;
44 typedef pinocchio::Motion Motion;
45 typedef pinocchio::SE3 SE3;
46
47 TaskSE3Equality(const std::string & name,
48 RobotWrapper & robot,
49 const std::string & frameName);
50
51 int dim() const;
52
53 const ConstraintBase & compute(const double t,
56 Data & data);
57
58 const ConstraintBase & getConstraint() const;
59
61 void setReference(const SE3 & ref);
62 const TrajectorySample & getReference() const;
63
68 const Vector & getDesiredAcceleration() const;
69
75
76 virtual void setMask(math::ConstRefVector mask);
77
82 const Vector & position_error() const;
83
88 const Vector & velocity_error() const;
89
90 const Vector & position() const;
91 const Vector & velocity() const;
92 const Vector & position_ref() const;
93 const Vector & velocity_ref() const;
94
95 const Vector & Kp() const;
96 const Vector & Kd() const;
97 void Kp(ConstRefVector Kp);
98 void Kd(ConstRefVector Kp);
99
100 Index frame_id() const;
101
109 void useLocalFrame(bool local_frame);
110
111 protected:
112
113 std::string m_frame_name;
132 };
133
134 }
135}
136
137#endif // ifndef __invdyn_task_se3_equality_hpp__
Abstract class representing a linear equality/inequality constraint. Equality constraints are represe...
Definition: constraint-base.hpp:37
Definition: constraint-equality.hpp:29
Wrapper for a robot based on pinocchio.
Definition: robot-wrapper.hpp:41
const std::string & name() const
Definition: task-base.cpp:30
Definition: task-motion.hpp:29
Definition: task-se3-equality.hpp:34
const Vector & Kp() const
Definition: task-se3-equality.cpp:81
pinocchio::SE3 SE3
Definition: task-se3-equality.hpp:45
pinocchio::Motion Motion
Definition: task-se3-equality.hpp:44
const Vector & position() const
Definition: task-se3-equality.cpp:136
Vector getAcceleration(ConstRefVector dv) const
Definition: task-se3-equality.cpp:161
Vector m_p_ref
Definition: task-se3-equality.hpp:119
Vector m_drift_masked
Definition: task-se3-equality.hpp:126
trajectories::TrajectorySample TrajectorySample
Definition: task-se3-equality.hpp:39
Matrix6x m_J
Definition: task-se3-equality.hpp:127
std::string m_frame_name
Definition: task-se3-equality.hpp:113
Motion m_a_ref
Definition: task-se3-equality.hpp:120
Motion m_v_error
Definition: task-se3-equality.hpp:115
int dim() const
Return the dimension of the task. \info should be overloaded in the child class.
Definition: task-se3-equality.cpp:76
Vector m_p_error_masked_vec
Definition: task-se3-equality.hpp:117
Vector m_v_error_vec
Definition: task-se3-equality.hpp:116
Vector m_v_error_masked_vec
Definition: task-se3-equality.hpp:117
math::Vector Vector
Definition: task-se3-equality.hpp:40
SE3 m_M_ref
Definition: task-se3-equality.hpp:121
Index m_frame_id
Definition: task-se3-equality.hpp:114
pinocchio::Data::Matrix6x Matrix6x
Definition: task-se3-equality.hpp:43
Vector m_v
Definition: task-se3-equality.hpp:118
Motion m_v_ref
Definition: task-se3-equality.hpp:120
Vector m_p_error_vec
Definition: task-se3-equality.hpp:116
Vector m_a_des
Definition: task-se3-equality.hpp:124
void useLocalFrame(bool local_frame)
Specifies if the jacobian and desired acceloration should be expressed in the local frame or the loca...
Definition: task-se3-equality.cpp:176
void setReference(TrajectorySample &ref)
Definition: task-se3-equality.cpp:97
const ConstraintBase & getConstraint() const
Definition: task-se3-equality.cpp:171
const Vector & getDesiredAcceleration() const
Definition: task-se3-equality.cpp:156
const ConstraintBase & compute(const double t, ConstRefVector q, ConstRefVector v, Data &data)
Definition: task-se3-equality.cpp:181
math::ConstraintEquality ConstraintEquality
Definition: task-se3-equality.hpp:41
const Vector & Kd() const
Definition: task-se3-equality.cpp:83
ConstraintEquality m_constraint
Definition: task-se3-equality.hpp:129
Vector m_p
Definition: task-se3-equality.hpp:118
Motion m_drift
Definition: task-se3-equality.hpp:125
virtual void setMask(math::ConstRefVector mask)
Definition: task-se3-equality.cpp:65
Motion m_p_error
Definition: task-se3-equality.hpp:115
const Vector & velocity() const
Definition: task-se3-equality.cpp:141
Vector m_Kd
Definition: task-se3-equality.hpp:123
Index frame_id() const
Definition: task-se3-equality.cpp:166
SE3 m_wMl
Definition: task-se3-equality.hpp:121
const Vector & velocity_error() const
Definition: task-se3-equality.cpp:131
const Vector & position_ref() const
Definition: task-se3-equality.cpp:146
Vector m_Kp
Definition: task-se3-equality.hpp:122
const TrajectorySample & getReference() const
Definition: task-se3-equality.cpp:121
pinocchio::Data Data
Definition: task-se3-equality.hpp:42
Vector m_v_ref_vec
Definition: task-se3-equality.hpp:119
Matrix6x m_J_rotated
Definition: task-se3-equality.hpp:128
bool m_local_frame
Definition: task-se3-equality.hpp:131
TrajectorySample m_ref
Definition: task-se3-equality.hpp:130
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef math::Index Index
Definition: task-se3-equality.hpp:38
const Vector & velocity_ref() const
Definition: task-se3-equality.cpp:151
const Vector & position_error() const
Definition: task-se3-equality.cpp:126
Vector m_a_des_masked
Definition: task-se3-equality.hpp:124
Definition: trajectory-base.hpp:36
std::size_t Index
Definition: fwd.hpp:55
Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > Vector
Definition: fwd.hpp:37
const Eigen::Ref< const Vector > ConstRefVector
Definition: fwd.hpp:50
Definition: constraint-bound.hpp:27