Franky 1.0.2
A High-Level Motion API for Franka
Loading...
Searching...
No Matches
velocity_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
17template <typename TargetType>
19
27template <typename ControlSignalType, typename TargetType>
28class VelocityWaypointMotion : public WaypointMotion<ControlSignalType, VelocityWaypoint<TargetType>, TargetType> {
29 public:
37 std::vector<VelocityWaypoint<TargetType>> waypoints, const RelativeDynamicsFactor &relative_dynamics_factor = 1.0)
39 relative_dynamics_factor_(relative_dynamics_factor) {}
40
41 protected:
43 const VelocityWaypoint<TargetType> &waypoint, ruckig::InputParameter<7> &input_parameter) const override {
45
46 auto relative_dynamics_factor =
47 waypoint.relative_dynamics_factor * relative_dynamics_factor_ * this->robot()->relative_dynamics_factor();
48
49 input_parameter.max_velocity = toStdD<7>(relative_dynamics_factor.acceleration() * acc_lim);
50 input_parameter.max_acceleration = toStdD<7>(relative_dynamics_factor.jerk() * jerk_lim);
51 input_parameter.max_jerk = toStdD<7>(Vector7d::Constant(std::numeric_limits<double>::infinity()));
52
53 if (relative_dynamics_factor.max_dynamics()) {
54 input_parameter.synchronization = ruckig::Synchronization::TimeIfNecessary;
55 } else {
56 input_parameter.synchronization = ruckig::Synchronization::Time;
57 if (waypoint.minimum_time.has_value()) input_parameter.minimum_duration = waypoint.minimum_time.value().toSec();
58 }
59 }
60
62 const RobotState &robot_state, const franka::Duration &time_step,
63 const ruckig::InputParameter<7> &input_parameter, ruckig::OutputParameter<7> &output_parameter) const override {
65
66 // We use the desired state here as this is likely what the robot uses internally as well
68
69 auto vel = toEigenD<7>(input_parameter.current_velocity);
70
71 // Retain difference between desired state and motion planner state
72 auto vel_diff = vel - vel_d;
73
74 auto new_vel_d = (vel_d + acc_d * time_step.toSec()).cwiseMin(vel_lim).cwiseMax(-vel_lim);
75
76 // Franka assumes a constant acceleration model if no new input is received.
77 // See https://frankaemika.github.io/docs/libfranka.html#under-the-hood
78 output_parameter.new_acceleration = toStdD<7>(Vector7d::Zero());
79 output_parameter.new_velocity = input_parameter.current_velocity;
81 }
82
83 [[nodiscard]] std::tuple<Vector7d, Vector7d, Vector7d> getAbsoluteInputLimits() const override = 0;
84
85 [[nodiscard]] virtual std::tuple<Vector7d, Vector7d, Vector7d> getDesiredState(const RobotState &robot_state) const = 0;
86
87 private:
88 RelativeDynamicsFactor relative_dynamics_factor_;
89};
90
91} // namespace franky
Robot * robot() const
Definition motion.hpp:114
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:128
A motion following multiple positional waypoints in a time-optimal way. Works with arbitrary initial ...
Definition velocity_waypoint_motion.hpp:28
void setInputLimits(const VelocityWaypoint< TargetType > &waypoint, ruckig::InputParameter< 7 > &input_parameter) const override
Definition velocity_waypoint_motion.hpp:42
VelocityWaypointMotion(std::vector< VelocityWaypoint< TargetType > > waypoints, const RelativeDynamicsFactor &relative_dynamics_factor=1.0)
Definition velocity_waypoint_motion.hpp:36
virtual std::tuple< Vector7d, Vector7d, Vector7d > getDesiredState(const RobotState &robot_state) const =0
std::tuple< Vector7d, Vector7d, Vector7d > getAbsoluteInputLimits() const override=0
void extrapolateMotion(const RobotState &robot_state, const franka::Duration &time_step, const ruckig::InputParameter< 7 > &input_parameter, ruckig::OutputParameter< 7 > &output_parameter) const override
Definition velocity_waypoint_motion.hpp:61
A motion following multiple waypoints in a time-optimal way. Works with arbitrary initial conditions.
Definition waypoint_motion.hpp:53
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
Full state of the robot.
Definition robot_state.hpp:20
A waypoint with a target and optional parameters.
Definition waypoint_motion.hpp:33