mc_rtc  2.14.0
TransformTask.h
Go to the documentation of this file.
1 /*
2  * Copyright 2015-2022 CNRS-UM LIRMM, CNRS-AIST JRL
3  */
4 
5 #pragma once
6 
8 
9 #include <mc_rbdyn/RobotFrame.h>
10 
11 namespace mc_tasks
12 {
13 
16 {
17 public:
27  double stiffness = 2.0,
28  double weight = 500.0,
29  bool showTarget = true,
30  bool showPose = true);
31 
47  TransformTask(const std::string & surfaceName,
48  const mc_rbdyn::Robots & robots,
49  unsigned int robotIndex,
50  double stiffness = 2.0,
51  double weight = 500,
52  bool showTarget = true,
53  bool showPose = true);
54 
62  void reset() override;
63 
65  virtual sva::PTransformd target() const;
66 
72  virtual void target(const sva::PTransformd & worldPos);
73 
78  virtual void targetVel(const sva::MotionVecd & worldVel);
79 
91  void targetSurface(unsigned int robotIndex,
92  const std::string & surfaceName,
93  const sva::PTransformd & offset = sva::PTransformd::Identity());
94 
104  void targetFrame(const mc_rbdyn::Frame & targetFrame, const sva::PTransformd & offset = sva::PTransformd::Identity());
105 
115  void targetFrameVelocity(const mc_rbdyn::Frame & targetFrame,
116  const sva::PTransformd & offset = sva::PTransformd::Identity());
117 
127  virtual void target(const mc_rbdyn::Frame & frame, const sva::PTransformd & offset);
128 
130  inline const std::string & surface() const noexcept { return frame_->name(); }
131 
133  const mc_rbdyn::RobotFrame & frame() const noexcept { return *frame_; }
134 
136  inline sva::PTransformd surfacePose() const noexcept { return frame_->position(); }
137 
146  std::function<bool(const mc_tasks::MetaTask & task, std::string &)> buildCompletionCriteria(
147  double dt,
148  const mc_rtc::Configuration & config) const override;
149 
150  void addToLogger(mc_rtc::Logger & logger) override;
151 
155 
163  void setGains(const sva::MotionVecd & stiffness, const sva::MotionVecd & damping)
164  {
165  return TrajectoryTaskGeneric::setGains(stiffness.vector(), damping.vector());
166  }
167 
175  void stiffness(const sva::MotionVecd & stiffness) { return TrajectoryTaskGeneric::stiffness(stiffness.vector()); }
176 
178  sva::MotionVecd mvStiffness() { return sva::MotionVecd(dimStiffness()); }
179 
185  void damping(const sva::MotionVecd & damping) { return TrajectoryTaskGeneric::damping(damping.vector()); }
186 
188  sva::MotionVecd mvDamping() { return sva::MotionVecd(dimDamping()); }
189 
195  void refVelB(const sva::MotionVecd & velB) { TrajectoryTaskGeneric::refVel(velB.vector()); }
196 
198  sva::MotionVecd refVelB() const { return sva::MotionVecd(TrajectoryTaskGeneric::refVel()); }
199 
205  void refAccel(const sva::MotionVecd & accel) { return TrajectoryTaskGeneric::refAccel(accel.vector()); }
206 
208  void load(mc_solver::QPSolver & solver, const mc_rtc::Configuration & config) override;
209 
210 protected:
212 
213  void addToGUI(mc_rtc::gui::StateBuilder & gui) override;
214 
215  /* Don't use parent's refVel() as the velocity frame (spatial or body) is
216  * ambiguous. */
218 
219  bool showTarget_ = true;
220  bool showPose_ = true;
221 };
222 
223 } // namespace mc_tasks
#define MC_TASKS_DLLAPI
Definition: api.h:50
std::shared_ptr< const RobotFrame > ConstRobotFramePtr
Definition: fwd.h:25
Definition: StabilizerStandingState.h:12
Definition: Frame.h:28
Definition: RobotFrame.h:22
Definition: Robots.h:16
Simplify access to values hold within a JSON file.
Definition: Configuration.h:166
Logs controller data to disk.
Definition: Logger.h:30
Definition: StateBuilder.h:28
Definition: QPSolver.h:86
Represents a generic task.
Definition: MetaTask.h:40
Generic wrapper for a trajectory dynamic over an error function.
Definition: TrajectoryTaskGeneric.h:27
double damping() const
Get the current task damping.
double stiffness() const
Get the current task stiffness.
const Eigen::VectorXd & refAccel() const
Get the trajectory reference acceleration.
const Eigen::VectorXd & refVel() const
Get the trajectory reference velocity.
void setGains(double stiffness, double damping)
Set both stiffness and damping.
Control a frame 6D pose.
Definition: TransformTask.h:16
std::function< bool(const mc_tasks::MetaTask &task, std::string &)> buildCompletionCriteria(double dt, const mc_rtc::Configuration &config) const override
mc_rbdyn::ConstRobotFramePtr frame_
Definition: TransformTask.h:211
void refAccel(const sva::MotionVecd &accel)
Set trajectory task's reference acceleration from motion vector.
Definition: TransformTask.h:205
sva::MotionVecd refVelB() const
Get reference velocity in frame coordinates as a motion vector.
Definition: TransformTask.h:198
TransformTask(const mc_rbdyn::RobotFrame &frame, double stiffness=2.0, double weight=500.0, bool showTarget=true, bool showPose=true)
Constructor.
virtual void target(const sva::PTransformd &worldPos)
Set the task's target.
void load(mc_solver::QPSolver &solver, const mc_rtc::Configuration &config) override
Load parameters from a Configuration object.
TransformTask(const std::string &surfaceName, const mc_rbdyn::Robots &robots, unsigned int robotIndex, double stiffness=2.0, double weight=500, bool showTarget=true, bool showPose=true)
Constructor.
void setGains(const sva::MotionVecd &stiffness, const sva::MotionVecd &damping)
Set dimensional stiffness and damping.
Definition: TransformTask.h:163
sva::PTransformd surfacePose() const noexcept
Definition: TransformTask.h:136
void targetFrame(const mc_rbdyn::Frame &targetFrame, const sva::PTransformd &offset=sva::PTransformd::Identity())
Targets a given frame with an optional offset.
void reset() override
Reset the task.
void targetFrameVelocity(const mc_rbdyn::Frame &targetFrame, const sva::PTransformd &offset=sva::PTransformd::Identity())
Targets a given frame velocity with an optional offset.
virtual sva::PTransformd target() const
Get the task's target.
const std::string & surface() const noexcept
Retrieve the controlled frame name.
Definition: TransformTask.h:130
void targetSurface(unsigned int robotIndex, const std::string &surfaceName, const sva::PTransformd &offset=sva::PTransformd::Identity())
Targets a robot surface with an optional offset. The offset is expressed in the target contact frame.
void addToGUI(mc_rtc::gui::StateBuilder &gui) override
void addToLogger(mc_rtc::Logger &logger) override
virtual void targetVel(const sva::MotionVecd &worldVel)
Get the task's target velocity.
void damping(const sva::MotionVecd &damping)
Set dimensional damping.
Definition: TransformTask.h:185
void stiffness(const sva::MotionVecd &stiffness)
Set dimensional stiffness.
Definition: TransformTask.h:175
const mc_rbdyn::RobotFrame & frame() const noexcept
Return the controlled frame (const)
Definition: TransformTask.h:133
void refVelB(const sva::MotionVecd &velB)
Set trajectory task's reference velocity from motion vector in frame coordinates.
Definition: TransformTask.h:195
virtual void target(const mc_rbdyn::Frame &frame, const sva::PTransformd &offset)
Targets a given frame with an optional offset.
sva::MotionVecd mvDamping()
Get dimensional damping as a motion vector.
Definition: TransformTask.h:188
sva::MotionVecd mvStiffness()
Get dimensional stiffness as a motion vector.
Definition: TransformTask.h:178