ORCA: Optimization-based framework for Robotic Control Applications
CartesianAccelerationPID.h
Go to the documentation of this file.
1 //| This file is a part of the ORCA framework.
2 //|
3 //| Copyright 2018, Fuzzy Logic Robotics
4 //| Copyright 2017, ISIR / Universite Pierre et Marie Curie (UPMC)
5 //|
6 //| Main contributor(s): Antoine Hoarau, Ryan Lober, and
7 //| Fuzzy Logic Robotics <info@fuzzylogicrobotics.com>
8 //|
9 //| ORCA is a whole-body reactive controller framework for robotics.
10 //|
11 //| This software is governed by the CeCILL-C license under French law and
12 //| abiding by the rules of distribution of free software. You can use,
13 //| modify and/ or redistribute the software under the terms of the CeCILL-C
14 //| license as circulated by CEA, CNRS and INRIA at the following URL
15 //| "http://www.cecill.info".
16 //|
17 //| As a counterpart to the access to the source code and rights to copy,
18 //| modify and redistribute granted by the license, users are provided only
19 //| with a limited warranty and the software's author, the holder of the
20 //| economic rights, and the successive licensors have only limited
21 //| liability.
22 //|
23 //| In this respect, the user's attention is drawn to the risks associated
24 //| with loading, using, modifying and/or developing or reproducing the
25 //| software by the user in light of its specific status of free software,
26 //| that may mean that it is complicated to manipulate, and that also
27 //| therefore means that it is reserved for developers and experienced
28 //| professionals having in-depth computer knowledge. Users are therefore
29 //| encouraged to load and test the software's suitability as regards their
30 //| requirements in conditions enabling the security of their systems and/or
31 //| data to be ensured and, more generally, to use and operate it in the
32 //| same conditions as regards security.
33 //|
34 //| The fact that you are presently reading this means that you have had
35 //| knowledge of the CeCILL-C license and that you accept its terms.
36 
37 
38 #pragma once
39 #include "orca/math/Utils.h"
42 #include "orca/common/TaskBase.h"
43 
44 namespace orca
45 {
46  namespace common
47  {
48  using math::Vector6d;
49 
51  {
52  public:
65  CartesianAccelerationPID(const std::string& name);
66 
75  void setDesired(const Eigen::Matrix4d& cartesian_position_traj
76  , const Vector6d& cartesian_velocity_traj
77  , const Vector6d& cartesian_acceleration_traj);
83  const Vector6d& getCommand() const;
89  const Eigen::Matrix4d& getCurrentCartesianPose() const;
95  const Eigen::Vector3d& getCurrentCartesianPosition() const;
101  const Eigen::Matrix3d& getCurrentCartesianRotation() const;
107  const Vector6d& getCurrentCartesianVelocity() const;
113  const Eigen::Matrix4d& getDesiredCartesianPose() const;
119  const Vector6d& getDesiredCartesianVelocity() const;
131  const Vector6d& getCartesianPoseError() const;
137  const Vector6d& getCartesianVelocityError() const;
142  void print() const;
149  protected:
150  void onResize();
151  void onActivation();
152  void onCompute(double current_time, double dt);
153  private:
154  Parameter< PIDController::Ptr > pid_ = std::make_shared<PIDController>("pid");
155  private:
156  Eigen::Matrix4d cart_pos_curr_,cart_pos_des_;
157  Eigen::Matrix3d cart_rot_curr_;
158  Eigen::Vector3d cart_position_curr_;
159  Vector6d cart_acc_cmd_
160  ,cart_acc_bias_
161  ,cart_acc_des_
162  ,cart_vel_des_
163  ,cart_pos_err_
164  ,cart_vel_err_
165  ,cart_vel_curr_;
166  bool desired_set_ = false;
167  };
168  } // namespace common
169 } // namespace orca
PIDController::Ptr pid()
The dimension 6 pid controller.
Definition: CartesianAccelerationPID.cc:88
const Vector6d & getCartesianPoseError() const
Returns the computed pose error as a 6D Vector [dx,dy,dz,drx,dry,drz].
Definition: CartesianAccelerationPID.cc:108
const Vector6d & getCartesianVelocityError() const
Returns the computed cartesian velocity.
Definition: CartesianAccelerationPID.cc:113
Definition: CartesianServoController.h:45
const Vector6d & getDesiredCartesianAcceleration() const
Return the desired cartesian acceleration.
Definition: CartesianAccelerationPID.cc:40
CartesianAccelerationPID(const std::string &name)
The cartesian acceleration PID helps any cartesian task servo around a desired cartesian pose...
Definition: CartesianAccelerationPID.cc:6
void print() const
Outputs info on std::cout.
Definition: CartesianAccelerationPID.cc:60
Definition: CartesianAccelerationPID.h:50
void onResize()
Definition: CartesianAccelerationPID.cc:45
void onActivation()
Definition: CartesianAccelerationPID.cc:93
const Vector6d & getDesiredCartesianVelocity() const
Returns the desired cartesian acceleration (mixed representation)
Definition: CartesianAccelerationPID.cc:36
const Eigen::Matrix4d & getDesiredCartesianPose() const
Returns desired pose (set with setDesired)
Definition: CartesianAccelerationPID.cc:32
void setDesired(const Eigen::Matrix4d &cartesian_position_traj, const Vector6d &cartesian_velocity_traj, const Vector6d &cartesian_acceleration_traj)
Set the control frame desired cartesian pose w.r.t the base frame. This is usually something to get f...
Definition: CartesianAccelerationPID.cc:78
const Eigen::Matrix3d & getCurrentCartesianRotation() const
Returns the current rotation of the control frame (w.r.t the base frame)
Definition: CartesianAccelerationPID.cc:17
const Vector6d & getCurrentCartesianVelocity() const
Returns the current cartesian velocity in mixed representation.
Definition: CartesianAccelerationPID.cc:27
Eigen::Matrix< double, 6, 1 > Vector6d
Definition: Utils.h:62
const Eigen::Vector3d & getCurrentCartesianPosition() const
Returns the current position of the control frame (w.r.t the base frame)
Definition: CartesianAccelerationPID.cc:12
const Vector6d & getCommand() const
Returns the computed command.
Definition: CartesianAccelerationPID.cc:134
Definition: CartesianAccelerationPID.h:44
void onCompute(double current_time, double dt)
Definition: CartesianAccelerationPID.cc:118
This class holds the conversion from a string (YAML string) to the data type.
Definition: Parameter.h:113
const Eigen::Matrix4d & getCurrentCartesianPose() const
Returns the current pose of the control frame (w.r.t the base frame)
Definition: CartesianAccelerationPID.cc:22
std::shared_ptr< PIDController > Ptr
Definition: PIDController.h:51