Skip to content

Commit 0d65020

Browse files
TimSchneider42TimSchneider42
committed
Implemented better handling of dropped packets
1 parent 8762dd1 commit 0d65020

File tree

3 files changed

+97
-68
lines changed

3 files changed

+97
-68
lines changed

‎include/franky/motion/position_waypoint_motion.hpp‎

Lines changed: 26 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -2,10 +2,10 @@
22

33
#include <ruckig/ruckig.hpp>
44

5-
#include "franky/util.hpp"
6-
#include "franky/relative_dynamics_factor.hpp"
75
#include "franky/motion/reference_type.hpp"
86
#include "franky/motion/waypoint_motion.hpp"
7+
#include "franky/relative_dynamics_factor.hpp"
8+
#include "franky/util.hpp"
99

1010
namespace franky {
1111

@@ -16,7 +16,7 @@ namespace franky {
1616
*
1717
* @param reference_type The reference type (absolute or relative).
1818
*/
19-
template<typename TargetType>
19+
template <typename TargetType>
2020
struct PositionWaypoint : public Waypoint<TargetType> {
2121
ReferenceType reference_type{ReferenceType::kAbsolute};
2222
};
@@ -28,7 +28,7 @@ struct PositionWaypoint : public Waypoint<TargetType> {
2828
* franka::CartesianVelocities, franka::JointPositions or franka::CartesianPose.
2929
* @tparam TargetType The type of the target of the waypoints.
3030
*/
31-
template<typename ControlSignalType, typename TargetType>
31+
template <typename ControlSignalType, typename TargetType>
3232
class PositionWaypointMotion : public WaypointMotion<ControlSignalType, PositionWaypoint<TargetType>, TargetType> {
3333
public:
3434
/**
@@ -40,10 +40,8 @@ class PositionWaypointMotion : public WaypointMotion<ControlSignalType, Position
4040
* last target.
4141
*/
4242
explicit PositionWaypointMotion(
43-
std::vector<PositionWaypoint<TargetType>> waypoints,
44-
const RelativeDynamicsFactor &relative_dynamics_factor = 1.0,
45-
bool return_when_finished = true
46-
)
43+
std::vector<PositionWaypoint<TargetType>> waypoints, const RelativeDynamicsFactor &relative_dynamics_factor = 1.0,
44+
bool return_when_finished = true)
4745
: WaypointMotion<ControlSignalType, PositionWaypoint<TargetType>, TargetType>(waypoints, return_when_finished),
4846
relative_dynamics_factor_(relative_dynamics_factor) {}
4947

@@ -63,11 +61,29 @@ class PositionWaypointMotion : public WaypointMotion<ControlSignalType, Position
6361
input_parameter.synchronization = ruckig::Synchronization::TimeIfNecessary;
6462
} else {
6563
input_parameter.synchronization = ruckig::Synchronization::Time;
66-
if (waypoint.minimum_time.has_value())
67-
input_parameter.minimum_duration = waypoint.minimum_time.value();
64+
if (waypoint.minimum_time.has_value()) input_parameter.minimum_duration = waypoint.minimum_time.value();
6865
}
6966
}
7067

68+
void extrapolateMotion(
69+
const franka::Duration &time_step, const ruckig::InputParameter<7> &input_parameter,
70+
ruckig::OutputParameter<7> &output_parameter) const override {
71+
auto [vel_lim, acc_lim, jerk_lim] = getAbsoluteInputLimits();
72+
73+
auto acc = toEigenD<7>(input_parameter.current_acceleration);
74+
auto vel = toEigenD<7>(input_parameter.current_velocity);
75+
auto pos = toEigenD<7>(input_parameter.current_position);
76+
77+
auto new_vel = (vel + acc * time_step.toSec()).cwiseMin(vel_lim).cwiseMax(-vel_lim);
78+
auto new_pos = pos + (vel + new_vel) * time_step.toSec() / 2.0;
79+
80+
// Franka assumes a constant acceleration model if no new input is received.
81+
// See https://frankaemika.github.io/docs/libfranka.html#under-the-hood
82+
output_parameter.new_acceleration = input_parameter.current_acceleration;
83+
output_parameter.new_velocity = toStdD<7>(new_vel);
84+
output_parameter.new_position = toStdD<7>(new_pos);
85+
}
86+
7187
[[nodiscard]] std::tuple<Vector7d, Vector7d, Vector7d> getAbsoluteInputLimits() const override = 0;
7288

7389
private:

‎include/franky/motion/velocity_waypoint_motion.hpp‎

Lines changed: 17 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -61,6 +61,23 @@ class VelocityWaypointMotion : public WaypointMotion<ControlSignalType, Velocity
6161
}
6262
}
6363

