haptic_thumb.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"
 
// mcp_pulley = 1.17
// idler = 0.575
// mcp_motor = 0.333
// cmc_plate = 2.322
// cmc_motor = 0.472
 
// t_0 = cmc_motor / cmc_plate = 0.2032
// t_1 = mcp_motor / idler = 0.5791
// t_2 = mcp_motor / idler * idler/ mcp_pulley = 0.2846
 
static constexpr float link_0_length = 0.047625;
static constexpr float link_0_height = 0.0392455;
static constexpr float link_1_length = 0.060832;
static constexpr float link_1_height = 0.039189;
 
static constexpr float transmission_0 = 0.2032;
static constexpr float transmission_1 = -0.5791 * transmission_0;
static constexpr float transmission_2 = -0.2846;
 
static constexpr float operator_origin_x = 0.0;
static constexpr float operator_origin_y = 0.0;
static constexpr float operator_origin_z = -0.05;
 
// Transmission Matrix
// [ t_0, 0   ]
// [ ?? , t_1 ]
 
static constexpr Limits<float> joint_0_limits{0.0, 70.0};
static constexpr Limits<float> joint_1_limits{0.0, 80.0};
 
static constexpr Limits<float> joint_0_soft_limits = convert_angular_units(
    {0.0, 70.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(
    0.0, AngleUnits::DEGREES,
    AngleUnits::RADIANS);
static constexpr float joint_1_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};
 
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 x = operator_origin_x +
                   (link_0_length + link_1_length * cos(theta_1) - link_1_height * sin(theta_1)) * cos(theta_0);
    const auto y = operator_origin_y +
                   (link_0_length + link_1_length * cos(theta_1) - link_1_height * sin(theta_1)) * sin(theta_0);
    const auto z = operator_origin_z + link_0_height - link_1_length * sin(theta_1) - link_1_height * cos(theta_1);
 
    return {x, y, z};
}
 
// /// @brief Performs inverse kinematics for the robot thumb.
// /// @note Enforces joint coupling.
// /// @param pos {x, y, z} in meters of the end effector.
// /// @returns {theta_0, theta_1, theta_2} of the joints in radians required to move to the location.
// std::vector<float> inverse_kinematics(std::vector<float> pos)
// {
//   const auto x = pos.at(0);
//   const auto y = pos.at(1);
//   const auto z = pos.at(2);
 
//   const auto theta_0 = std::atan2(y, x);
 
//   const auto radius = x * x + y * y + z * z;
//   const auto theta_1 = std::acos(radius - link_1_length * link_1_length - link_2_length * link_2_length) /
//      ( 2 * link_1_length * link_2_length);
 
//   return {theta_0, theta_1, 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_0 * transmission_1 + phi_1 * transmission_2;
 
    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 = -transmission_1 / (transmission_0 * transmission_2) * theta_0 + theta_1 / transmission_2;
    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;
    return joints_to_motors(joint_thetas);
}
 
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_1;
    const auto torque_1 = tau_1 * 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 / (transmission_0 * transmission_2);
    const auto tau_1 = torque_1 / transmission_2;
 
    return {tau_0, tau_1};
}
 
Matrix spatial_jacobian(std::vector<float> joint_thetas)
{
    return Matrix();
}
 
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));
 
    return {0.0, 0.0};
}