RosCartesianAccelerationPIDProxy.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 #pragma once
40 
41 namespace orca_ros
42 {
43  namespace common
44  {
45 
47  {
48  public:
49  RosCartesianAccelerationPIDProxy( const std::string& robot_name,
50  const std::string& controller_name,
51  const std::string& task_name);
53 
54  public:
55  std::shared_ptr<RosPIDControllerProxy> pid();
56 
57  public:
58  void setDesired( const Eigen::Matrix4d& cartesian_position_traj,
59  const orca::math::Vector6d& cartesian_velocity_traj,
60  const orca::math::Vector6d& cartesian_acceleration_traj);
61  orca::math::Vector6d getCommand();
62  Eigen::Matrix4d getCurrentCartesianPose();
63  orca::math::Vector6d getCurrentCartesianVelocity();
64  Eigen::Matrix4d getDesiredCartesianPose();
65  orca::math::Vector6d getDesiredCartesianVelocity();
66  orca::math::Vector6d getDesiredCartesianAcceleration();
67  void print();
68 
69  private:
70  std::shared_ptr<RosPIDControllerProxy> internal_pid_proxy_;
71 
72  private:
73  ros::ServiceClient sc_setDesired_;
74  ros::ServiceClient sc_getCommand_;
75  ros::ServiceClient sc_getCurrentCartesianPose_;
76  ros::ServiceClient sc_getCurrentCartesianVelocity_;
77  ros::ServiceClient sc_getDesiredCartesianPose_;
78  ros::ServiceClient sc_getDesiredCartesianVelocity_;
79  ros::ServiceClient sc_getDesiredCartesianAcceleration_;
80  ros::ServiceClient sc_print_;
81  };
82  }
83 }
std::shared_ptr< RosPIDControllerProxy > pid()
Definition: RosCartesianAccelerationPIDProxy.cc:28
Eigen::Matrix4d getCurrentCartesianPose()
Definition: RosCartesianAccelerationPIDProxy.cc:59
Definition: RosWrapperBase.h:50
orca::math::Vector6d getDesiredCartesianAcceleration()
Definition: RosCartesianAccelerationPIDProxy.cc:108
orca::math::Vector6d getCurrentCartesianVelocity()
Definition: RosCartesianAccelerationPIDProxy.cc:71
void setDesired(const Eigen::Matrix4d &cartesian_position_traj, const orca::math::Vector6d &cartesian_velocity_traj, const orca::math::Vector6d &cartesian_acceleration_traj)
Definition: RosCartesianAccelerationPIDProxy.cc:33
orca::math::Vector6d getCommand()
Definition: RosCartesianAccelerationPIDProxy.cc:47
RosCartesianAccelerationPIDProxy(const std::string &robot_name, const std::string &controller_name, const std::string &task_name)
Definition: RosCartesianAccelerationPIDProxy.cc:5
Definition: RosCartesianAccelerationPID.h:42
void print()
Definition: RosCartesianAccelerationPIDProxy.cc:120
Eigen::Matrix4d getDesiredCartesianPose()
Definition: RosCartesianAccelerationPIDProxy.cc:83
~RosCartesianAccelerationPIDProxy()
Definition: RosCartesianAccelerationPIDProxy.cc:23
orca::math::Vector6d getDesiredCartesianVelocity()
Definition: RosCartesianAccelerationPIDProxy.cc:95
Definition: RosCartesianAccelerationPIDProxy.h:46