3#include <franka/robot_state.h>
7#include <ruckig/ruckig.hpp>
41 const RobotState &robot_state,
const std::optional<franka::CartesianVelocities> &previous_command,
42 ruckig::InputParameter<7> &input_parameter)
override;
45 const RobotState &robot_state,
const std::optional<franka::CartesianVelocities> &previous_command,
51 const RobotState &robot_state,
const franka::Duration &time_step,
52 const std::optional<franka::CartesianVelocities> &previous_command,
53 const ruckig::InputParameter<7> &input_parameter)
override;
57 [[nodiscard]] std::tuple<Vector7d, Vector7d, Vector7d>
getGoalTolerance()
const override;
61 double last_elbow_pos_{};
62 double last_elbow_vel_{};
64 static Vector7d vec_cart_rot_elbow(
double cart,
double rot,
double elbow) {
65 return {cart, cart, cart, rot, rot, rot, elbow};
Cartesian velocity waypoint motion.
Definition cartesian_velocity_waypoint_motion.hpp:22
franka::CartesianVelocities getControlSignal(const RobotState &robot_state, const franka::Duration &time_step, const std::optional< franka::CartesianVelocities > &previous_command, const ruckig::InputParameter< 7 > &input_parameter) override
Definition cartesian_velocity_waypoint_motion.cpp:50
void checkWaypoint(const VelocityWaypoint< RobotVelocity > &waypoint) const override
Definition cartesian_velocity_waypoint_motion.cpp:17
void initWaypointMotion(const RobotState &robot_state, const std::optional< franka::CartesianVelocities > &previous_command, ruckig::InputParameter< 7 > &input_parameter) override
Definition cartesian_velocity_waypoint_motion.cpp:26
std::tuple< Vector7d, Vector7d, Vector7d > getStateEstimate(const RobotState &robot_state) const override
Definition cartesian_velocity_waypoint_motion.cpp:101
std::tuple< Vector7d, Vector7d, Vector7d > getAbsoluteInputLimits() const override
Definition cartesian_velocity_waypoint_motion.cpp:89
void setNewWaypoint(const RobotState &robot_state, const std::optional< franka::CartesianVelocities > &previous_command, const VelocityWaypoint< RobotVelocity > &new_waypoint, ruckig::InputParameter< 7 > &input_parameter) override
Definition cartesian_velocity_waypoint_motion.cpp:78
std::tuple< Vector7d, Vector7d, Vector7d > getGoalTolerance() const override
Definition cartesian_velocity_waypoint_motion.cpp:110
Relative dynamics factors.
Definition relative_dynamics_factor.hpp:13
A motion following multiple positional waypoints in a time-optimal way. Works with arbitrary initial ...
Definition velocity_waypoint_motion.hpp:28
Definition dynamics_limit.cpp:8
Eigen::Vector< double, 7 > Vector7d
Definition types.hpp:12
Eigen::Affine3d Affine
Definition types.hpp:16
Full state of the robot.
Definition robot_state.hpp:20
A waypoint with a target and optional parameters.
Definition waypoint_motion.hpp:38