stop_motion.hpp

Go to the documentation of this file.

#pragma once
 
#include <franka/control_types.h>
 
#include <optional>
 
#include "franky/motion/cartesian_motion.hpp"
#include "franky/motion/cartesian_velocity_motion.hpp"
#include "franky/motion/joint_impedance_base.hpp"
#include "franky/motion/joint_motion.hpp"
#include "franky/motion/joint_velocity_motion.hpp"
#include "franky/motion/motion.hpp"
#include "franky/motion/reference_type.hpp"
#include "franky/motion/torque_control_utils.hpp"
#include "franky/robot_state.hpp"
#include "franky/types.hpp"
 
namespace franky {
 
template <typename ControlSignalType>
class StopMotion;
 
template <>
class StopMotion<franka::JointPositions> : public JointMotion {
 public:
  explicit StopMotion(const RelativeDynamicsFactor &relative_dynamics_factor = 1.0)
      : JointMotion(JointState(Vector7d::Zero()), ReferenceType::kRelative, relative_dynamics_factor) {}
};
 
template <>
class StopMotion<franka::JointVelocities> : public JointVelocityMotion {
 public:
  explicit StopMotion(const RelativeDynamicsFactor &relative_dynamics_factor = 1.0)
      : JointVelocityMotion(Vector7d::Zero(), franka::Duration(0), relative_dynamics_factor) {}
};
 
template <>
class StopMotion<franka::CartesianPose> : public CartesianWaypointMotion {
 public:
  explicit StopMotion(const RelativeDynamicsFactor &relative_dynamics_factor = 1.0)
      : CartesianWaypointMotion(
            {PositionWaypoint<CartesianState>{
                {
                    .target = RobotPose(),
                    .hold_target_duration = franka::Duration(50),
                },
                ReferenceType::kRelative}},
            relative_dynamics_factor) {}
};
 
template <>
class StopMotion<franka::CartesianVelocities> : public CartesianVelocityMotion {
 public:
  explicit StopMotion(const RelativeDynamicsFactor &relative_dynamics_factor = 1.0)
      : CartesianVelocityMotion(RobotVelocity(), franka::Duration(0), relative_dynamics_factor) {}
};
 
struct TorqueStopParams {
  Vector7d damping{defaultJointImpedanceDamping()};
 
  double ramp_duration{0.2};
 
  double velocity_epsilon{0.02};
 
  double max_duration{2.0};
 
  bool compensate_coriolis{true};
 
  double max_delta_tau{1.0};
};
 
template <>
class StopMotion<franka::Torques> : public Motion<franka::Torques> {
 public:
  explicit StopMotion(const TorqueStopParams &params = {}) : params_(params) {}
 
 protected:
  void initImpl(const RobotState &robot_state, const std::optional<franka::Torques> &previous_command) override;
 
  franka::Torques nextCommandImpl(
      const RobotState &robot_state, franka::Duration time_step, franka::Duration rel_time, franka::Duration abs_time,
      const std::optional<franka::Torques> &previous_command) override;
 
 private:
  TorqueStopParams params_;
  // Initial torque command captured in initImpl.
  Vector7d tau_start_{Vector7d::Zero()};
};
 
}  // namespace franky