52 const std::string& controller_name,
53 const std::string& task_name);
57 std::shared_ptr<orca_ros::common::RosCartesianAccelerationPIDProxy>
servoController();
60 void setDesired(
const orca::math::Vector6d& cartesian_acceleration_des);
67 void setDesired(
const Eigen::Matrix4d& des_pose,
68 const orca::math::Vector6d& des_vel,
69 const orca::math::Vector6d& des_acc);
82 void currentStateSubscriberCb(
const orca_ros::CartesianTaskState::ConstPtr& msg);
83 void waitForFirstState();
86 Eigen::Matrix4d current_pose_;
87 orca::math::Vector6d current_velocity_;
89 Eigen::Matrix4d desired_pose_;
90 orca::math::Vector6d desired_velocity_;
91 orca::math::Vector6d desired_acceleration_;
93 bool first_cart_task_state_received_ =
false;
97 std::shared_ptr<orca_ros::common::RosCartesianAccelerationPIDProxy> cart_servo_proxy_;
99 ros::Subscriber current_state_sub_;
100 ros::Publisher desired_state_pub_;
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_;
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