Skip to content

Commit 8baadee

Browse files
TimSchneider42TimSchneider42
committed
Removed kinematics code and implemented model loading instead
1 parent f212231 commit 8baadee

File tree

14 files changed

+413
-476
lines changed

14 files changed

+413
-476
lines changed

‎include/franky.hpp‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -22,7 +22,7 @@
2222
#include "franky/gripper.hpp"
2323
#include "franky/elbow_state.hpp"
2424
#include "franky/joint_state.hpp"
25-
#include "franky/kinematics.hpp"
25+
#include "franky/model.hpp"
2626
#include "franky/robot.hpp"
2727
#include "franky/robot_pose.hpp"
2828
#include "franky/robot_velocity.hpp"

‎include/franky/kinematics.hpp‎

Lines changed: 0 additions & 64 deletions
This file was deleted.

‎include/franky/model.hpp‎

Lines changed: 179 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,179 @@
1+
#pragma once
2+
3+
#include <Eigen/Dense>
4+
#include <memory>
5+
6+
#include <franka/model.h>
7+
#include <franka/robot_state.h>
8+
9+
#include "franky/types.hpp"
10+
11+
namespace franky {
12+
13+
/**
14+
* @brief A wrapper around franka::Model that uses Eigen types.
15+
*
16+
* This class exposes the same functionality as franka::Model, but uses Eigen types for inputs and outputs
17+
* instead of std::array. All 2D arrays are returned as Eigen matrices.
18+
*/
19+
class Model {
20+
public:
21+
/**
22+
* @param model The underlying franka::Model instance to wrap.
23+
*/
24+
explicit Model(franka::Model model);
25+
26+
Model(const Model &) = delete;
27+
Model &operator=(const Model &) = delete;
28+
29+
Model(Model &&other) noexcept;
30+
Model &operator=(Model &&other) noexcept;
31+
32+
/**
33+
* @brief Calculates the pose of a frame relative to the base frame.
34+
*
35+
* @param frame The frame whose pose should be returned.
36+
* @param state The current robot state.
37+
* @return The pose as an affine transformation matrix.
38+
*/
39+
[[nodiscard]] Affine pose(franka::Frame frame, const franka::RobotState &state) const;
40+
41+
/**
42+
* @brief Calculates the pose of a frame relative to the base frame.
43+
*
44+
* @param frame The frame whose pose should be returned.
45+
* @param q Robot joint angles [rad].
46+
* @param F_T_EE Transformation from flange to end-effector frame.
47+
* @param EE_T_K Transformation from end-effector frame to stiffness frame.
48+
* @return The pose as an affine transformation matrix.
49+
*/
50+
[[nodiscard]] Affine pose(franka::Frame frame, const Vector7d &q, const Affine &F_T_EE, const Affine &EE_T_K) const;
51+
52+
/**
53+
* @brief Calculates the body Jacobian in base frame.
54+
*
55+
* @param frame The frame for which the Jacobian is computed.
56+
* @param state The current robot state.
57+
* @return The 6x7 body Jacobian matrix.
58+
*/
59+
[[nodiscard]] Jacobian bodyJacobian(franka::Frame frame, const franka::RobotState &state) const;
60+
61+
/**
62+
* @brief Calculates the body Jacobian in base frame.
63+
*
64+
* @param frame The frame for which the Jacobian is computed.
65+
* @param q Robot joint angles [rad].
66+
* @param F_T_EE Transformation from flange to end-effector frame.
67+
* @param EE_T_K Transformation from end-effector frame to stiffness frame.
68+
* @return The 6x7 body Jacobian matrix.
69+
*/
70+
[[nodiscard]] Jacobian bodyJacobian(franka::Frame frame,
71+
const Vector7d &q,
72+
const Affine &F_T_EE,
73+
const Affine &EE_T_K) const;
74+
75+
/**
76+
* @brief Calculates the zero Jacobian in base frame.
77+
*
78+
* @param frame The frame for which the Jacobian is computed.
79+
* @param state The current robot state.
80+
* @return The 6x7 zero Jacobian matrix.
81+
*/
82+
[[nodiscard]] Jacobian zeroJacobian(franka::Frame frame, const franka::RobotState &state) const;
83+
84+
/**
85+
* @brief Calculates the zero Jacobian in base frame.
86+
*
87+
* @param frame The frame for which the Jacobian is computed.
88+
* @param q Robot joint angles [rad].
89+
* @param F_T_EE Transformation from flange to end-effector frame.
90+
* @param EE_T_K Transformation from end-effector frame to stiffness frame.
91+
* @return The 6x7 zero Jacobian matrix.
92+
*/
93+
[[nodiscard]] Jacobian zeroJacobian(franka::Frame frame,
94+
const Vector7d &q,
95+
const Affine &F_T_EE,
96+
const Affine &EE_T_K) const;
97+
98+
/**
99+
* @brief Calculates the mass matrix.
100+
*
101+
* @param state The current robot state.
102+
* @return The 7x7 mass matrix.
103+
*/
104+
[[nodiscard]] Eigen::Matrix<double, 7, 7> mass(const franka::RobotState &state) const;
105+
106+
/**
107+
* @brief Calculates the mass matrix.
108+
*
109+
* @param q Robot joint angles [rad].
110+
* @param I_total Combined load and robot inertia [kg·m²].
111+
* @param m_total Combined mass of robot and load [kg].
112+
* @param F_x_Ctotal Center of mass relative to flange frame [m].
113+
* @return The 7x7 mass matrix.
114+
*/
115+
[[nodiscard]] Eigen::Matrix<double, 7, 7> mass(const Vector7d &q,
116+
const Eigen::Matrix3d &I_total,
117+
double m_total,
118+
const Eigen::Vector3d &F_x_Ctotal) const;
119+
120+
/**
121+
* @brief Calculates the Coriolis force vector.
122+
*
123+
* @param state The current robot state.
124+
* @return The Coriolis vector [Nm].
125+
*/
126+
[[nodiscard]] Vector7d coriolis(const franka::RobotState &state) const;
127+
128+
/**
129+
* @brief Calculates the Coriolis force vector.
130+
*
131+
* @param q Robot joint angles [rad].
132+
* @param dq Robot joint velocities [rad/s].
133+
* @param I_total Combined load and robot inertia [kg·m²].
134+
* @param m_total Combined mass of robot and load [kg].
135+
* @param F_x_Ctotal Center of mass relative to flange frame [m].
136+
* @return The Coriolis vector [Nm].
137+
*/
138+
[[nodiscard]] Vector7d coriolis(const Vector7d &q,
139+
const Vector7d &dq,
140+
const Eigen::Matrix3d &I_total,
141+
double m_total,
142+
const Eigen::Vector3d &F_x_Ctotal) const;
143+
144+
/**
145+
* @brief Calculates the gravity vector.
146+
*
147+
* @param state The current robot state.
148+
* @param gravity_earth Gravity vector in base frame [m/s²].
149+
* @return The gravity vector [Nm].
150+
*/
151+
[[nodiscard]] Vector7d gravity(const franka::RobotState &state, const Eigen::Vector3d &gravity_earth) const;
152+
153+
/**
154+
* @brief Calculates the gravity vector using default gravity direction (0, 0, -9.81).
155+
*
156+
* @param state The current robot state.
157+
* @return The gravity vector [Nm].
158+
*/
159+
[[nodiscard]] Vector7d gravity(const franka::RobotState &state) const;
160+
161+
/**
162+
* @brief Calculates the gravity vector.
163+
*
164+
* @param q Robot joint angles [rad].
165+
* @param m_total Combined mass of robot and load [kg].
166+
* @param F_x_Ctotal Center of mass relative to flange frame [m].
167+
* @param gravity_earth Gravity vector in base frame [m/s²], default is (0, 0, -9.81).
168+
* @return The gravity vector [Nm].
169+
*/
170+
[[nodiscard]] Vector7d gravity(const Vector7d &q,
171+
double m_total,
172+
const Eigen::Vector3d &F_x_Ctotal,
173+
const Eigen::Vector3d &gravity_earth = {0., 0., -9.81}) const;
174+
175+
private:
176+
franka::Model model_;
177+
};
178+
179+
} // namespace franky