64+
void extrapolateMotion(
65+
const franka::Duration &time_step, const ruckig::InputParameter<7> &input_parameter,
66+
ruckig::OutputParameter<7> &output_parameter) const override {
67+
auto [vel_lim, acc_lim, jerk_lim] = getAbsoluteInputLimits();
68+
69+
auto acc = toEigenD<7>(input_parameter.current_velocity);
70+
auto vel = toEigenD<7>(input_parameter.current_position);
71+
72+
auto new_vel = (vel + acc * time_step.toSec()).cwiseMin(vel_lim).cwiseMax(-vel_lim);
73+
74+
// Franka assumes a constant acceleration model if no new input is received.
75+
// See https://frankaemika.github.io/docs/libfranka.html#under-the-hood
76+
output_parameter.new_acceleration = toStdD<7>(Vector7d::Zero());
77+
output_parameter.new_velocity = input_parameter.current_velocity;
78+
output_parameter.new_position = toStdD<7>(new_vel);
79+
}
80+
6481
[[nodiscard]] std::tuple<Vector7d, Vector7d, Vector7d> getAbsoluteInputLimits() const override = 0;
6582

6683
private:

‎include/franky/motion/waypoint_motion.hpp‎

Lines changed: 54 additions & 58 deletions
Original file line numberDiff line numberDiff line change
@@ -27,7 +27,7 @@ struct MotionPlannerException : std::runtime_error {
2727
* @param minimum_time The minimum time to get to the next waypoint in [s].
2828
* @param hold_target_duration For how long to hold the target of this waypoint after it has been reached.
2929
*/
30-
template<typename TargetType>
30+
template <typename TargetType>
3131
struct Waypoint {
3232
TargetType target;
3333

@@ -45,10 +45,11 @@ struct Waypoint {
4545
* @tparam WaypointType The type of the waypoints. Must subclass Waypoint<TargetType>.
4646
* @tparam TargetType The type of the target of the waypoints.
4747
*/
48-
template<typename ControlSignalType, typename WaypointType, typename TargetType>
48+
template <typename ControlSignalType, typename WaypointType, typename TargetType>
4949
class WaypointMotion : public Motion<ControlSignalType> {
50-
static_assert(std::is_base_of_v<Waypoint<TargetType>, WaypointType>,
51-
"WaypointType must inherit from Waypoint<TargetType>");
50+
static_assert(
51+
std::is_base_of_v<Waypoint<TargetType>, WaypointType>, "WaypointType must inherit from Waypoint<TargetType>");
52+
5253
public:
5354
/**
5455
* @param waypoints The waypoints to follow.
@@ -59,8 +60,7 @@ class WaypointMotion : public Motion<ControlSignalType> {
5960
: waypoints_(std::move(waypoints)), return_when_finished_(return_when_finished), prev_result_() {}
6061

6162
protected:
62-
void initImpl(const RobotState &robot_state,
63-
const std::optional<ControlSignalType> &previous_command) override {
63+
void initImpl(const RobotState &robot_state, const std::optional<ControlSignalType> &previous_command) override {
6464
target_reached_time_ = std::nullopt;
6565

6666
initWaypointMotion(robot_state, previous_command, input_parameter_);
@@ -76,76 +76,73 @@ class WaypointMotion : public Motion<ControlSignalType> {
7676
}
7777
}
7878

79-
ControlSignalType
80-
nextCommandImpl(
81-
const RobotState &robot_state,
82-
franka::Duration time_step,
83-
franka::Duration rel_time,
84-
franka::Duration abs_time,
79+
ControlSignalType nextCommandImpl(
80+
const RobotState &robot_state, franka::Duration time_step, franka::Duration rel_time, franka::Duration abs_time,
8581
const std::optional<ControlSignalType> &previous_command) override {
86-
const auto sub_step = franka::Duration(1);
87-
const uint64_t steps = std::max<uint64_t>(time_step / sub_step, 1);
88-
for (size_t i = 0; i < steps; i++) {
89-
auto rel_time_i = rel_time - time_step + (i + 1) * time_step / steps;
90-
if (prev_result_ == ruckig::Result::Finished) {
91-
if (!target_reached_time_.has_value()) {
92-
target_reached_time_ = rel_time_i;
93-
}
94-
auto hold_target_duration = waypoint_iterator_->hold_target_duration;
95-
if (waypoint_iterator_ + 1 != waypoints_.end()) {
96-
// Allow cooldown of motion, so that the low-pass filter has time to adjust to target values
97-
hold_target_duration = std::max(hold_target_duration, franka::Duration(5));
98-
}
99-
if (rel_time_i - target_reached_time_.value() >= hold_target_duration) {
100-
target_reached_time_ = std::nullopt;
101-
if (waypoint_iterator_ != waypoints_.end()) {
102-
++waypoint_iterator_;
103-
}
104-
}
105-
if (waypoint_iterator_ == waypoints_.end()) {
106-
auto command = getControlSignal(robot_state, time_step, previous_command, input_parameter_);
107-
if (!return_when_finished_)
108-
return command;
109-
return franka::MotionFinished(command);
110-
} else {
111-
checkWaypoint(*waypoint_iterator_);
112-
setNewWaypoint(robot_state, previous_command, *waypoint_iterator_, input_parameter_);
113-
setInputLimits(*waypoint_iterator_, input_parameter_);
114-
}
82+
const auto expected_time_step = franka::Duration(1);
83+
if (time_step > expected_time_step) {
84+
// In this case, we missed a couple of steps for some reason. Hence, extrapolate the way the robot does if it
85+
// does not receive data (constant acceleration model).
86+
// See https://frankaemika.github.io/docs/libfranka.html#under-the-hood
87+
extrapolateMotion(time_step - expected_time_step, input_parameter_, output_parameter_);
88+
output_parameter_.pass_to_input(input_parameter_);
89+
}
90+
if (prev_result_ == ruckig::Result::Finished) {
91+
if (!target_reached_time_.has_value()) {
92+
target_reached_time_ = rel_time;
93+
}
94+
auto hold_target_duration = waypoint_iterator_->hold_target_duration;
95+
if (waypoint_iterator_ + 1 != waypoints_.end()) {
96+
// Allow cooldown of motion, so that the low-pass filter has time to adjust to target values
97+
hold_target_duration = std::max(hold_target_duration, franka::Duration(5));
11598
}
116-
if (waypoint_iterator_ != waypoints_.end()) {
117-
prev_result_ = trajectory_generator_.update(input_parameter_, output_parameter_);
118-
if (prev_result_ == ruckig::Result::Working || prev_result_ == ruckig::Result::Finished) {
119-
output_parameter_.pass_to_input(input_parameter_);
120-
} else {
121-
throw MotionPlannerException("Motion planner failed with error code " + std::to_string(prev_result_));
99+
if (rel_time - target_reached_time_.value() >= hold_target_duration) {
100+
target_reached_time_ = std::nullopt;
101+
if (waypoint_iterator_ != waypoints_.end()) {
102+
++waypoint_iterator_;
122103
}
123104
}
105+
if (waypoint_iterator_ == waypoints_.end()) {
106+
auto command = getControlSignal(robot_state, time_step, previous_command, input_parameter_);
107+
if (!return_when_finished_) return command;
108+
return franka::MotionFinished(command);
109+
} else {
110+
checkWaypoint(*waypoint_iterator_);
111+
setNewWaypoint(robot_state, previous_command, *waypoint_iterator_, input_parameter_);
112+
setInputLimits(*waypoint_iterator_, input_parameter_);
113+
}
114+
}
115+
if (waypoint_iterator_ != waypoints_.end()) {
116+
prev_result_ = trajectory_generator_.update(input_parameter_, output_parameter_);
117+
if (prev_result_ == ruckig::Result::Working || prev_result_ == ruckig::Result::Finished) {
118+
output_parameter_.pass_to_input(input_parameter_);
119+
} else {
120+
throw MotionPlannerException("Motion planner failed with error code " + std::to_string(prev_result_));
121+
}
124122
}
125123

126124
return getControlSignal(robot_state, time_step, previous_command, input_parameter_);
127125
};
128126

129127
virtual void initWaypointMotion(
130-
const RobotState &robot_state,
131-
const std::optional<ControlSignalType> &previous_command,
128+
const RobotState &robot_state, const std::optional<ControlSignalType> &previous_command,
132129
ruckig::InputParameter<7> &input_parameter) = 0;
133130

134131
virtual void setNewWaypoint(
135-
const RobotState &robot_state,
136-
const std::optional<ControlSignalType> &previous_command,
137-
const WaypointType &new_waypoint,
138-
ruckig::InputParameter<7> &input_parameter) = 0;
132+
const RobotState &robot_state, const std::optional<ControlSignalType> &previous_command,
133+
const WaypointType &new_waypoint, ruckig::InputParameter<7> &input_parameter) = 0;
134+
135+
virtual void extrapolateMotion(
136+
const franka::Duration &time_step, const ruckig::InputParameter<7> &input_parameter,
137+
ruckig::OutputParameter<7> &output_parameter) const = 0;
139138

140139
virtual void checkWaypoint(const WaypointType &waypoint) const {}
141140

142141
[[nodiscard]] virtual std::tuple<Vector7d, Vector7d, Vector7d> getAbsoluteInputLimits() const = 0;
143142

144143
[[nodiscard]] virtual ControlSignalType getControlSignal(
145-
const RobotState &robot_state,
146-
const franka::Duration &time_step,
147-
const std::optional<ControlSignalType> &previous_command,
148-
const ruckig::InputParameter<7> &input_parameter) = 0;
144+
const RobotState &robot_state, const franka::Duration &time_step,
145+
const std::optional<ControlSignalType> &previous_command, const ruckig::InputParameter<7> &input_parameter) = 0;
149146

150147
virtual void setInputLimits(const WaypointType &waypoint, ruckig::InputParameter<7> &input_parameter) const = 0;
151148

@@ -161,7 +158,6 @@ class WaypointMotion : public Motion<ControlSignalType> {
161158
typename std::vector<WaypointType>::iterator waypoint_iterator_;
162159

163160
std::optional<franka::Duration> target_reached_time_;
164-
165161
};
166162

167163
} // namespace franky

0 commit comments

Comments
 (0)