|
| 1 | +/** |
| 2 | + * @file SerialLinkImpedance.h |
| 3 | + * @author Jon Woolfrey |
| 4 | + |
| 5 | + * @date August 2025 |
| 6 | + * @version 1.0 |
| 7 | + * |
| 8 | + * @brief Implements control laws for joint and Cartesian impedance control. |
| 9 | + * |
| 10 | + * @details This class implements dynamic-free joint and Cartesian impedance control. |
| 11 | + * It deliberately avoids the use of the robot inertia because of poor modeling in many |
| 12 | + * robot arms. The SerialLinkDynamics should be used if a more accurate model is available. |
| 13 | + * |
| 14 | + * @copyright (c) 2025 Jon Woolfrey |
| 15 | + * |
| 16 | + * @license OSCL - Free for non-commercial open-source use only. |
| 17 | + * Commercial use requires a license. |
| 18 | + * |
| 19 | + * @see https://github.com/Woolfrey/software_robot_library for more information. |
| 20 | + * @see https://github.com/Woolfrey/software_simple_qp for the optimisation algorithm used in the control. |
| 21 | + */ |
| 22 | + |
| 23 | +#ifndef SERIAL_LINK_IMPEDANCE_H |
| 24 | +#define SERIAL_LINK_IMPEDANCE_H |
| 25 | + |
| 26 | +#include <Control/SerialLinkBase.h> |
| 27 | + |
| 28 | +#include <memory> |
| 29 | + |
| 30 | +namespace RobotLibrary { namespace Control { |
| 31 | + |
| 32 | +/** |
| 33 | + * @brief Algorithms for velocity control of a serial link robot arm. |
| 34 | + */ |
| 35 | +class SerialLinkImpedance : public SerialLinkBase |
| 36 | +{ |
| 37 | + public: |
| 38 | + /** |
| 39 | + * @brief Constructor. |
| 40 | + * @param model A pointer to a KinematicTree object. |
| 41 | + * @param endpointName The name of the reference frame in the KinematicTree to be controlled. |
| 42 | + * @pram parameters Control gains and algorithm parameters. |
| 43 | + */ |
| 44 | + SerialLinkImpedance(std::shared_ptr<RobotLibrary::Model::KinematicTree> model, |
| 45 | + const std::string &endpointName, |
| 46 | + const RobotLibrary::Control::SerialLinkParameters ¶meters = SerialLinkParameters()); |
| 47 | + |
| 48 | + /** |
| 49 | + * @brief Solve the joint torques required to move the endpoint at a given acceleration. |
| 50 | + * @param endpointMotion A vector containing linear & angular acceleration. |
| 51 | + * @return A vector of joint torques (nx1) |
| 52 | + */ |
| 53 | + Eigen::VectorXd |
| 54 | + resolve_endpoint_motion(const Eigen::Vector<double,6> &endPointMotion); |
| 55 | + |
| 56 | + /** |
| 57 | + * @brief Compute the joint torque required to execute the specified endpoint twist (linear & angular velocity). |
| 58 | + This overrides the virtual method defined in the base class. |
| 59 | + * @param twist The desired linear & angular velocity, as a 6D vector. |
| 60 | + * @return The required joint torque. |
| 61 | + */ |
| 62 | + Eigen::VectorXd |
| 63 | + resolve_endpoint_twist(const Eigen::Vector<double,6> &twist); |
| 64 | + |
| 65 | + /** |
| 66 | + * @brief Solve the joint torques required to track a Cartesian trajectory. |
| 67 | + * @param desiredPose The desired position & orientation (pose) for the endpoint. |
| 68 | + * @param desiredVel The desired linear & angular velocity (twist) for the endpoint. |
| 69 | + * @param desiredAcc The desired linear & angular acceleration for the endpoint. |
| 70 | + * @return The joint torques (nx1) required to track the trajectory. |
| 71 | + */ |
| 72 | + Eigen::VectorXd |
| 73 | + track_endpoint_trajectory(const RobotLibrary::Model::Pose &desiredPose, |
| 74 | + const Eigen::Vector<double,6> &desiredVelocity, |
| 75 | + const Eigen::Vector<double,6> &desiredAcceleration); |
| 76 | + |
| 77 | + /** |
| 78 | + * @brief Solve the joint torques required to track a joint space trajectory. |
| 79 | + * @param desiredPos The desired joint position (nx1). |
| 80 | + * @param desiredVel The desired joint velocity (nx1). |
| 81 | + * @param desiredAcc The desired joint acceleration (nx1). |
| 82 | + * @return The control torque (nx1). |
| 83 | + */ |
| 84 | + Eigen::VectorXd |
| 85 | + track_joint_trajectory(const Eigen::VectorXd &desiredPosition, |
| 86 | + const Eigen::VectorXd &desiredVelocity, |
| 87 | + const Eigen::VectorXd &desiredAcceleration); |
| 88 | + |
| 89 | + /** |
| 90 | + * @brief Convert a desired endpoint wrench to the required joint torques. |
| 91 | + * @param wrench A 6x1 vector of forces (3x1) and moments (3x1). |
| 92 | + * @return The joint torque (nx1). |
| 93 | + */ |
| 94 | + Eigen::VectorXd |
| 95 | + map_wrench_to_torque(const Eigen::Vector<double,6> &wrench); |
| 96 | + |
| 97 | + /** |
| 98 | + * @brief Set the desired configuration for the redundancy. |
| 99 | + */ |
| 100 | + bool |
| 101 | + set_desired_configuration(const Eigen::VectorXd &configuration); |
| 102 | + |
| 103 | + protected: |
| 104 | + |
| 105 | + Eigen::VectorXd _desiredConfiguration; ///< For redundancy |
| 106 | + |
| 107 | + /** |
| 108 | + * @brief Compute the instantaneous limits on the joint acceleration control. |
| 109 | + * It computes the minimum between joint positions, speed, and acceleration limits. |
| 110 | + * @param jointNumber The joint for which to compute the limits. |
| 111 | + * @return The upper and lower joint speed at the current time. |
| 112 | + */ |
| 113 | + RobotLibrary::Model::Limits |
| 114 | + compute_control_limits(const unsigned int &jointNumber); |
| 115 | + |
| 116 | +}; // Semicolon needed after a class declaration |
| 117 | + |
| 118 | +} } // namespace |
| 119 | + |
| 120 | +#endif |
0 commit comments