‎include/franky/motion/robot_state_estimator.hpp‎

Lines changed: 36 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,36 @@
1+
#pragma once
2+
3+
#include <franka/robot_state.h>
4+
#include <franka/model.h>
5+
6+
namespace franky {
7+
8+
/**
9+
* @brief TODO
10+
*
11+
* TODO
12+
*/
13+
class RobotStateEstimator {
14+
public:
15+
16+
/**
17+
* @param smoothing_factor TODO
18+
* @param desired_actual_blending_factor TODO
19+
*/
20+
explicit RobotStateEstimator(
21+
std::shared_ptr<franka::Model> model, double smoothing_factor = 0.9, double desired_actual_blending_factor = 0.5)
22+
: model_(model),
23+
smoothing_factor_(smoothing_factor),
24+
desired_actual_blending_factor_(desired_actual_blending_factor) {}
25+
26+
void update(const franka::RobotState &robot_state) {
27+
28+
}
29+
30+
private:
31+
const std::shared_ptr<franka::Model> model_;
32+
const double smoothing_factor_;
33+
const double desired_actual_blending_factor_;
34+
};
35+
36+
} // namespace franky

‎include/franky/robot.hpp‎

Lines changed: 12 additions & 34 deletions
Original file line numberDiff line numberDiff line change
@@ -16,7 +16,6 @@
1616
#include "franky/robot_pose.hpp"
1717
#include "franky/robot_velocity.hpp"
1818
#include "franky/cartesian_state.hpp"
19-
#include "franky/kinematics.hpp"
2019
#include "franky/motion/motion_generator.hpp"
2120
#include "franky/motion/motion.hpp"
2221
#include "franky/scope_guard.hpp"
@@ -25,6 +24,7 @@
2524
#include "franky/joint_state.hpp"
2625
#include "franky/dynamics_limit.hpp"
2726
#include "franky/util.hpp"
27+
#include "franky/model.hpp"
2828

