robot_finger.hpp

Go to the documentation of this file.

#pragma once
#include <math.h>
#include "wiring.h"
#include "utils/angles.hpp"
#include <vector>
#include "utils/helpers.hpp"
#include "utils/matrix.hpp"
// #include "utils/vector.hpp"
 
static constexpr float link_0_length = 0.048;
static constexpr float link_1_length = 0.048;
static constexpr float link_2_length = 0.032;
 
static constexpr float transmission_0 = -1.f / 4.f;
static constexpr float transmission_1 = -1.f / 4.f;
static constexpr float transmission_2 = -3.f / 8.f;
 
// [  t0  0  ]
// [ -t0  t1 ]
// [  0   t2 ]
 
static constexpr Limits<float> joint_0_limits = convert_angular_units(
    {0.0, 90.0},
    AngleUnits::DEGREES,
    AngleUnits::RADIANS);
static constexpr Limits<float> joint_1_limits = convert_angular_units(
    {0.0, 90.0},
    AngleUnits::DEGREES,
    AngleUnits::RADIANS);
static constexpr Limits<float> joint_2_limits = convert_angular_units(
    {0.0, 90.0},
    AngleUnits::DEGREES,
    AngleUnits::RADIANS);
 
static constexpr Limits<float> joint_0_soft_limits = convert_angular_units(
    {15.0, 70.0},
    AngleUnits::DEGREES,
    AngleUnits::RADIANS);
 
static constexpr Limits<float> joint_1_soft_limits = convert_angular_units(
    {15.0, 70.0},
    AngleUnits::DEGREES,
    AngleUnits::RADIANS);
 
static constexpr Limits<float> joint_2_soft_limits = convert_angular_units(
    {15.0, 70.0},
    AngleUnits::DEGREES,
    AngleUnits::RADIANS);
 
static constexpr float motor_torque_limit = 5.0f; // N*m
 
static constexpr float joint_0_cali_offset = convert_angular_units(
    0.0, AngleUnits::DEGREES,
    AngleUnits::RADIANS);
 
static constexpr float joint_1_cali_offset = convert_angular_units(
    0.0, AngleUnits::DEGREES,
    AngleUnits::RADIANS);
 
static constexpr float joint_2_cali_offset = convert_angular_units(
    0.0, AngleUnits::DEGREES,
    AngleUnits::RADIANS);
 
static constexpr Limits<float> joint_0_vel_limits{-10.0, 10.0};
static constexpr Limits<float> joint_1_vel_limits{-10.0, 10.0};
 
std::vector<float> forward_kinematics(std::vector<float> joint_angles)
{
    const auto theta_0 = joint_angles.at(0);
    const auto theta_1 = joint_angles.at(1);
    const auto theta_2 = joint_angles.at(2);
 
    const float xe = link_0_length * cos(theta_0) + link_1_length * cos(theta_0 + theta_1) +
                     link_2_length * cos(theta_0 + theta_1 + theta_2);
    const float ye = 0.f;
    const float ze = -link_0_length * sin(theta_0) - link_1_length * sin(theta_0 + theta_1) -
                     link_2_length * sin(theta_0 + theta_1 + theta_2);
 
    return {xe, ye, ze};
}
 
std::vector<float> inverse_kinematics(std::vector<float> pos)
{
    const auto radius = pos.at(0) * pos.at(0) + pos.at(2) * pos.at(2);
    const auto theta_1 = (radius - link_0_length * link_0_length - link_1_length * link_1_length) /
                         (2 * link_0_length * link_1_length);
    const auto theta_0 = std::atan2(pos.at(2), pos.at(0)) -
                         std::atan2(
                             link_1_length * sin(theta_1),
                             link_0_length + link_1_length * cos(theta_1));
 
    return {-theta_0, -theta_1};
}
 
std::vector<float> motors_to_joints(std::vector<float> motor_vars)
{
    const auto phi_0 = motor_vars.at(0);
    const auto phi_1 = motor_vars.at(1);
    const auto theta_0 = phi_0 * transmission_0;
    const auto theta_1 = phi_1 * transmission_1 - theta_0;
    const auto theta_2 = 0.0f;
    return {theta_0, theta_1, theta_2};
}
 
