#include <jrl/walkgen/pinocchiorobot.hh>
Public Member Functions | |
EIGEN_MAKE_ALIGNED_OPERATOR_NEW | PinocchioRobot () |
Constructor and Destructor. | |
~PinocchioRobot () | |
void | computeInverseDynamics () |
Functions computing kinematics and dynamics. | |
void | computeInverseDynamics (MAL_VECTOR_TYPE(double)&q, MAL_VECTOR_TYPE(double)&v, MAL_VECTOR_TYPE(double)&a) |
void | computeForwardKinematics () |
void | computeForwardKinematics (MAL_VECTOR_TYPE(double)&q) |
void | RPYToSpatialFreeFlyer (Eigen::Vector3d &rpy, Eigen::Vector3d &drpy, Eigen::Vector3d &ddrpy, Eigen::Quaterniond &quat, Eigen::Vector3d &omega, Eigen::Vector3d &domega) |
virtual bool | ComputeSpecializedInverseKinematics (const se3::JointIndex &jointRoot, const se3::JointIndex &jointEnd, const MAL_S4x4_MATRIX_TYPE(double)&jointRootPosition, const MAL_S4x4_MATRIX_TYPE(double)&jointEndPosition, MAL_VECTOR_TYPE(double)&q) |
ComputeSpecializedInverseKinematics : compute POSITION (not velocity) of the joints from end effector pose This is the implementation of the analitycal inverse kinematic extracted from the book of Kajita Authors Shuuji Kajita ; Hirohisa Hirukawa ; Kensuke Harada ; Kazuhito Yokoi ISBN 9782287877162 ; 9782287877155 It needs a specific distribution of the joint to work. | |
virtual bool | testInverseKinematics () |
testInverseKinematics : test if the robot has the good joint configuration to use the analitical inverse geometry to be overloaded if the user wants another inverse kinematics | |
virtual void | initializeInverseKinematics () |
initializeInverseKinematics : initialize the internal data for the inverse kinematic e.g. | |
std::vector< se3::JointIndex > | jointsBetween (se3::JointIndex first, se3::JointIndex second) |
tools : | |
std::vector< se3::JointIndex > | fromRootToIt (se3::JointIndex it) |
se3::Data * | Data () |
Getters /////. | |
se3::Data * | DataInInitialePose () |
se3::Model * | Model () |
PRFoot * | leftFoot () |
PRFoot * | rightFoot () |
se3::JointIndex | leftWrist () |
se3::JointIndex | rightWrist () |
se3::JointIndex | chest () |
se3::JointIndex | waist () |
double | mass () |
se3::JointModelVector & | getActuatedJoints () |
MAL_VECTOR_TYPE (double) currentConfiguration() | |
MAL_VECTOR_TYPE (double) currentVelocity() | |
MAL_VECTOR_TYPE (double) currentAcceleration() | |
unsigned | numberDof () |
void | zeroMomentumPoint (MAL_S3_VECTOR_TYPE(double)&zmp) |
void | positionCenterOfMass (MAL_S3_VECTOR_TYPE(double)&com) |
void | CenterOfMass (MAL_S3_VECTOR_TYPE(double)&com, MAL_S3_VECTOR_TYPE(double)&dcom, MAL_S3_VECTOR_TYPE(double)&ddcom) |
void | currentConfiguration (MAL_VECTOR_TYPE(double) conf) |
SETTERS /////. | |
void | currentVelocity (MAL_VECTOR_TYPE(double) vel) |
void | currentAcceleration (MAL_VECTOR_TYPE(double) acc) |
bool | isInitialized () |
Initialization functions //////////////////////. | |
bool | checkModel (se3::Model *robotModel) |
bool | initializeRobotModelAndData (se3::Model *robotModel, se3::Data *robotData) |
bool | initializeLeftFoot (PRFoot leftFoot) |
bool | initializeRightFoot (PRFoot rightFoot) |
EIGEN_MAKE_ALIGNED_OPERATOR_NEW PatternGeneratorJRL::PinocchioRobot::PinocchioRobot | ( | ) |
Constructor and Destructor.
PatternGeneratorJRL::PinocchioRobot::~PinocchioRobot | ( | ) |
void PatternGeneratorJRL::PinocchioRobot::CenterOfMass | ( | MAL_S3_VECTOR_TYPE(double)& | com, |
MAL_S3_VECTOR_TYPE(double)& | dcom, | ||
MAL_S3_VECTOR_TYPE(double)& | ddcom | ||
) | [inline] |
bool PatternGeneratorJRL::PinocchioRobot::checkModel | ( | se3::Model * | robotModel | ) |
se3::JointIndex PatternGeneratorJRL::PinocchioRobot::chest | ( | ) | [inline] |
void PatternGeneratorJRL::PinocchioRobot::computeForwardKinematics | ( | ) |
void PatternGeneratorJRL::PinocchioRobot::computeForwardKinematics | ( | MAL_VECTOR_TYPE(double)& | q | ) |
void PatternGeneratorJRL::PinocchioRobot::computeInverseDynamics | ( | ) |
Functions computing kinematics and dynamics.
Referenced by PatternGeneratorJRL::ZMPPreviewControlWithMultiBodyZMP::EvaluateMultiBodyZMP().
void PatternGeneratorJRL::PinocchioRobot::computeInverseDynamics | ( | MAL_VECTOR_TYPE(double)& | q, |
MAL_VECTOR_TYPE(double)& | v, | ||
MAL_VECTOR_TYPE(double)& | a | ||
) |
virtual bool PatternGeneratorJRL::PinocchioRobot::ComputeSpecializedInverseKinematics | ( | const se3::JointIndex & | jointRoot, |
const se3::JointIndex & | jointEnd, | ||
const MAL_S4x4_MATRIX_TYPE(double)& | jointRootPosition, | ||
const MAL_S4x4_MATRIX_TYPE(double)& | jointEndPosition, | ||
MAL_VECTOR_TYPE(double)& | q | ||
) | [virtual] |
ComputeSpecializedInverseKinematics : compute POSITION (not velocity) of the joints from end effector pose This is the implementation of the analitycal inverse kinematic extracted from the book of Kajita Authors Shuuji Kajita ; Hirohisa Hirukawa ; Kensuke Harada ; Kazuhito Yokoi ISBN 9782287877162 ; 9782287877155 It needs a specific distribution of the joint to work.
a test at the initialization of the class is [should be] done to check if everything is correct param root joint index, i.e. the waist or the shoulder param end joint index, i.e, the wrist or ankle indexes param root joint homogenous matrix position, param root joint homogenous matrix index, param 6D vector output, filled with zeros if the robot is not compatible
void PatternGeneratorJRL::PinocchioRobot::currentAcceleration | ( | MAL_VECTOR_TYPE(double) | acc | ) | [inline] |
void PatternGeneratorJRL::PinocchioRobot::currentConfiguration | ( | MAL_VECTOR_TYPE(double) | conf | ) | [inline] |
SETTERS /////.
Referenced by PatternGeneratorJRL::ZMPPreviewControlWithMultiBodyZMP::CallToComAndFootRealization(), PatternGeneratorJRL::ZMPPreviewControlWithMultiBodyZMP::EvaluateMultiBodyZMP(), PatternGeneratorJRL::ZMPPreviewControlWithMultiBodyZMP::Setup(), and PatternGeneratorJRL::ZMPPreviewControlWithMultiBodyZMP::SetupFirstPhase().
void PatternGeneratorJRL::PinocchioRobot::currentVelocity | ( | MAL_VECTOR_TYPE(double) | vel | ) | [inline] |
se3::Data* PatternGeneratorJRL::PinocchioRobot::Data | ( | ) | [inline] |
Getters /////.
se3::Data* PatternGeneratorJRL::PinocchioRobot::DataInInitialePose | ( | ) | [inline] |
std::vector<se3::JointIndex> PatternGeneratorJRL::PinocchioRobot::fromRootToIt | ( | se3::JointIndex | it | ) |
se3::JointModelVector& PatternGeneratorJRL::PinocchioRobot::getActuatedJoints | ( | ) | [inline] |
virtual void PatternGeneratorJRL::PinocchioRobot::initializeInverseKinematics | ( | ) | [virtual] |
initializeInverseKinematics : initialize the internal data for the inverse kinematic e.g.
the femur length
bool PatternGeneratorJRL::PinocchioRobot::initializeLeftFoot | ( | PRFoot | leftFoot | ) |
bool PatternGeneratorJRL::PinocchioRobot::initializeRightFoot | ( | PRFoot | rightFoot | ) |
bool PatternGeneratorJRL::PinocchioRobot::initializeRobotModelAndData | ( | se3::Model * | robotModel, |
se3::Data * | robotData | ||
) |
bool PatternGeneratorJRL::PinocchioRobot::isInitialized | ( | ) | [inline] |
Initialization functions //////////////////////.
std::vector<se3::JointIndex> PatternGeneratorJRL::PinocchioRobot::jointsBetween | ( | se3::JointIndex | first, |
se3::JointIndex | second | ||
) |
tools :
PRFoot* PatternGeneratorJRL::PinocchioRobot::leftFoot | ( | ) | [inline] |
se3::JointIndex PatternGeneratorJRL::PinocchioRobot::leftWrist | ( | ) | [inline] |
PatternGeneratorJRL::PinocchioRobot::MAL_VECTOR_TYPE | ( | double | ) | [inline] |
PatternGeneratorJRL::PinocchioRobot::MAL_VECTOR_TYPE | ( | double | ) | [inline] |
PatternGeneratorJRL::PinocchioRobot::MAL_VECTOR_TYPE | ( | double | ) | [inline] |
double PatternGeneratorJRL::PinocchioRobot::mass | ( | ) | [inline] |
se3::Model* PatternGeneratorJRL::PinocchioRobot::Model | ( | ) | [inline] |
unsigned PatternGeneratorJRL::PinocchioRobot::numberDof | ( | ) | [inline] |
void PatternGeneratorJRL::PinocchioRobot::positionCenterOfMass | ( | MAL_S3_VECTOR_TYPE(double)& | com | ) | [inline] |
PRFoot* PatternGeneratorJRL::PinocchioRobot::rightFoot | ( | ) | [inline] |
se3::JointIndex PatternGeneratorJRL::PinocchioRobot::rightWrist | ( | ) | [inline] |
void PatternGeneratorJRL::PinocchioRobot::RPYToSpatialFreeFlyer | ( | Eigen::Vector3d & | rpy, |
Eigen::Vector3d & | drpy, | ||
Eigen::Vector3d & | ddrpy, | ||
Eigen::Quaterniond & | quat, | ||
Eigen::Vector3d & | omega, | ||
Eigen::Vector3d & | domega | ||
) |
virtual bool PatternGeneratorJRL::PinocchioRobot::testInverseKinematics | ( | ) | [virtual] |
testInverseKinematics : test if the robot has the good joint configuration to use the analitical inverse geometry to be overloaded if the user wants another inverse kinematics
se3::JointIndex PatternGeneratorJRL::PinocchioRobot::waist | ( | ) | [inline] |
void PatternGeneratorJRL::PinocchioRobot::zeroMomentumPoint | ( | MAL_S3_VECTOR_TYPE(double)& | zmp | ) | [inline] |