robot_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"
 
static constexpr float link_0_length = 0.036;
static constexpr float link_1_length = 0.03085;
static constexpr float link_2_length = 0.05225;
 
static constexpr float transmission_0 = 6.f / 44.f;
static constexpr float transmission_1 = 1.f / 7.9375f;
static constexpr float transmission_2 = 1.f / 7.9375f;
 
// Transmission Matrix
// [ t_0, 0   ]
// [ 0  , t_1 ]
// [ 0  , t_2 ]
 
static constexpr Limits<float> joint_0_limits{0.0, 60.0};
 
static constexpr Limits<float> joint_1_limits{0.0, 40.0};
static constexpr Limits<float> joint_2_limits{0.0, 40.0};
 
static constexpr Limits<float> joint_0_soft_limits = convert_angular_units(
    {15.0, 50.0},
    AngleUnits::DEGREES,
    AngleUnits::RADIANS);
 
static constexpr Limits<float> joint_1_soft_limits = convert_angular_units(
    {15.0, 50.0},
    AngleUnits::DEGREES,
    AngleUnits::RADIANS);
 
static constexpr Limits<float> joint_2_soft_limits = convert_angular_units(
    {15.0, 50.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 auto x1 = link_0_length * sin(theta_0);
    const auto y1 = link_0_length * cos(theta_0);
    const auto z1 = 0;
 
    const auto x2 = x1 + link_1_length * cos(theta_1) * sin(theta_0);
    const auto y2 = y1 + link_1_length * cos(theta_1) * cos(theta_0);
    const auto z2 = z1 + link_1_length * sin(theta_1);
 
    const auto x3 = x2 + link_2_length * cos(theta_1 + theta_2) * sin(theta_0);
    const auto y3 = y2 + link_2_length * cos(theta_1 + theta_2) * cos(theta_0);
    const auto z3 = z2 + link_2_length * sin(theta_1 + theta_2);
 
    return {x3, y3, -z3};
}
 
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_1 * transmission_1;
    const auto theta_2 = phi_1 * transmission_2;
 
    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 is redundant and discarded.
 
    const auto phi_0 = theta_0 / transmission_0;
    const auto phi_1 = theta_1 / 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;
    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;
    // This is honestly just a guess Technically the ratio can be anything so long as it sums to one?
    const auto tau_1 = 0.5f * torque_1 / transmission_1;
    const auto tau_2 = 0.5f * torque_1 / transmission_2;
 
    return {tau_0, tau_1, tau_2};
}
 
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.0;
    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};
}