Quad-SDK
|
A lightweight library for quad kinematic functions. More...
#include <quad_kd.h>
Public Member Functions | |
QuadKD () | |
Constructor for QuadKD Class. | |
QuadKD (std::string ns) | |
Constructor for QuadKD Class. | |
void | initModel (std::string ns) |
Initialize model for the class. | |
Eigen::Matrix4d | createAffineMatrix (Eigen::Vector3d trans, Eigen::Vector3d rpy) const |
Create an Eigen Eigen::Matrix4d containing a homogeneous transform from a specified translation and a roll, pitch, and yaw vector. | |
Eigen::Matrix4d | createAffineMatrix (Eigen::Vector3d trans, Eigen::AngleAxisd rot) const |
Create an Eigen Eigen::Matrix4d containing a homogeneous transform from a specified translation and an AngleAxis object. | |
void | transformBodyToWorld (Eigen::Vector3d body_pos, Eigen::Vector3d body_rpy, Eigen::Matrix4d transform_body, Eigen::Matrix4d &transform_world) const |
Transform a transformation matrix from the body frame to the world frame. | |
void | transformWorldToBody (Eigen::Vector3d body_pos, Eigen::Vector3d body_rpy, Eigen::Matrix4d transform_world, Eigen::Matrix4d &transform_body) const |
Transform a transformation matrix from the world frame to the body frame. | |
void | bodyToFootFKBodyFrame (int leg_index, Eigen::Vector3d joint_state, Eigen::Matrix4d &g_body_foot) const |
Compute forward kinematics for a specified leg from the body COM. | |
void | bodyToFootFKBodyFrame (int leg_index, Eigen::Vector3d joint_state, Eigen::Vector3d &foot_pos_body) const |
Compute forward kinematics for a specified leg from the body COM. | |
void | worldToFootFKWorldFrame (int leg_index, Eigen::Vector3d body_pos, Eigen::Vector3d body_rpy, Eigen::Vector3d joint_state, Eigen::Matrix4d &g_world_foot) const |
Compute forward kinematics for a specified leg. | |
void | worldToFootFKWorldFrame (int leg_index, Eigen::Vector3d body_pos, Eigen::Vector3d body_rpy, Eigen::Vector3d joint_state, Eigen::Vector3d &foot_pos_world) const |
Compute forward kinematics for a specified leg. | |
void | worldToKneeFKWorldFrame (int leg_index, Eigen::Vector3d body_pos, Eigen::Vector3d body_rpy, Eigen::Vector3d joint_state, Eigen::Matrix4d &g_world_knee) const |
Compute forward kinematics for a specified leg. | |
void | worldToKneeFKWorldFrame (int leg_index, Eigen::Vector3d body_pos, Eigen::Vector3d body_rpy, Eigen::Vector3d joint_state, Eigen::Vector3d &knee_pos_world) const |
Compute forward kinematics for a specified leg. | |
bool | worldToFootIKWorldFrame (int leg_index, Eigen::Vector3d body_pos, Eigen::Vector3d body_rpy, Eigen::Vector3d foot_pos_world, Eigen::Vector3d &joint_state) const |
Compute inverse kinematics for a specified leg. | |
bool | legbaseToFootIKLegbaseFrame (int leg_index, Eigen::Vector3d foot_pos_legbase, Eigen::Vector3d &joint_state) const |
Compute inverse kinematics for a specified leg in the leg base frame. | |
double | getJointLowerLimit (int leg_index, int joint_index) const |
Get the lower joint limit of a particular joint. | |
double | getJointUpperLimit (int leg_index, int joint_index) const |
Get the upper joint limit of a particular joint. | |
double | getLinkLength (int leg_index, int link_index) const |
Get the upper joint limit of a particular joint. | |
void | worldToLegbaseFKWorldFrame (int leg_index, Eigen::Vector3d body_pos, Eigen::Vector3d body_rpy, Eigen::Matrix4d &g_world_legbase) const |
Get the transform from the world frame to the leg base. | |
void | worldToLegbaseFKWorldFrame (int leg_index, Eigen::Vector3d body_pos, Eigen::Vector3d body_rpy, Eigen::Vector3d &leg_base_pos_world) const |
Get the position of the leg base frame origin in the world frame. | |
void | worldToNominalHipFKWorldFrame (int leg_index, Eigen::Vector3d body_pos, Eigen::Vector3d body_rpy, Eigen::Vector3d &nominal_hip_pos_world) const |
Get the position of the nominal hip location in the world frame. | |
void | getJacobianGenCoord (const Eigen::VectorXd &state, Eigen::MatrixXd &jacobian) const |
Compute Jacobian for generalized coordinates. | |
void | getJacobianBodyAngVel (const Eigen::VectorXd &state, Eigen::MatrixXd &jacobian) const |
Compute Jacobian for angular velocity in body frame. | |
void | getJacobianWorldAngVel (const Eigen::VectorXd &state, Eigen::MatrixXd &jacobian) const |
Compute Jacobian for angular velocity in world frame. | |
void | getRotationMatrix (const Eigen::VectorXd &rpy, Eigen::Matrix3d &rot) const |
Compute rotation matrix given roll pitch and yaw. | |
void | computeInverseDynamics (const Eigen::VectorXd &state_pos, const Eigen::VectorXd &state_vel, const Eigen::VectorXd &foot_acc, const Eigen::VectorXd &grf, const std::vector< int > &contact_mode, Eigen::VectorXd &tau) const |
Compute inverse dynamics for swing leg. | |
bool | convertCentroidalToFullBody (const Eigen::VectorXd &body_state, const Eigen::VectorXd &foot_positions, const Eigen::VectorXd &foot_velocities, const Eigen::VectorXd &grfs, Eigen::VectorXd &joint_positions, Eigen::VectorXd &joint_velocities, Eigen::VectorXd &torques) |
Convert centroidal model states (foot coordinates and grfs) to full body (joints and torques) | |
bool | applyMotorModel (const Eigen::VectorXd &torques, Eigen::VectorXd &constrained_torques) |
Apply a uniform maximum torque to a given set of joint torques. | |
bool | applyMotorModel (const Eigen::VectorXd &torques, const Eigen::VectorXd &joint_velocities, Eigen::VectorXd &constrained_torques) |
Apply a linear motor model to a given set of joint torques and velocities. | |
bool | isValidFullState (const Eigen::VectorXd &body_state, const Eigen::VectorXd &joint_state, const Eigen::VectorXd &torques, const grid_map::GridMap &terrain, Eigen::VectorXd &state_violation, Eigen::VectorXd &control_violation) |
Check if state is valid. | |
bool | isValidCentroidalState (const Eigen::VectorXd &body_state, const Eigen::VectorXd &foot_positions, const Eigen::VectorXd &foot_velocities, const Eigen::VectorXd &grfs, const grid_map::GridMap &terrain, Eigen::VectorXd &joint_positions, Eigen::VectorXd &joint_velocities, Eigen::VectorXd &torques, Eigen::VectorXd &state_violation, Eigen::VectorXd &control_violation) |
Check if state is valid. | |
double | getGroundClearance (const Eigen::Vector3d &point, const grid_map::GridMap &terrain) |
Private Attributes | |
const int | num_feet_ = 4 |
Number of feet. | |
std::vector< double > | l0_vec_ |
Vector of the abad link lengths. | |
double | l1_ |
Upper link length. | |
double | l2_ |
Lower link length. | |
Eigen::Vector3d | abad_offset_ |
Abad offset from legbase. | |
Eigen::Vector3d | knee_offset_ |
Knee offset from hip. | |
Eigen::Vector3d | foot_offset_ |
Foot offset from knee. | |
std::vector< Eigen::Vector3d > | legbase_offsets_ |
Vector of legbase offsets. | |
std::vector< Eigen::Matrix4d > | g_body_legbases_ |
Vector of legbase offsets. | |
const double | joint_eps = 0.1 |
Epsilon offset for joint bounds. | |
std::vector< std::vector< double > > | joint_min_ |
Vector of the joint lower limits. | |
std::vector< std::vector< double > > | joint_max_ |
Vector of the joint upper limits. | |
RigidBodyDynamics::Model * | model_ |
std::vector< std::string > | body_name_list_ |
std::vector< unsigned int > | body_id_list_ |
std::vector< int > | leg_idx_list_ |
const double | abad_tau_max_ = 21 |
Abad max joint torque. | |
const double | hip_tau_max_ = 21 |
Hip max joint torque. | |
const double | knee_tau_max_ = 32 |
Knee max joint torque. | |
const Eigen::VectorXd | tau_max_ |
Vector of max torques. | |
const double | abad_vel_max_ = 37.7 |
Abad max joint velocity. | |
const double | hip_vel_max_ = 37.7 |
Hip max joint velocity. | |
const double | knee_vel_max_ = 25.1 |
Knee max joint velocity. | |
const Eigen::VectorXd | vel_max_ |
Vector of max velocities. | |
const Eigen::VectorXd | mm_slope_ = tau_max_.cwiseQuotient(vel_max_) |
A lightweight library for quad kinematic functions.
This library includes several functions and classes to aid in quad kinematic calculations. It relies on Eigen, as well as some MATLAB codegen for more complicated computations that would be a pain to write out by hand.
QuadKD::QuadKD | ( | std::string | ns | ) |
bool QuadKD::applyMotorModel | ( | const Eigen::VectorXd & | torques, |
const Eigen::VectorXd & | joint_velocities, | ||
Eigen::VectorXd & | constrained_torques | ||
) |
Apply a linear motor model to a given set of joint torques and velocities.
[in] | torques | Joint torques. in Nm |
[in] | joint_velocities | Velocities of each joint. in rad/s |
[in] | constrained_torques | Joint torques after applying motor model, in Nm |
bool QuadKD::applyMotorModel | ( | const Eigen::VectorXd & | torques, |
Eigen::VectorXd & | constrained_torques | ||
) |
Apply a uniform maximum torque to a given set of joint torques.
[in] | torques | Joint torques. in Nm |
[in] | constrained_torques | Joint torques after applying max, in Nm |
void QuadKD::bodyToFootFKBodyFrame | ( | int | leg_index, |
Eigen::Vector3d | joint_state, | ||
Eigen::Matrix4d & | g_body_foot | ||
) | const |
Compute forward kinematics for a specified leg from the body COM.
[in] | leg_index | Quad leg (0 = FL, 1 = BL, 2 = FR, 3 = BR) |
[in] | joint_state | Joint states for the specified leg (abad, hip, knee) |
[out] | g_body_foot | Transform of the specified foot in world frame |
void QuadKD::bodyToFootFKBodyFrame | ( | int | leg_index, |
Eigen::Vector3d | joint_state, | ||
Eigen::Vector3d & | foot_pos_body | ||
) | const |
Compute forward kinematics for a specified leg from the body COM.
[in] | leg_index | Quad leg (0 = FL, 1 = BL, 2 = FR, 3 = BR) |
[in] | joint_state | Joint states for the specified leg (abad, hip, knee) |
[out] | foot_pos_world | Position of the specified foot in world frame |
void QuadKD::computeInverseDynamics | ( | const Eigen::VectorXd & | state_pos, |
const Eigen::VectorXd & | state_vel, | ||
const Eigen::VectorXd & | foot_acc, | ||
const Eigen::VectorXd & | grf, | ||
const std::vector< int > & | contact_mode, | ||
Eigen::VectorXd & | tau | ||
) | const |
Compute inverse dynamics for swing leg.
[in] | state_pos | Position states |
[in] | state_vel | Velocity states |
[in] | foot_acc | Foot absolute acceleration in world frame |
[in] | grf | Ground reaction force |
[in] | contact_mode | Contact mode of the legs |
[out] | tau | Joint torques |
bool QuadKD::convertCentroidalToFullBody | ( | const Eigen::VectorXd & | body_state, |
const Eigen::VectorXd & | foot_positions, | ||
const Eigen::VectorXd & | foot_velocities, | ||
const Eigen::VectorXd & | grfs, | ||
Eigen::VectorXd & | joint_positions, | ||
Eigen::VectorXd & | joint_velocities, | ||
Eigen::VectorXd & | torques | ||
) |
Convert centroidal model states (foot coordinates and grfs) to full body (joints and torques)
[in] | body_state | Position states |
[in] | foot_positions | Foot positions in the world frame |
[in] | foot_velocities | Foot velocities in the world frame |
[in] | grfs | Ground reaction forces |
[out] | joint_positions | Joint positions |
[out] | joint_velocities | Joint velocities |
[out] | tau | Joint torques |
Eigen::Matrix4d QuadKD::createAffineMatrix | ( | Eigen::Vector3d | trans, |
Eigen::AngleAxisd | rot | ||
) | const |
Create an Eigen Eigen::Matrix4d containing a homogeneous transform from a specified translation and an AngleAxis object.
[in] | trans | Translation from input frame to output frame |
[in] | rot | Rotation from input frame to output frame as AngleAxis |
Eigen::Matrix4d QuadKD::createAffineMatrix | ( | Eigen::Vector3d | trans, |
Eigen::Vector3d | rpy | ||
) | const |
Create an Eigen Eigen::Matrix4d containing a homogeneous transform from a specified translation and a roll, pitch, and yaw vector.
[in] | trans | Translation from input frame to output frame |
[in] | rpy | Rotation from input frame to output frame as roll, pitch, yaw |
void QuadKD::getJacobianBodyAngVel | ( | const Eigen::VectorXd & | state, |
Eigen::MatrixXd & | jacobian | ||
) | const |
Compute Jacobian for angular velocity in body frame.
[in] | state | Joint and body states |
[out] | jacobian | Jacobian for angular velocity in body frame |
void QuadKD::getJacobianGenCoord | ( | const Eigen::VectorXd & | state, |
Eigen::MatrixXd & | jacobian | ||
) | const |
Compute Jacobian for generalized coordinates.
[in] | state | Joint and body states |
[out] | jacobian | Jacobian for generalized coordinates |
void QuadKD::getJacobianWorldAngVel | ( | const Eigen::VectorXd & | state, |
Eigen::MatrixXd & | jacobian | ||
) | const |
Compute Jacobian for angular velocity in world frame.
[in] | state | Joint and body states |
[out] | jacobian | Jacobian for angular velocity in world frame |
double QuadKD::getJointLowerLimit | ( | int | leg_index, |
int | joint_index | ||
) | const |
Get the lower joint limit of a particular joint.
[in] | leg_index | Quad leg (0 = FL, 1 = BL, 2 = FR, 3 = BR) |
[in] | joint_index | Index for joint (0 = abad, 1 = hip, 2 = knee) |
double QuadKD::getJointUpperLimit | ( | int | leg_index, |
int | joint_index | ||
) | const |
Get the upper joint limit of a particular joint.
[in] | leg_index | Quad leg (0 = FL, 1 = BL, 2 = FR, 3 = BR) |
[in] | joint_index | Index for joint (0 = abad, 1 = hip, 2 = knee) |
double QuadKD::getLinkLength | ( | int | leg_index, |
int | link_index | ||
) | const |
Get the upper joint limit of a particular joint.
[in] | leg_index | Quad leg (0 = FL, 1 = BL, 2 = FR, 3 = BR) |
[in] | link_index | Index for link (0 = abad, 1 = upper, 2 = lower) |
void QuadKD::getRotationMatrix | ( | const Eigen::VectorXd & | rpy, |
Eigen::Matrix3d & | rot | ||
) | const |
Compute rotation matrix given roll pitch and yaw.
[in] | rpy | Roll pitch and yaw |
[out] | rot | Rotation matrix |
void QuadKD::initModel | ( | std::string | ns | ) |
Initialize model for the class.
[in] | ns | Namespace |
bool QuadKD::isValidCentroidalState | ( | const Eigen::VectorXd & | body_state, |
const Eigen::VectorXd & | foot_positions, | ||
const Eigen::VectorXd & | foot_velocities, | ||
const Eigen::VectorXd & | grfs, | ||
const grid_map::GridMap & | terrain, | ||
Eigen::VectorXd & | joint_positions, | ||
Eigen::VectorXd & | joint_velocities, | ||
Eigen::VectorXd & | torques, | ||
Eigen::VectorXd & | state_violation, | ||
Eigen::VectorXd & | control_violation | ||
) |
Check if state is valid.
[in] | body_state | Robot body positions and velocities |
[in] | foot_positions | Foot positions |
[in] | foot_velocities | Foot velocities |
[in] | grfs | Ground reaction forces in the world frame |
[in] | terrain | Map of the terrain for collision checking |
bool QuadKD::isValidFullState | ( | const Eigen::VectorXd & | body_state, |
const Eigen::VectorXd & | joint_state, | ||
const Eigen::VectorXd & | torques, | ||
const grid_map::GridMap & | terrain, | ||
Eigen::VectorXd & | state_violation, | ||
Eigen::VectorXd & | control_violation | ||
) |
Check if state is valid.
[in] | body_state | Robot body positions and velocities |
[in] | joint_state | Joint positions and velocities |
[in] | torques | Joint torques |
[in] | terrain | Map of the terrain for collision checking |
bool QuadKD::legbaseToFootIKLegbaseFrame | ( | int | leg_index, |
Eigen::Vector3d | foot_pos_legbase, | ||
Eigen::Vector3d & | joint_state | ||
) | const |
Compute inverse kinematics for a specified leg in the leg base frame.
[in] | leg_index | Quad leg (0 = FL, 1 = BL, 2 = FR, 3 = BR) |
[in] | foot_pos_legbase | Position of the specified foot in leg base frame |
[out] | joint_state | Joint states for the specified leg (abad, hip, knee) |
void QuadKD::transformBodyToWorld | ( | Eigen::Vector3d | body_pos, |
Eigen::Vector3d | body_rpy, | ||
Eigen::Matrix4d | transform_body, | ||
Eigen::Matrix4d & | transform_world | ||
) | const |
Transform a transformation matrix from the body frame to the world frame.
[in] | body_pos | Position of center of body frame |
[in] | body_rpy | Orientation of body frame in roll, pitch, yaw |
[in] | transform_body | Specified transform in the body frame |
[out] | transform_world | Specified transform in the world frame |
void QuadKD::transformWorldToBody | ( | Eigen::Vector3d | body_pos, |
Eigen::Vector3d | body_rpy, | ||
Eigen::Matrix4d | transform_world, | ||
Eigen::Matrix4d & | transform_body | ||
) | const |
Transform a transformation matrix from the world frame to the body frame.
[in] | body_pos | Position of center of body frame |
[in] | body_rpy | Orientation of body frame in roll, pitch, yaw |
[in] | transform_world | Specified transform in the world frame |
[out] | transform_body | Specified transform in the body frame |
void QuadKD::worldToFootFKWorldFrame | ( | int | leg_index, |
Eigen::Vector3d | body_pos, | ||
Eigen::Vector3d | body_rpy, | ||
Eigen::Vector3d | joint_state, | ||
Eigen::Matrix4d & | g_world_foot | ||
) | const |
Compute forward kinematics for a specified leg.
[in] | leg_index | Quad leg (0 = FL, 1 = BL, 2 = FR, 3 = BR) |
[in] | body_pos | Position of center of body frame |
[in] | body_rpy | Orientation of body frame in roll, pitch, yaw |
[in] | joint_state | Joint states for the specified leg (abad, hip, knee) |
[out] | g_world_foot | Transform of the specified foot in world frame |
void QuadKD::worldToFootFKWorldFrame | ( | int | leg_index, |
Eigen::Vector3d | body_pos, | ||
Eigen::Vector3d | body_rpy, | ||
Eigen::Vector3d | joint_state, | ||
Eigen::Vector3d & | foot_pos_world | ||
) | const |
Compute forward kinematics for a specified leg.
[in] | leg_index | Quad leg (0 = FL, 1 = BL, 2 = FR, 3 = BR) |
[in] | body_pos | Position of center of body frame |
[in] | body_rpy | Orientation of body frame in roll, pitch, yaw |
[in] | joint_state | Joint states for the specified leg (abad, hip, knee) |
[out] | foot_pos_world | Position of the specified foot in world frame |
bool QuadKD::worldToFootIKWorldFrame | ( | int | leg_index, |
Eigen::Vector3d | body_pos, | ||
Eigen::Vector3d | body_rpy, | ||
Eigen::Vector3d | foot_pos_world, | ||
Eigen::Vector3d & | joint_state | ||
) | const |
Compute inverse kinematics for a specified leg.
[in] | leg_index | Quad leg (0 = FL, 1 = BL, 2 = FR, 3 = BR) |
[in] | body_pos | Position of center of body frame |
[in] | body_rpy | Orientation of body frame in roll, pitch, yaw |
[in] | foot_pos_world | Position of the specified foot in world frame |
[out] | joint_state | Joint states for the specified leg (abad, hip, knee) |
void QuadKD::worldToKneeFKWorldFrame | ( | int | leg_index, |
Eigen::Vector3d | body_pos, | ||
Eigen::Vector3d | body_rpy, | ||
Eigen::Vector3d | joint_state, | ||
Eigen::Matrix4d & | g_world_knee | ||
) | const |
Compute forward kinematics for a specified leg.
[in] | leg_index | Spirit leg (0 = FL, 1 = BL, 2 = FR, 3 = BR) |
[in] | body_pos | Position of center of body frame |
[in] | body_rpy | Orientation of body frame in roll, pitch, yaw |
[in] | joint_state | Joint states for the specified leg (abad, hip, knee) |
[out] | g_world_knee | Transform of the specified knee in world frame |
void QuadKD::worldToKneeFKWorldFrame | ( | int | leg_index, |
Eigen::Vector3d | body_pos, | ||
Eigen::Vector3d | body_rpy, | ||
Eigen::Vector3d | joint_state, | ||
Eigen::Vector3d & | knee_pos_world | ||
) | const |
Compute forward kinematics for a specified leg.
[in] | leg_index | Spirit leg (0 = FL, 1 = BL, 2 = FR, 3 = BR) |
[in] | body_pos | Position of center of body frame |
[in] | body_rpy | Orientation of body frame in roll, pitch, yaw |
[in] | joint_state | Joint states for the specified leg (abad, hip, knee) |
[out] | knee_pos_world | Position of the specified knee in world frame |
void QuadKD::worldToLegbaseFKWorldFrame | ( | int | leg_index, |
Eigen::Vector3d | body_pos, | ||
Eigen::Vector3d | body_rpy, | ||
Eigen::Matrix4d & | g_world_legbase | ||
) | const |
Get the transform from the world frame to the leg base.
[in] | leg_index | Quad leg (0 = FL, 1 = BL, 2 = FR, 3 = BR) |
[in] | body_pos | Position of center of body frame |
[in] | body_rpy | Orientation of body frame in roll, pitch, yaw |
[out] | g_world_legbase | Transformation matrix of world to leg base |
void QuadKD::worldToLegbaseFKWorldFrame | ( | int | leg_index, |
Eigen::Vector3d | body_pos, | ||
Eigen::Vector3d | body_rpy, | ||
Eigen::Vector3d & | leg_base_pos_world | ||
) | const |
Get the position of the leg base frame origin in the world frame.
[in] | leg_index | Quad leg (0 = FL, 1 = BL, 2 = FR, 3 = BR) |
[in] | body_pos | Position of center of body frame |
[in] | body_rpy | Orientation of body frame in roll, pitch, yaw |
[out] | leg_base_pos_world | Origin of leg base frame in world frame |
void QuadKD::worldToNominalHipFKWorldFrame | ( | int | leg_index, |
Eigen::Vector3d | body_pos, | ||
Eigen::Vector3d | body_rpy, | ||
Eigen::Vector3d & | nominal_hip_pos_world | ||
) | const |
Get the position of the nominal hip location in the world frame.
[in] | leg_index | Quad leg (0 = FL, 1 = BL, 2 = FR, 3 = BR) |
[in] | body_pos | Position of center of body frame |
[in] | body_rpy | Orientation of body frame in roll, pitch, yaw |
[out] | nominal_hip_pos_world | Location of nominal hip in world frame |
|
private |
Vector of max torques.
|
private |
Vector of max velocities.