4#include <ruckig/ruckig.hpp>
14 using std::runtime_error::runtime_error;
32template <
typename TargetType>
52template <
typename ControlSignalType,
typename Waypo
intType,
typename TargetType>
55 std::is_base_of_v<Waypoint<TargetType>,
WaypointType>,
"WaypointType must inherit from Waypoint<TargetType>");
64 : waypoints_(std::move(
waypoints)), return_when_finished_(return_when_finished), prev_result_() {}
68 target_reached_time_ = std::nullopt;
69 waypoint_started_time_ = franka::Duration(0);
71 waypoint_iterator_ = waypoints_.begin();
73 if (waypoint_iterator_ != waypoints_.end()) {
77 prev_result_ = ruckig::Result::Working;
79 prev_result_ = ruckig::Result::Finished;
92 output_parameter_.pass_to_input(input_parameter_);
96 if (waypoint_iterator_ != waypoints_.end() && waypoint_iterator_->max_total_duration.has_value()) {
101 if (!target_reached_time_.has_value()) {
104 if (waypoint_iterator_ != waypoints_.end()) {
105 auto hold_target_duration = waypoint_iterator_->hold_target_duration;
106 if (waypoint_iterator_ + 1 == waypoints_.end()) {
108 hold_target_duration = std::max(hold_target_duration, franka::Duration(10));
111 target_reached_time_ = std::nullopt;
112 ++waypoint_iterator_;
113 if (waypoint_iterator_ != waypoints_.end()) {
121 if (waypoint_iterator_ == waypoints_.end()) {
123 if (!return_when_finished_)
return command;
124 return franka::MotionFinished(
command);
128 assert(waypoint_iterator_ != waypoints_.end());
130 prev_result_ = trajectory_generator_.update(input_parameter_, output_parameter_);
131 if (prev_result_ == ruckig::Result::Working || prev_result_ == ruckig::Result::Finished) {
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;
140 output_parameter_.pass_to_input(input_parameter_);
171 std::vector<WaypointType> waypoints_;
172 bool return_when_finished_{
true};
175 ruckig::Result prev_result_;
176 ruckig::InputParameter<7> input_parameter_;
177 ruckig::OutputParameter<7> output_parameter_;
179 typename std::vector<WaypointType>::iterator waypoint_iterator_;
181 std::optional<franka::Duration> target_reached_time_;
182 franka::Duration waypoint_started_time_;
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