Program Listing for File model.h
↰ Return to documentation for file (include/franka/model.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 <array>
#include <memory>
#include <franka/robot.h>
#include <franka/robot_model_base.h>
#include <franka/robot_state.h>
namespace franka {
enum class Frame {
kJoint1,
kJoint2,
kJoint3,
kJoint4,
kJoint5,
kJoint6,
kJoint7,
kFlange,
kEndEffector,
kStiffness
};
Frame operator++(Frame& frame, int /* dummy */) noexcept;
class ModelLibrary;
class Network;
class Model {
public:
explicit Model(franka::Network& network, const std::string& urdf_model);
explicit Model(franka::Network& network, std::unique_ptr<RobotModelBase> robot_model);
Model(Model&& model) noexcept;
Model& operator=(Model&& model) noexcept;
~Model() noexcept;
std::array<double, 16> pose(Frame frame, const franka::RobotState& robot_state) const;
std::array<double, 16> pose(
Frame frame,
const std::array<double, 7>& q,
const std::array<double, 16>& F_T_EE, // NOLINT(readability-identifier-naming)
const std::array<double, 16>& EE_T_K) // NOLINT(readability-identifier-naming)
const;
std::array<double, 42> bodyJacobian(Frame frame, const franka::RobotState& robot_state) const;
std::array<double, 42> bodyJacobian(
Frame frame,
const std::array<double, 7>& q,
const std::array<double, 16>& F_T_EE, // NOLINT(readability-identifier-naming)
const std::array<double, 16>& EE_T_K) // NOLINT(readability-identifier-naming)
const;
std::array<double, 42> zeroJacobian(Frame frame, const franka::RobotState& robot_state) const;
std::array<double, 42> zeroJacobian(
Frame frame,
const std::array<double, 7>& q,
const std::array<double, 16>& F_T_EE, // NOLINT(readability-identifier-naming)
const std::array<double, 16>& EE_T_K) // NOLINT(readability-identifier-naming)
const;
std::array<double, 49> mass(const franka::RobotState& robot_state) const noexcept;
std::array<double, 49> mass(
const std::array<double, 7>& q,
const std::array<double, 9>& I_total, // NOLINT(readability-identifier-naming)
double m_total,
const std::array<double, 3>& F_x_Ctotal) // NOLINT(readability-identifier-naming)
const noexcept;
std::array<double, 7> coriolis(const franka::RobotState& robot_state) const noexcept;
std::array<double, 7> coriolis(
const std::array<double, 7>& q,
const std::array<double, 7>& dq,
const std::array<double, 9>& I_total, // NOLINT(readability-identifier-naming)
double m_total,
const std::array<double, 3>& F_x_Ctotal) // NOLINT(readability-identifier-naming)
const noexcept;
std::array<double, 7> gravity(
const std::array<double, 7>& q,
double m_total,
const std::array<double, 3>& F_x_Ctotal, // NOLINT(readability-identifier-naming)
const std::array<double, 3>& gravity_earth = {{0., 0., -9.81}}) const noexcept;
std::array<double, 7> gravity(const franka::RobotState& robot_state,
const std::array<double, 3>& gravity_earth) const noexcept;
std::array<double, 7> gravity(const franka::RobotState& robot_state) const noexcept;
Model(const Model&) = delete;
Model& operator=(const Model&) = delete;
private:
std::unique_ptr<ModelLibrary> library_;
std::unique_ptr<RobotModelBase> robot_model_;
};
} // namespace franka