2929
namespace franky {
3030

@@ -76,24 +76,6 @@ class Robot : public franka::Robot {
7676
franka::RealtimeConfig realtime_config{franka::RealtimeConfig::kEnforce};
7777
};
7878

79-
/**
80-
* @brief The kinematic chain of the robot.
81-
*/
82-
const KinematicChain<7> kinematics = KinematicChain<7>(
83-
{{
84-
{0.0, 0.333, 0.0},
85-
{-M_PI / 2, 0.0, 0.0},
86-
{M_PI / 2, 0.316, 0.0},
87-
{M_PI / 2, 0.0, 0.0825},
88-
{-M_PI / 2, 0.384, -0.0825},
89-
{M_PI / 2, 0.0, 0.0},
90-
{M_PI / 2, 0.0, 0.088}}},
91-
Affine().fromPositionOrientationScale(
92-
Eigen::Vector3d(0, 0, 0.107),
93-
Euler(M_PI / 4, 0, M_PI),
94-
Eigen::Matrix<double, 3, 1>::Ones())
95-
);
96-
9779
// clang-format off
9880
/**
9981
* @brief Translational velocity limit [m/s].
@@ -340,6 +322,15 @@ class Robot : public franka::Robot {
340322
*/
341323
[[nodiscard]] std::optional<ControlSignalType> current_control_signal_type();
342324

325+
/**
326+
* @brief The model of the robot.
327+
*
328+
* The model is loaded in the constructor, so calling this function does not incur any overhead.
329+
*/
330+
[[nodiscard]] inline std::shared_ptr<const Model> model() const {
331+
return model_;
332+
}
333+
343334
/**
344335
* @brief Wait for the current motion to finish. Throw any exceptions that occurred during the motion.
345336
*/
@@ -428,22 +419,9 @@ class Robot : public franka::Robot {
428419
}, async);
429420
}
430421

431-
/**
432-
* @brief Calculate the inverse kinematics for the given target pose.
433-
* @param target The target pose.
434-
* @param q0 The initial guess for the joint positions.
435-
* @return A vector containing the joint positions.
436-
*/
437-
// [[nodiscard]] static Vector7d inverseKinematics(const Affine &target, const Vector7d &q0);
438-
439-
/**
440-
* @brief Calculate the forward kinematics for the given joint positions.
441-
* @param q The joint positions.
442-
* @return The forward kinematics.
443-
*/
444-
[[nodiscard]] static Affine forwardKinematics(const Vector7d &q);
445-
446422
private:
423+
std::shared_ptr<const Model> model_;
424+
447425
template<typename ControlSignalType>
448426
using ControlFunc = std::function<ControlSignalType(const franka::RobotState &, franka::Duration)>;
449427
using MotionGeneratorVariant = std::variant<

‎include/franky/types.hpp‎

Lines changed: 1 addition & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -10,8 +10,7 @@ namespace franky {
1010

1111
using Vector6d = Eigen::Vector<double, 6>;
1212
using Vector7d = Eigen::Vector<double, 7>;
13-
14-
using Euler = Eigen::EulerAngles<double, Eigen::EulerSystemXYZ>;
13+
using Jacobian = Eigen::Matrix<double, 6, 7>;
1514

1615
using Affine = Eigen::Affine3d;
1716

0 commit comments

Comments
 (0)