std::vector<float> joints_to_motors(std::vector<float> joint_vars)
{
    const auto theta_0 = joint_vars.at(0);
    const auto theta_1 = joint_vars.at(1);
    // Theta 2 redudnat and discarded.
 
    const auto phi_0 = theta_0 / transmission_0;
    const auto phi_1 = theta_1 / transmission_1 + theta_0 / transmission_1;
    return {phi_0, phi_1};
}
 
std::vector<float> phi_to_theta(std::vector<float> motor_phis)
{
    auto joint_thetas = motors_to_joints(motor_phis);
    joint_thetas.at(0) += joint_0_cali_offset;
    joint_thetas.at(1) += joint_1_cali_offset;
    joint_thetas.at(2) += joint_2_cali_offset;
    return joint_thetas;
}
 
std::vector<float> theta_to_phi(std::vector<float> joint_thetas)
{
    joint_thetas.at(0) -= joint_0_cali_offset;
    joint_thetas.at(1) -= joint_1_cali_offset;
    joint_thetas.at(2) -= joint_2_cali_offset;
    const auto motor_phis = joints_to_motors(joint_thetas);
    return motor_phis;
}
 
std::vector<float> tau_to_torque(std::vector<float> joint_taus)
{
    const auto tau_0 = joint_taus.at(0);
    const auto tau_1 = joint_taus.at(1);
    const auto tau_2 = joint_taus.at(2);
 
    const auto torque_0 = tau_0 * transmission_0 - tau_1 * transmission_0;
    const auto torque_1 = tau_1 * transmission_1 + tau_2 * transmission_2;
 
    return {torque_0, torque_1};
}
 
std::vector<float> torque_to_tau(std::vector<float> motor_torques)
{
    const auto torque_0 = motor_torques.at(0);
    const auto torque_1 = motor_torques.at(1);
 
    const auto tau_0 = torque_0 / transmission_0 + torque_1 / transmission_1;
    const auto tau_1 = torque_1 / transmission_1;
 
    return {tau_0, tau_1};
}
 
Matrix spatial_jacobian(std::vector<float> joint_angles)
{
    const auto theta_0 = joint_angles.at(0);
    const auto theta_1 = joint_angles.at(1);
    const auto theta_2 = joint_angles.at(2);
 
    Matrix J_s(2, 3);
 
    J_s(
        0,
        0) = -link_0_length * sin(theta_0) - link_1_length * sin(theta_0 + theta_1) - link_2_length * sin(theta_0 + theta_1 + theta_2);
    J_s(0, 1) = -link_1_length * sin(theta_0 + theta_1) - link_2_length * sin(
                                                                              theta_0 + theta_1 + theta_2);
    J_s(0, 2) = -link_2_length * sin(theta_0 + theta_1 + theta_2);
    J_s(
        1,
        0) = -link_0_length * cos(theta_0) - link_1_length * cos(theta_0 + theta_1) - link_2_length * cos(theta_0 + theta_1 + theta_2);
    J_s(1, 1) = -link_1_length * cos(theta_0 + theta_1) - link_2_length * cos(
                                                                              theta_0 + theta_1 + theta_2);
    J_s(1, 2) = -link_2_length * cos(theta_0 + theta_1 + theta_2);
 
    return J_s;
}
 
std::vector<float> soft_limit_torques(std::vector<float> joint_thetas)
{
    static constexpr float discouraging_stiffness = 0.175f;
    float joint_0_d_t = -discouraging_stiffness * joint_0_soft_limits.over_limits(joint_thetas.at(0));
    float joint_1_d_t = -discouraging_stiffness * joint_1_soft_limits.over_limits(joint_thetas.at(1));
    // float joint_2_d_t = -discouraging_stiffness * joint_2_soft_limits.over_limits(joint_thetas.at(2));
 
    return {joint_0_d_t, joint_1_d_t, 0.0f};
}