Program Listing for File robot.h
↰ Return to documentation for file (include/franka/robot.h
)
// Copyright (c) 2023 Franka Robotics GmbH
// Use of this source code is governed by the Apache-2.0 license, see LICENSE
#pragma once
#include <functional>
#include <memory>
#include <mutex>
#include <string>
#include <franka/control_types.h>
#include <franka/duration.h>
#include <franka/lowpass_filter.h>
#include <franka/robot_model_base.h>
#include <franka/robot_state.h>
#include <research_interface/robot/service_types.h>
#include <franka/commands/get_robot_model_command.hpp>
namespace franka {
class Model;
class ActiveControlBase;
class Robot {
public:
using ServerVersion = uint16_t;
explicit Robot(const std::string& franka_address,
RealtimeConfig realtime_config = RealtimeConfig::kEnforce,
size_t log_size = 50);
Robot(Robot&& other) noexcept;
Robot& operator=(Robot&& other) noexcept;
virtual ~Robot() noexcept;
void control(std::function<Torques(const RobotState&, franka::Duration)> control_callback,
bool limit_rate = false,
double cutoff_frequency = kDefaultCutoffFrequency);
void control(
std::function<Torques(const RobotState&, franka::Duration)> control_callback,
std::function<JointPositions(const RobotState&, franka::Duration)> motion_generator_callback,
bool limit_rate = false,
double cutoff_frequency = kDefaultCutoffFrequency);
void control(
std::function<Torques(const RobotState&, franka::Duration)> control_callback,
std::function<JointVelocities(const RobotState&, franka::Duration)> motion_generator_callback,
bool limit_rate = false,
double cutoff_frequency = kDefaultCutoffFrequency);
void control(
std::function<Torques(const RobotState&, franka::Duration)> control_callback,
std::function<CartesianPose(const RobotState&, franka::Duration)> motion_generator_callback,
bool limit_rate = false,
double cutoff_frequency = kDefaultCutoffFrequency);
void control(std::function<Torques(const RobotState&, franka::Duration)> control_callback,
std::function<CartesianVelocities(const RobotState&, franka::Duration)>
motion_generator_callback,
bool limit_rate = false,
double cutoff_frequency = kDefaultCutoffFrequency);
void control(
std::function<JointPositions(const RobotState&, franka::Duration)> motion_generator_callback,
ControllerMode controller_mode = ControllerMode::kJointImpedance,
bool limit_rate = false,
double cutoff_frequency = kDefaultCutoffFrequency);
void control(
std::function<JointVelocities(const RobotState&, franka::Duration)> motion_generator_callback,
ControllerMode controller_mode = ControllerMode::kJointImpedance,
bool limit_rate = false,
double cutoff_frequency = kDefaultCutoffFrequency);
void control(
std::function<CartesianPose(const RobotState&, franka::Duration)> motion_generator_callback,
ControllerMode controller_mode = ControllerMode::kJointImpedance,
bool limit_rate = false,
double cutoff_frequency = kDefaultCutoffFrequency);
void control(std::function<CartesianVelocities(const RobotState&, franka::Duration)>
motion_generator_callback,
ControllerMode controller_mode = ControllerMode::kJointImpedance,
bool limit_rate = false,
double cutoff_frequency = kDefaultCutoffFrequency);
void read(std::function<bool(const RobotState&)> read_callback);
virtual RobotState readOnce();
auto getRobotModel() -> std::string;
void setCollisionBehavior(const std::array<double, 7>& lower_torque_thresholds_acceleration,
const std::array<double, 7>& upper_torque_thresholds_acceleration,
const std::array<double, 7>& lower_torque_thresholds_nominal,
const std::array<double, 7>& upper_torque_thresholds_nominal,
const std::array<double, 6>& lower_force_thresholds_acceleration,
const std::array<double, 6>& upper_force_thresholds_acceleration,
const std::array<double, 6>& lower_force_thresholds_nominal,
const std::array<double, 6>& upper_force_thresholds_nominal);
void setCollisionBehavior(const std::array<double, 7>& lower_torque_thresholds,
const std::array<double, 7>& upper_torque_thresholds,
const std::array<double, 6>& lower_force_thresholds,
const std::array<double, 6>& upper_force_thresholds);
void setJointImpedance(
const std::array<double, 7>& K_theta); // NOLINT(readability-identifier-naming)
void setCartesianImpedance(
const std::array<double, 6>& K_x); // NOLINT(readability-identifier-naming)
void setGuidingMode(const std::array<bool, 6>& guiding_mode, bool elbow);
void setK(const std::array<double, 16>& EE_T_K); // NOLINT(readability-identifier-naming)
void setEE(const std::array<double, 16>& NE_T_EE); // NOLINT(readability-identifier-naming)
void setLoad(double load_mass,
const std::array<double, 3>& F_x_Cload, // NOLINT(readability-identifier-naming)
const std::array<double, 9>& load_inertia);
void automaticErrorRecovery();
virtual std::unique_ptr<ActiveControlBase> startTorqueControl();
virtual std::unique_ptr<ActiveControlBase> startJointPositionControl(
const research_interface::robot::Move::ControllerMode& control_type);
virtual std::unique_ptr<ActiveControlBase> startJointVelocityControl(
const research_interface::robot::Move::ControllerMode& control_type);
virtual std::unique_ptr<ActiveControlBase> startCartesianPoseControl(
const research_interface::robot::Move::ControllerMode& control_type);
virtual std::unique_ptr<ActiveControlBase> startCartesianVelocityControl(
const research_interface::robot::Move::ControllerMode& control_type);
void stop();
Model loadModel();
// Loads the model library for the unittests mockRobotModel
Model loadModel(std::unique_ptr<RobotModelBase> robot_model);
ServerVersion serverVersion() const noexcept;
Robot(const Robot&) = delete;
Robot& operator=(const Robot&) = delete;
class Impl;
protected:
Robot(std::shared_ptr<Impl> robot_impl);
Robot() = default;
private:
template <typename T>
std::unique_ptr<ActiveControlBase> startControl(
const research_interface::robot::Move::ControllerMode& controller_type);
std::shared_ptr<Impl> impl_;
std::mutex control_mutex_;
};
} // namespace franka