RosCartesianTaskProxy.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
38 
41 
42 namespace orca_ros
43 {
44 namespace task
45 {
46 
47 
49 {
50 public:
51  RosCartesianTaskProxy( const std::string& robot_name,
52  const std::string& controller_name,
53  const std::string& task_name);
54  virtual ~RosCartesianTaskProxy();
55 
56 public:
57  std::shared_ptr<orca_ros::common::RosCartesianAccelerationPIDProxy> servoController();
58 
59 public:
60  void setDesired(const orca::math::Vector6d& cartesian_acceleration_des);
61  void setBaseFrame(const std::string& base_ref_frame);
62  void setControlFrame(const std::string& control_frame);
63  std::string getBaseFrame();
64  std::string getControlFrame();
65 
66 public: // not part of the base ORCA API (just some helper functions)
67  void setDesired( const Eigen::Matrix4d& des_pose,
68  const orca::math::Vector6d& des_vel,
69  const orca::math::Vector6d& des_acc);
70  void setDesiredPose(const Eigen::Matrix4d& des_pose);
71  void setDesiredVelocity(const orca::math::Vector6d& des_vel);
72  void setDesiredAcceleration(const orca::math::Vector6d& des_acc);
73 
74  const Eigen::Matrix4d& getDesiredPose();
75  const orca::math::Vector6d& getDesiredVelocity();
76  const orca::math::Vector6d& getDesiredAcceleration();
77 
78  const Eigen::Matrix4d& getCurrentPose();
79  const orca::math::Vector6d& getCurrentVelocity();
80 
81 private:
82  void currentStateSubscriberCb(const orca_ros::CartesianTaskState::ConstPtr& msg);
83  void waitForFirstState();
84 
85 private:
86  Eigen::Matrix4d current_pose_;
87  orca::math::Vector6d current_velocity_;
88 
89  Eigen::Matrix4d desired_pose_;
90  orca::math::Vector6d desired_velocity_;
91  orca::math::Vector6d desired_acceleration_;
92 
93  bool first_cart_task_state_received_ = false;
94 
95 
96 private:
97  std::shared_ptr<orca_ros::common::RosCartesianAccelerationPIDProxy> cart_servo_proxy_;
98 
99  ros::Subscriber current_state_sub_;
100  ros::Publisher desired_state_pub_;
101 
102  ros::ServiceClient sc_setDesired_;
103  ros::ServiceClient sc_setBaseFrame_;
104  ros::ServiceClient sc_setControlFrame_;
105  ros::ServiceClient sc_getBaseFrame_;
106  ros::ServiceClient sc_getControlFrame_;
107 };
108 
109 } // namespace task
110 } // namesapce orca
void setControlFrame(const std::string &control_frame)
Definition: RosCartesianTaskProxy.cc:75
void setDesiredPose(const Eigen::Matrix4d &des_pose)
Definition: RosCartesianTaskProxy.cc:113
const Eigen::Matrix4d & getDesiredPose()
Definition: RosCartesianTaskProxy.cc:125
void setDesiredAcceleration(const orca::math::Vector6d &des_acc)
Definition: RosCartesianTaskProxy.cc:121
RosCartesianTaskProxy(const std::string &robot_name, const std::string &controller_name, const std::string &task_name)
Definition: RosCartesianTaskProxy.cc:6
void setDesiredVelocity(const orca::math::Vector6d &des_vel)
Definition: RosCartesianTaskProxy.cc:117
void setDesired(const orca::math::Vector6d &cartesian_acceleration_des)
Definition: RosCartesianTaskProxy.cc:55
const orca::math::Vector6d & getDesiredVelocity()
Definition: RosCartesianTaskProxy.cc:129
const Eigen::Matrix4d & getCurrentPose()
Definition: RosCartesianTaskProxy.cc:137
Definition: RosCartesianAccelerationPID.h:42
std::string getControlFrame()
Definition: RosCartesianTaskProxy.cc:95
Definition: RosGenericTaskProxy.h:46
const orca::math::Vector6d & getDesiredAcceleration()
Definition: RosCartesianTaskProxy.cc:133
std::shared_ptr< orca_ros::common::RosCartesianAccelerationPIDProxy > servoController()
Definition: RosCartesianTaskProxy.cc:50
Definition: RosCartesianTaskProxy.h:48
void setBaseFrame(const std::string &base_ref_frame)
Definition: RosCartesianTaskProxy.cc:65
const orca::math::Vector6d & getCurrentVelocity()
Definition: RosCartesianTaskProxy.cc:141
virtual ~RosCartesianTaskProxy()
Definition: RosCartesianTaskProxy.cc:35
std::string getBaseFrame()
Definition: RosCartesianTaskProxy.cc:85