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 
28 namespace tsid
29 {
30  namespace tasks
31  {
32 
33  class TaskSE3Equality : public TaskMotion
34  {
35  public:
36  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
37 
38  typedef math::Index Index;
43  typedef pinocchio::Data::Matrix6x Matrix6x;
44  typedef pinocchio::Motion Motion;
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 
60  void setReference(TrajectorySample & ref);
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__
tsid::tasks::TaskSE3Equality::position_ref
const Vector & position_ref() const
Definition: task-se3-equality.cpp:146
tsid::tasks::TaskSE3Equality::m_M_ref
SE3 m_M_ref
Definition: task-se3-equality.hpp:121
tsid::tasks::TaskSE3Equality::m_p_ref
Vector m_p_ref
Definition: task-se3-equality.hpp:119
tsid::tasks::TaskSE3Equality::Matrix6x
pinocchio::Data::Matrix6x Matrix6x
Definition: task-se3-equality.hpp:43
tsid::tasks::TaskSE3Equality::velocity_ref
const Vector & velocity_ref() const
Definition: task-se3-equality.cpp:151
tsid::tasks::TaskSE3Equality::velocity_error
const Vector & velocity_error() const
Definition: task-se3-equality.cpp:131
tsid::tasks::TaskSE3Equality::m_v_error_masked_vec
Vector m_v_error_masked_vec
Definition: task-se3-equality.hpp:117
tsid::tasks::TaskSE3Equality::ConstraintEquality
math::ConstraintEquality ConstraintEquality
Definition: task-se3-equality.hpp:41
tsid::trajectories::TrajectorySample
Definition: trajectory-base.hpp:35
tsid::math::ConstraintEquality
Definition: constraint-equality.hpp:28
tsid::tasks::TaskSE3Equality::m_a_des_masked
Vector m_a_des_masked
Definition: task-se3-equality.hpp:124
Data
pinocchio::Data Data
Definition: inverse-dynamics-formulation-acc-force.cpp:29
task-motion.hpp
tsid::math::ConstraintBase
Abstract class representing a linear equality/inequality constraint. Equality constraints are represe...
Definition: constraint-base.hpp:36
tsid::tasks::TaskSE3Equality::Index
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef math::Index Index
Definition: task-se3-equality.hpp:38
tsid::math::Vector
Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > Vector
Definition: fwd.hpp:37
tsid::tasks::TaskSE3Equality::m_p
Vector m_p
Definition: task-se3-equality.hpp:118
tsid::tasks::TaskSE3Equality::SE3
pinocchio::SE3 SE3
Definition: task-se3-equality.hpp:45
tsid::tasks::TaskSE3Equality::setMask
virtual void setMask(math::ConstRefVector mask)
Definition: task-se3-equality.cpp:65
tsid::tasks::TaskSE3Equality::m_wMl
SE3 m_wMl
Definition: task-se3-equality.hpp:121
tsid::tasks::TaskSE3Equality::getReference
const TrajectorySample & getReference() const
Definition: task-se3-equality.cpp:121
tsid::tasks::TaskSE3Equality::Data
pinocchio::Data Data
Definition: task-se3-equality.hpp:42
constraint-equality.hpp
tsid::tasks::TaskSE3Equality::getDesiredAcceleration
const Vector & getDesiredAcceleration() const
Definition: task-se3-equality.cpp:156
tsid::tasks::TaskSE3Equality::compute
const ConstraintBase & compute(const double t, ConstRefVector q, ConstRefVector v, Data &data)
Definition: task-se3-equality.cpp:181
tsid::tasks::TaskSE3Equality::getAcceleration
Vector getAcceleration(ConstRefVector dv) const
Definition: task-se3-equality.cpp:161
tsid::tasks::TaskSE3Equality::m_ref
TrajectorySample m_ref
Definition: task-se3-equality.hpp:130
tsid::python::SE3
pinocchio::SE3 SE3
Definition: trajectory-base.hpp:34
tsid::tasks::TaskSE3Equality::m_p_error
Motion m_p_error
Definition: task-se3-equality.hpp:115
tsid::tasks::TaskSE3Equality::m_a_des
Vector m_a_des
Definition: task-se3-equality.hpp:124
tsid::tasks::TaskSE3Equality::m_a_ref
Motion m_a_ref
Definition: task-se3-equality.hpp:120
trajectory-base.hpp
tsid::tasks::TaskSE3Equality::velocity
const Vector & velocity() const
Definition: task-se3-equality.cpp:141
tsid::tasks::TaskSE3Equality::m_frame_id
Index m_frame_id
Definition: task-se3-equality.hpp:114
tsid::tasks::TaskSE3Equality::m_drift_masked
Vector m_drift_masked
Definition: task-se3-equality.hpp:126
tsid::tasks::TaskSE3Equality::Motion
pinocchio::Motion Motion
Definition: task-se3-equality.hpp:44
tsid::tasks::TaskSE3Equality::m_Kd
Vector m_Kd
Definition: task-se3-equality.hpp:123
tsid::math::Index
std::size_t Index
Definition: fwd.hpp:55
tsid::tasks::TaskSE3Equality::TaskSE3Equality
TaskSE3Equality(const std::string &name, RobotWrapper &robot, const std::string &frameName)
Definition: task-se3-equality.cpp:31
tsid::tasks::TaskSE3Equality::m_local_frame
bool m_local_frame
Definition: task-se3-equality.hpp:131
tsid::tasks::TaskSE3Equality::getConstraint
const ConstraintBase & getConstraint() const
Definition: task-se3-equality.cpp:171
tsid::tasks::TaskSE3Equality::TrajectorySample
trajectories::TrajectorySample TrajectorySample
Definition: task-se3-equality.hpp:39
tsid::tasks::TaskSE3Equality::m_drift
Motion m_drift
Definition: task-se3-equality.hpp:125
tsid::tasks::TaskSE3Equality::m_v_ref
Motion m_v_ref
Definition: task-se3-equality.hpp:120
tsid::tasks::TaskSE3Equality::m_J
Matrix6x m_J
Definition: task-se3-equality.hpp:127
tsid::tasks::TaskSE3Equality::position_error
const Vector & position_error() const
Definition: task-se3-equality.cpp:126
tsid::tasks::TaskSE3Equality::m_p_error_vec
Vector m_p_error_vec
Definition: task-se3-equality.hpp:116
tsid::tasks::TaskSE3Equality::m_Kp
Vector m_Kp
Definition: task-se3-equality.hpp:122
tsid::tasks::TaskSE3Equality::Kp
const Vector & Kp() const
Definition: task-se3-equality.cpp:81
tsid::tasks::TaskMotion
Definition: task-motion.hpp:28
tsid::math::ConstRefVector
const typedef Eigen::Ref< const Vector > ConstRefVector
Definition: fwd.hpp:50
tsid::tasks::TaskSE3Equality::dim
int dim() const
Return the dimension of the task. \info should be overloaded in the child class.
Definition: task-se3-equality.cpp:76
tsid::tasks::TaskSE3Equality::m_constraint
ConstraintEquality m_constraint
Definition: task-se3-equality.hpp:129
tsid::tasks::TaskBase::Data
pinocchio::Data Data
Definition: task-base.hpp:43
tsid::tasks::TaskSE3Equality::m_v_error
Motion m_v_error
Definition: task-se3-equality.hpp:115
tsid::tasks::TaskSE3Equality::m_J_rotated
Matrix6x m_J_rotated
Definition: task-se3-equality.hpp:128
tsid::tasks::TaskSE3Equality::frame_id
Index frame_id() const
Definition: task-se3-equality.cpp:166
tsid::tasks::TaskSE3Equality::m_p_error_masked_vec
Vector m_p_error_masked_vec
Definition: task-se3-equality.hpp:117
tsid
Definition: constraint-bound.hpp:26
tsid::tasks::TaskSE3Equality::setReference
void setReference(TrajectorySample &ref)
Definition: task-se3-equality.cpp:97
tsid::robots::RobotWrapper
Wrapper for a robot based on pinocchio.
Definition: robot-wrapper.hpp:40
tsid::tasks::TaskSE3Equality::m_v_ref_vec
Vector m_v_ref_vec
Definition: task-se3-equality.hpp:119
tsid::tasks::TaskSE3Equality::Kd
const Vector & Kd() const
Definition: task-se3-equality.cpp:83
tsid::tasks::TaskSE3Equality::Vector
math::Vector Vector
Definition: task-se3-equality.hpp:40
tsid::tasks::TaskBase::name
const std::string & name() const
Definition: task-base.cpp:30
tsid::tasks::TaskSE3Equality::m_v
Vector m_v
Definition: task-se3-equality.hpp:118
tsid::tasks::TaskSE3Equality::position
const Vector & position() const
Definition: task-se3-equality.cpp:136
tsid::tasks::TaskSE3Equality
Definition: task-se3-equality.hpp:33
tsid::tasks::TaskSE3Equality::useLocalFrame
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
tsid::tasks::TaskBase::ConstRefVector
math::ConstRefVector ConstRefVector
Definition: task-base.hpp:42
tsid::tasks::TaskSE3Equality::m_v_error_vec
Vector m_v_error_vec
Definition: task-se3-equality.hpp:116
tsid::tasks::TaskSE3Equality::m_frame_name
std::string m_frame_name
Definition: task-se3-equality.hpp:113