haptic_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.0726;
static constexpr float link_1_length = 0.0942;
 
static constexpr float operator_x_offset = 0.0f;
static constexpr float operator_y_offset = 0.0f;
static constexpr float operator_z_offset = -0.05f;
 
static constexpr float fk_joint_0_offset = convert_angular_units(
    21.532f, AngleUnits::DEGREES,
    AngleUnits::RADIANS);
static constexpr float fk_joint_1_offset = convert_angular_units(
    -42.44f, AngleUnits::DEGREES,
    AngleUnits::RADIANS);
 
static constexpr float transmission_0 = -1.f / 7.f;
static constexpr float transmission_1 = -1.f / 3.f;
 
static constexpr Limits<float> joint_0_limits{-18.719, 107.719};
 
static constexpr Limits<float> joint_1_limits{38, 90.0};
 
static constexpr Limits<float> joint_0_soft_limits = convert_angular_units(
    {0.0, 90.0},
    AngleUnits::DEGREES,
    AngleUnits::RADIANS);
 
static constexpr Limits<float> joint_1_soft_limits = convert_angular_units(
    {0.0, 80.0},
    AngleUnits::DEGREES,
    AngleUnits::RADIANS);
 
static constexpr float joint_0_cali_offset = convert_angular_units(
    -18.719, AngleUnits::DEGREES,
    AngleUnits::RADIANS);
 
static constexpr float joint_1_cali_offset = convert_angular_units(
    38.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};
 
static constexpr float motor_torque_limit = 5.0f; // N*m
 
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_0_hat = theta_0 - fk_joint_0_offset;
    const auto theta_1_hat = theta_1 - fk_joint_1_offset;
 
    const float xe = link_0_length * cos(theta_0_hat) + link_1_length * cos(theta_0_hat + theta_1_hat) - operator_x_offset;
    const float ye = 0.f - operator_y_offset;
    const float ze = -link_0_length * sin(theta_0_hat) - link_1_length * sin(theta_0_hat + theta_1_hat) - operator_z_offset;
 
    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 - fk_joint_0_offset, -theta_1 - fk_joint_1_offset};
}
 
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;
    return {theta_0, theta_1};
}
 
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);
 
    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;
    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;
    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 torque_0 = tau_0 * transmission_0 - tau_1 * transmission_0;
    const auto torque_1 = tau_1 * transmission_1;
 
    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_0_hat = theta_0 - fk_joint_0_offset;
    const auto theta_1_hat = theta_1 - fk_joint_1_offset;
 
    Matrix J_s{2, 2};
    J_s(0, 0) = -link_0_length * sin(theta_0_hat) - link_1_length * sin(theta_0_hat + theta_1_hat);
    J_s(0, 1) = -link_1_length * sin(theta_0_hat + theta_1_hat);
    J_s(1, 0) = -link_0_length * cos(theta_0_hat) - link_1_length * cos(theta_0_hat + theta_1_hat);
    J_s(1, 1) = -link_1_length * cos(theta_0_hat + theta_1_hat);
 
    return J_s;
}
 
std::vector<float> soft_limit_torques(std::vector<float> joint_thetas)
{
    static constexpr float discouraging_stiffness = 0.0f;
    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 {0.0, 0.0};
}