Franky 0.12.0
A High-Level Motion API for Franka
Loading...
Searching...
No Matches
cartesian_velocity_waypoint_motion.hpp
Go to the documentation of this file.
1#pragma once
2
3#include <franka/robot_state.h>
4
5#include <atomic>
6#include <optional>
7#include <ruckig/ruckig.hpp>
8
11#include "franky/robot.hpp"
12#include "franky/robot_pose.hpp"
13#include "franky/util.hpp"
14
15namespace franky {
16
22class CartesianVelocityWaypointMotion : public VelocityWaypointMotion<franka::CartesianVelocities, RobotVelocity> {
23 public:
34 const std::vector<VelocityWaypoint<RobotVelocity>> &waypoints,
35 const RelativeDynamicsFactor &relative_dynamics_factor = 1.0, Affine ee_frame = Affine::Identity());
36
37 protected:
38 void checkWaypoint(const VelocityWaypoint<RobotVelocity> &waypoint) const override;
39
41 const RobotState &robot_state, const std::optional<franka::CartesianVelocities> &previous_command,
42 ruckig::InputParameter<7> &input_parameter) override;
43
44 void setNewWaypoint(
45 const RobotState &robot_state, const std::optional<franka::CartesianVelocities> &previous_command,
46 const VelocityWaypoint<RobotVelocity> &new_waypoint, ruckig::InputParameter<7> &input_parameter) override;
47
48 [[nodiscard]] std::tuple<Vector7d, Vector7d, Vector7d> getAbsoluteInputLimits() const override;
49
50 [[nodiscard]] franka::CartesianVelocities getControlSignal(
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;
54
55 [[nodiscard]] std::tuple<Vector7d, Vector7d, Vector7d> getStateEstimate(const RobotState &robot_state) const override;
56
57 [[nodiscard]] std::tuple<Vector7d, Vector7d, Vector7d> getGoalTolerance() const override;
58
59 private:
60 Affine ee_frame_;
61 double last_elbow_pos_{};
62 double last_elbow_vel_{};
63
64 static Vector7d vec_cart_rot_elbow(double cart, double rot, double elbow) {
65 return {cart, cart, cart, rot, rot, rot, elbow};
66 }
67};
68
69} // namespace franky
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