Franky 0.12.0
A High-Level Motion API for Franka
Loading...
Searching...
No Matches
position_waypoint_motion.hpp
Go to the documentation of this file.
1#pragma once
2
3#include <ruckig/ruckig.hpp>
4
8#include "franky/util.hpp"
9
10namespace franky {
11
19template <typename TargetType>
23
31template <typename ControlSignalType, typename TargetType>
32class PositionWaypointMotion : public WaypointMotion<ControlSignalType, PositionWaypoint<TargetType>, TargetType> {
33 public:
43 std::vector<PositionWaypoint<TargetType>> waypoints, const RelativeDynamicsFactor &relative_dynamics_factor = 1.0,
44 bool return_when_finished = true)
45 : WaypointMotion<ControlSignalType, PositionWaypoint<TargetType>, TargetType>(waypoints, return_when_finished),
46 relative_dynamics_factor_(relative_dynamics_factor) {}
47
48 protected:
50 const PositionWaypoint<TargetType> &waypoint, ruckig::InputParameter<7> &input_parameter) const override {
51 auto [vel_lim, acc_lim, jerk_lim] = getAbsoluteInputLimits();
52
53 auto relative_dynamics_factor =
54 waypoint.relative_dynamics_factor * relative_dynamics_factor_ * this->robot()->relative_dynamics_factor();
55
56 input_parameter.max_velocity = toStdD<7>(relative_dynamics_factor.velocity() * vel_lim);
57 input_parameter.max_acceleration = toStdD<7>(relative_dynamics_factor.acceleration() * acc_lim);
58 input_parameter.max_jerk = toStdD<7>(relative_dynamics_factor.jerk() * jerk_lim);
59
60 if (relative_dynamics_factor.max_dynamics()) {
61 input_parameter.synchronization = ruckig::Synchronization::TimeIfNecessary;
62 } else {
63 input_parameter.synchronization = ruckig::Synchronization::Time;
64 if (waypoint.minimum_time.has_value()) input_parameter.minimum_duration = waypoint.minimum_time.value().toSec();
65 }
66 }
67
69 const franka::Duration &time_step, const ruckig::InputParameter<7> &input_parameter,
70 ruckig::OutputParameter<7> &output_parameter) const override {
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
87 [[nodiscard]] std::tuple<Vector7d, Vector7d, Vector7d> getAbsoluteInputLimits() const override = 0;
88
89 private:
90 RelativeDynamicsFactor relative_dynamics_factor_;
91};
92
93} // namespace franky
Robot * robot() const
Definition motion.hpp:114
A motion following multiple positional waypoints in a time-optimal way. Works with arbitrary initial ...
Definition position_waypoint_motion.hpp:32
void extrapolateMotion(const franka::Duration &time_step, const ruckig::InputParameter< 7 > &input_parameter, ruckig::OutputParameter< 7 > &output_parameter) const override
Definition position_waypoint_motion.hpp:68
void setInputLimits(const PositionWaypoint< TargetType > &waypoint, ruckig::InputParameter< 7 > &input_parameter) const override
Definition position_waypoint_motion.hpp:49
std::tuple< Vector7d, Vector7d, Vector7d > getAbsoluteInputLimits() const override=0
PositionWaypointMotion(std::vector< PositionWaypoint< TargetType > > waypoints, const RelativeDynamicsFactor &relative_dynamics_factor=1.0, bool return_when_finished=true)
Definition position_waypoint_motion.hpp:42
Relative dynamics factors.
Definition relative_dynamics_factor.hpp:13
RelativeDynamicsFactor relative_dynamics_factor()
Returns the current global relative dynamics factor of the robot.
Definition robot.cpp:136
A motion following multiple waypoints in a time-optimal way. Works with arbitrary initial conditions.
Definition waypoint_motion.hpp:60
Definition dynamics_limit.cpp:8
std::array< double, dims > toStdD(const Eigen::Matrix< double, dims, 1 > &vector)
Definition util.hpp:18
ReferenceType
Enum class for reference types.
Definition reference_type.hpp:10
ControlSignalType
Type of control signal.
Definition control_signal_type.hpp:8
A position waypoint with a target and optional parameters.
Definition position_waypoint_motion.hpp:20
ReferenceType reference_type
Definition position_waypoint_motion.hpp:21
A waypoint with a target and optional parameters.
Definition waypoint_motion.hpp:38
RelativeDynamicsFactor relative_dynamics_factor
Definition waypoint_motion.hpp:41