Franky 1.0.2
A High-Level Motion API for Franka
Loading...
Searching...
No Matches
waypoint_motion.hpp
Go to the documentation of this file.
1#pragma once
2
3#include <optional>
4#include <ruckig/ruckig.hpp>
5
7
8namespace franky {
9
13struct MotionPlannerException : std::runtime_error {
14 using std::runtime_error::runtime_error;
15};
16
32template <typename TargetType>
33struct Waypoint {
35
37
38 std::optional<franka::Duration> minimum_time{std::nullopt};
39
40 franka::Duration hold_target_duration{0};
41
42 std::optional<franka::Duration> max_total_duration{std::nullopt};
43};
44
52template <typename ControlSignalType, typename WaypointType, typename TargetType>
53class WaypointMotion : public Motion<ControlSignalType> {
54 static_assert(
55 std::is_base_of_v<Waypoint<TargetType>, WaypointType>, "WaypointType must inherit from Waypoint<TargetType>");
56
57 public:
63 explicit WaypointMotion(std::vector<WaypointType> waypoints, bool return_when_finished = true)
64 : waypoints_(std::move(waypoints)), return_when_finished_(return_when_finished), prev_result_() {}
65
66 protected:
67 void initImpl(const RobotState &robot_state, const std::optional<ControlSignalType> &previous_command) override {
68 target_reached_time_ = std::nullopt;
69 waypoint_started_time_ = franka::Duration(0);
70
71 waypoint_iterator_ = waypoints_.begin();
73 if (waypoint_iterator_ != waypoints_.end()) {
74 checkWaypoint(*waypoint_iterator_);
75 setNewWaypoint(robot_state, previous_command, *waypoint_iterator_, input_parameter_);
76 setInputLimits(*waypoint_iterator_, input_parameter_);
77 prev_result_ = ruckig::Result::Working;
78 } else {
79 prev_result_ = ruckig::Result::Finished;
80 }
81 }
82
84 const RobotState &robot_state, franka::Duration time_step, franka::Duration rel_time, franka::Duration abs_time,
85 const std::optional<ControlSignalType> &previous_command) override {
86 const auto expected_time_step = franka::Duration(1);
88 // In this case, we missed a couple of steps for some reason. Hence, extrapolate the way the robot does if it
89 // does not receive data (constant acceleration model).
90 // See https://frankaemika.github.io/docs/libfranka.html#under-the-hood
91 extrapolateMotion(robot_state, time_step - expected_time_step, input_parameter_, output_parameter_);
92 output_parameter_.pass_to_input(input_parameter_);
93 }
94
95 auto max_time_reached = false;
96 if (waypoint_iterator_ != waypoints_.end() && waypoint_iterator_->max_total_duration.has_value()) {
97 max_time_reached = rel_time - waypoint_started_time_ >= waypoint_iterator_->max_total_duration.value();
98 }
99
100 if (prev_result_ == ruckig::Finished || max_time_reached) {
101 if (!target_reached_time_.has_value()) {
102 target_reached_time_ = rel_time;
103 }
104 if (waypoint_iterator_ != waypoints_.end()) {
105 auto hold_target_duration = waypoint_iterator_->hold_target_duration;
106 if (waypoint_iterator_ + 1 == waypoints_.end()) {
107 // Allow cooldown of motion, so that the low-pass filter has time to adjust to target values
108 hold_target_duration = std::max(hold_target_duration, franka::Duration(10));
109 }
110 if (rel_time - target_reached_time_.value() >= hold_target_duration || max_time_reached) {
111 target_reached_time_ = std::nullopt;
112 ++waypoint_iterator_;
113 if (waypoint_iterator_ != waypoints_.end()) {
114 checkWaypoint(*waypoint_iterator_);
115 setNewWaypoint(robot_state, previous_command, *waypoint_iterator_, input_parameter_);
116 setInputLimits(*waypoint_iterator_, input_parameter_);
117 waypoint_started_time_ = rel_time;
118 }
119 }
120 }
121 if (waypoint_iterator_ == waypoints_.end()) {
123 if (!return_when_finished_) return command;
124 return franka::MotionFinished(command);
125 }
126 }
127
128 assert(waypoint_iterator_ != waypoints_.end());
129
130 prev_result_ = trajectory_generator_.update(input_parameter_, output_parameter_);
131 if (prev_result_ == ruckig::Result::Working || prev_result_ == ruckig::Result::Finished) {
132 // This is a workaround to prevent NaNs from popping up. Must be some bug in ruckig.
133 for (int i = 0; i < 7; i++) {
134 if (!input_parameter_.enabled[i]) {
135 output_parameter_.new_position[i] = 0.0;
136 output_parameter_.new_velocity[i] = 0.0;
137 output_parameter_.new_acceleration[i] = 0.0;
138 }
139 }
140 output_parameter_.pass_to_input(input_parameter_);
141 } else {
142 throw MotionPlannerException("Motion planner failed with error code " + std::to_string(prev_result_));
143 }
144
145 return getControlSignal(robot_state, time_step, previous_command, input_parameter_);
146 };
147
148 virtual void initWaypointMotion(
149 const RobotState &robot_state, const std::optional<ControlSignalType> &previous_command,
150 ruckig::InputParameter<7> &input_parameter) = 0;
151
152 virtual void setNewWaypoint(
153 const RobotState &robot_state, const std::optional<ControlSignalType> &previous_command,
154 const WaypointType &new_waypoint, ruckig::InputParameter<7> &input_parameter) = 0;
155
156 virtual void extrapolateMotion(
157 const RobotState &robot_state, const franka::Duration &time_step,
158 const ruckig::InputParameter<7> &input_parameter, ruckig::OutputParameter<7> &output_parameter) const = 0;
159
160 virtual void checkWaypoint(const WaypointType &waypoint) const {}
161
162 [[nodiscard]] virtual std::tuple<Vector7d, Vector7d, Vector7d> getAbsoluteInputLimits() const = 0;
163
165 const RobotState &robot_state, const franka::Duration &time_step,
166 const std::optional<ControlSignalType> &previous_command, const ruckig::InputParameter<7> &input_parameter) = 0;
167
168 virtual void setInputLimits(const WaypointType &waypoint, ruckig::InputParameter<7> &input_parameter) const = 0;
169
170 private:
171 std::vector<WaypointType> waypoints_;
172 bool return_when_finished_{true};
173
174 ruckig::Ruckig<7> trajectory_generator_{Robot::control_rate};
175 ruckig::Result prev_result_;
176 ruckig::InputParameter<7> input_parameter_;
177 ruckig::OutputParameter<7> output_parameter_;
178
179 typename std::vector<WaypointType>::iterator waypoint_iterator_;
180
181 std::optional<franka::Duration> target_reached_time_;
182 franka::Duration waypoint_started_time_;
183};
184
185} // namespace franky
Base class for motions.
Definition motion.hpp:23
Relative dynamics factors.
Definition relative_dynamics_factor.hpp:13
static constexpr double control_rate
Definition robot.hpp:139
A motion following multiple waypoints in a time-optimal way. Works with arbitrary initial conditions.
Definition waypoint_motion.hpp:53
virtual void extrapolateMotion(const RobotState &robot_state, const franka::Duration &time_step, const ruckig::InputParameter< 7 > &input_parameter, ruckig::OutputParameter< 7 > &output_parameter) const =0
virtual void initWaypointMotion(const RobotState &robot_state, const std::optional< ControlSignalType > &previous_command, ruckig::InputParameter< 7 > &input_parameter)=0
virtual void setInputLimits(const WaypointType &waypoint, ruckig::InputParameter< 7 > &input_parameter) const =0
WaypointMotion(std::vector< WaypointType > waypoints, bool return_when_finished=true)
Definition waypoint_motion.hpp:63
virtual void checkWaypoint(const WaypointType &waypoint) const
Definition waypoint_motion.hpp:160
virtual ControlSignalType getControlSignal(const RobotState &robot_state, const franka::Duration &time_step, const std::optional< ControlSignalType > &previous_command, const ruckig::InputParameter< 7 > &input_parameter)=0
virtual void setNewWaypoint(const RobotState &robot_state, const std::optional< ControlSignalType > &previous_command, const WaypointType &new_waypoint, ruckig::InputParameter< 7 > &input_parameter)=0
void initImpl(const RobotState &robot_state, const std::optional< ControlSignalType > &previous_command) override
Definition waypoint_motion.hpp:67
ControlSignalType nextCommandImpl(const RobotState &robot_state, franka::Duration time_step, franka::Duration rel_time, franka::Duration abs_time, const std::optional< ControlSignalType > &previous_command) override
Definition waypoint_motion.hpp:83
virtual std::tuple< Vector7d, Vector7d, Vector7d > getAbsoluteInputLimits() const =0
Definition dynamics_limit.cpp:8
std::array< double, dims > toStdD(const Eigen::Matrix< double, dims, 1 > &vector)
Definition util.hpp:18
ControlSignalType
Type of control signal.
Definition control_signal_type.hpp:8
Exception thrown if the motion planner fails.
Definition waypoint_motion.hpp:13
Full state of the robot.
Definition robot_state.hpp:20
A waypoint with a target and optional parameters.
Definition waypoint_motion.hpp:33
std::optional< franka::Duration > max_total_duration
Definition waypoint_motion.hpp:42
franka::Duration hold_target_duration
Definition waypoint_motion.hpp:40
TargetType target
Definition waypoint_motion.hpp:34
RelativeDynamicsFactor relative_dynamics_factor
Definition waypoint_motion.hpp:36
std::optional< franka::Duration > minimum_time
Definition waypoint_motion.hpp:38