Converts a hyq foot position to joint angles. More...
#include <hyqleg_inverse_kinematics.h>
Public Types | |
| enum | KneeBend { Forward, Backward } |
Public Member Functions | |
| void | EnforceLimits (double &q, HyqJointID joint) const |
| Restricts the joint angles to lie inside the feasible range. | |
| Vector3d | GetJointAngles (const Vector3d &ee_pos_H, KneeBend bend=Forward) const |
| Returns the joint angles to reach a Cartesian foot position. | |
| HyqlegInverseKinematics () | |
| Default c'tor initializing leg lengths with standard values. | |
| virtual | ~HyqlegInverseKinematics () |
Private Attributes | |
| Vector3d | hfe_to_haa_z = Vector3d(0.0, 0.0, 0.08) |
| double | length_shank = 0.33 |
| double | length_thigh = 0.35 |
Converts a hyq foot position to joint angles.
Definition at line 42 of file hyqleg_inverse_kinematics.h.
Definition at line 45 of file hyqleg_inverse_kinematics.h.
Default c'tor initializing leg lengths with standard values.
| virtual xpp::HyqlegInverseKinematics::~HyqlegInverseKinematics | ( | ) | [virtual] |
| void xpp::HyqlegInverseKinematics::EnforceLimits | ( | double & | q, |
| HyqJointID | joint | ||
| ) | const |
Restricts the joint angles to lie inside the feasible range.
| q[in/out] | Current joint angle that is adapted if it exceeds the specified range. |
| joint | Which joint (HAA, HFE, KFE) this value represents. |
Definition at line 119 of file hyqleg_inverse_kinematics.cc.
| HyqlegInverseKinematics::Vector3d xpp::HyqlegInverseKinematics::GetJointAngles | ( | const Vector3d & | ee_pos_H, |
| KneeBend | bend = Forward |
||
| ) | const |
Returns the joint angles to reach a Cartesian foot position.
| ee_pos_H | Foot position xyz expressed in the frame attached at the hip-aa (H). |
Definition at line 42 of file hyqleg_inverse_kinematics.cc.
Vector3d xpp::HyqlegInverseKinematics::hfe_to_haa_z = Vector3d(0.0, 0.0, 0.08) [private] |
Definition at line 69 of file hyqleg_inverse_kinematics.h.
double xpp::HyqlegInverseKinematics::length_shank = 0.33 [private] |
Definition at line 71 of file hyqleg_inverse_kinematics.h.
double xpp::HyqlegInverseKinematics::length_thigh = 0.35 [private] |
Definition at line 70 of file hyqleg_inverse_kinematics.h.