![]() |
ORCA: Optimization-based framework for Robotic Control Applications
|
The robot model class allow to make kinematics and dynamics computations. More...
#include <RobotModel.h>
Public Types | |
using | Ptr = std::shared_ptr< RobotModel > |
![]() | |
using | Ptr = std::shared_ptr< ConfigurableOrcaObject > |
using | ParamMap = std::map< std::string, ParameterBase *> |
Public Member Functions | |
RobotModel (const std::string &name="") | |
The default constructor. More... | |
virtual | ~RobotModel () |
bool | loadModelFromFile (const std::string &modelFile) |
Load the model from an absolute urdf file path. More... | |
bool | loadModelFromString (const std::string &modelString) |
Load the model from a full urdf string (xacro not supported) More... | |
const std::string & | getUrdfUrl () const |
Get the file url if it was loaded from a file. Empty string otherwise. More... | |
const std::string & | getUrdfString () const |
Get the urdf expanded string used to build the model. More... | |
void | setRobotState (const Eigen::Matrix4d &world_H_base, const Eigen::VectorXd &jointPos, const Eigen::Matrix< double, 6, 1 > &baseVel, const Eigen::VectorXd &jointVel, const Eigen::Vector3d &gravity) |
Set the complete robot state. More... | |
void | setRobotState (const Eigen::VectorXd &jointPos, const Eigen::VectorXd &jointVel, const Eigen::Vector3d &global_gravity_vector) |
Set the partial robot state. More... | |
void | setRobotState (const Eigen::VectorXd &jointPos, const Eigen::VectorXd &jointVel) |
Set the partial robot state. Useful for fixed based robots. More... | |
const std::string & | getName () const |
Returns the name of the robot, either provided via constructor or extracted from the urdf is no name is provided. More... | |
void | print () const |
Print information about the model to cout. More... | |
void | setBaseFrame (const std::string &fixed_base_or_free_floating_frame) |
Set the base frame / the free-floating frame that attaches the robot to the world frame For a humanoid it would be generally somewhere in the hips. For a fixed based robot it would be the frame between the robot and the table. This frame is also the frame of reference for the jacobians, transforms etc when refFrameName is not specified. More... | |
const std::string & | getBaseFrame () const |
Returns the base/free-floating frame. More... | |
void | setGravity (const Eigen::Vector3d &global_gravity_vector) |
Return the global gravity vector. More... | |
unsigned int | getNrOfDegreesOfFreedom () const |
Return the number of actuated joints. More... | |
unsigned int | getConfigurationSpaceDimension () const |
Returns the number of DOF + Free floating DOF (6) More... | |
bool | frameExists (const std::string &frame_name) const |
Returns true is the frame exists on the model. More... | |
const Eigen::Matrix4d & | getRelativeTransform (const std::string &refFrameName, const std::string &frameName) |
Get the transform (or pose) between a frame to another. More... | |
const Eigen::Matrix4d & | getTransform (const std::string &frameName) |
Get the transform of a frame w.r.t the base frame. More... | |
bool | addAdditionalFrameToLink (const std::string &linkName, const std::string &frameName, const Eigen::Matrix4d &link_H_frame) |
Add an extra frame to a link. This re-creates the whole model, so it should not be used on the control loop. More... | |
const Eigen::Matrix< double, 6, 1 > & | getFrameVel (const std::string &frameName) |
const Eigen::Matrix< double, 6, 1 > & | getFrameBiasAcc (const std::string &frameName) |
const Eigen::MatrixXd & | getFreeFloatingMassMatrix () |
const Eigen::MatrixXd & | getMassMatrix () |
const Eigen::VectorXd & | getJointPos () const |
const Eigen::VectorXd & | getJointVel () const |
const Eigen::VectorXd & | getMinJointPos () |
const Eigen::VectorXd & | getMaxJointPos () |
const Eigen::MatrixXd & | getJacobian (const std::string &frameName) |
const Eigen::MatrixXd & | getRelativeJacobian (const std::string &refFrameName, const std::string &frameName) |
const Eigen::MatrixXd & | getFrameFreeFloatingJacobian (const std::string &frameName) |
const Eigen::VectorXd & | generalizedBiasForces () |
const Eigen::VectorXd & | getJointGravityTorques () |
const Eigen::VectorXd & | getJointCoriolisTorques () |
const Eigen::VectorXd & | getJointGravityAndCoriolisTorques () |
unsigned int | getNrOfJoints () const |
std::string | getJointName (unsigned int idx) const |
unsigned int | getJointIndex (const std::string &jointName) const |
const std::vector< std::string > & | getLinkNames () const |
const std::vector< std::string > & | getFrameNames () const |
const std::vector< std::string > & | getJointNames () const |
bool | isInitialized () const |
void | onRobotInitializedCallback (std::function< void(void)> cb) |
![]() | |
ConfigurableOrcaObject (const std::string &config_name) | |
virtual | ~ConfigurableOrcaObject () |
template<class T > | |
void | addParameter (const std::string ¶m_name, Parameter< T > *param, ParamPolicy policy=ParamPolicy::Required, std::function< void()> on_loading_success=0) |
Returns true if all params added with have been set. More... | |
template<class T > | |
void | addParameter (const std::string ¶m_name, T ¶m, ParamPolicy policy=ParamPolicy::Required, std::function< void()> on_loading_success=0) |
ParameterBase * | getParameter (const std::string ¶m_name) |
Returns a param via its name. More... | |
void | printParameters () const |
Print all parameters to std::cout. More... | |
bool | configureFromFile (const std::string &yaml_url) |
Configure the task from YAML/JSON file. It must contain all the required parameters. More... | |
bool | configureFromString (const std::string &yaml_str) |
Configure the task from YAML/JSON string. It must contain all the required parameters. More... | |
bool | isConfigured () const |
const ParamMap & | getParameters () const |
void | onConfigureSuccess (std::function< void()> f) |
![]() | |
OrcaObject (const std::string &name) | |
virtual | ~OrcaObject () |
const std::string & | getName () const |
void | setName (const std::string &name) |
Protected Types | |
enum | RobotModelImplType { iDynTree, KDL } |
Protected Member Functions | |
bool | loadFromParameters () |
Protected Attributes | |
RobotModelImplType | robot_kinematics_type_ = iDynTree |
std::function< void(void)> | robot_initialized_cb_ |
bool | is_initialized_ = false |
common::Parameter< std::string > | urdf_url_ |
common::Parameter< std::string > | urdf_str_ |
common::Parameter< std::string > | base_frame_ |
common::Parameter< Eigen::Vector3d > | gravity_ |
common::Parameter< Eigen::VectorXd > | home_joint_positions_ |
common::Parameter< std::string > | name_ |
The robot model class allow to make kinematics and dynamics computations.
using orca::robot::RobotModel::Ptr = std::shared_ptr<RobotModel> |
|
protected |
RobotModel::RobotModel | ( | const std::string & | name = "" | ) |
The default constructor.
name | You can explicitely give it a name, otherwise it will be extracted from the urdf |
|
virtual |
bool RobotModel::addAdditionalFrameToLink | ( | const std::string & | linkName, |
const std::string & | frameName, | ||
const Eigen::Matrix4d & | link_H_frame | ||
) |
Add an extra frame to a link. This re-creates the whole model, so it should not be used on the control loop.
linkName | The link from which you want to attach a new frame |
frameName | The new frame |
link_H_frame | The transform between the link to the frame |
bool RobotModel::frameExists | ( | const std::string & | frame_name | ) | const |
Returns true is the frame exists on the model.
frame_name | The test frame |
const Eigen::VectorXd & RobotModel::generalizedBiasForces | ( | ) |
const std::string & RobotModel::getBaseFrame | ( | ) | const |
Returns the base/free-floating frame.
unsigned int RobotModel::getConfigurationSpaceDimension | ( | ) | const |
Returns the number of DOF + Free floating DOF (6)
const Eigen::Matrix< double, 6, 1 > & RobotModel::getFrameBiasAcc | ( | const std::string & | frameName | ) |
const Eigen::MatrixXd & RobotModel::getFrameFreeFloatingJacobian | ( | const std::string & | frameName | ) |
const std::vector< std::string > & RobotModel::getFrameNames | ( | ) | const |
const Eigen::Matrix< double, 6, 1 > & RobotModel::getFrameVel | ( | const std::string & | frameName | ) |
const Eigen::MatrixXd & RobotModel::getFreeFloatingMassMatrix | ( | ) |
const Eigen::MatrixXd & RobotModel::getJacobian | ( | const std::string & | frameName | ) |
const Eigen::VectorXd & RobotModel::getJointCoriolisTorques | ( | ) |
const Eigen::VectorXd & RobotModel::getJointGravityAndCoriolisTorques | ( | ) |
const Eigen::VectorXd & RobotModel::getJointGravityTorques | ( | ) |
unsigned int RobotModel::getJointIndex | ( | const std::string & | jointName | ) | const |
std::string RobotModel::getJointName | ( | unsigned int | idx | ) | const |
const std::vector< std::string > & RobotModel::getJointNames | ( | ) | const |
const Eigen::VectorXd & RobotModel::getJointPos | ( | ) | const |
const Eigen::VectorXd & RobotModel::getJointVel | ( | ) | const |
const std::vector< std::string > & RobotModel::getLinkNames | ( | ) | const |
const Eigen::MatrixXd & RobotModel::getMassMatrix | ( | ) |
const Eigen::VectorXd & RobotModel::getMaxJointPos | ( | ) |
const Eigen::VectorXd & RobotModel::getMinJointPos | ( | ) |
const std::string & RobotModel::getName | ( | ) | const |
Returns the name of the robot, either provided via constructor or extracted from the urdf is no name is provided.
unsigned int RobotModel::getNrOfDegreesOfFreedom | ( | ) | const |
Return the number of actuated joints.
unsigned int RobotModel::getNrOfJoints | ( | ) | const |
const Eigen::MatrixXd & RobotModel::getRelativeJacobian | ( | const std::string & | refFrameName, |
const std::string & | frameName | ||
) |
const Eigen::Matrix4d & RobotModel::getRelativeTransform | ( | const std::string & | refFrameName, |
const std::string & | frameName | ||
) |
Get the transform (or pose) between a frame to another.
refFrameName | The reference frame |
frameName | The frame you want to compute w.r.t the reference frame |
const Eigen::Matrix4d & RobotModel::getTransform | ( | const std::string & | frameName | ) |
Get the transform of a frame w.r.t the base frame.
frameName | The frame you want to compute w.r.t the base frame |
const std::string & RobotModel::getUrdfString | ( | ) | const |
Get the urdf expanded string used to build the model.
const std::string & RobotModel::getUrdfUrl | ( | ) | const |
Get the file url if it was loaded from a file. Empty string otherwise.
bool RobotModel::isInitialized | ( | ) | const |
|
protected |
bool RobotModel::loadModelFromFile | ( | const std::string & | modelFile | ) |
Load the model from an absolute urdf file path.
modelFile | The absolute file path |
bool RobotModel::loadModelFromString | ( | const std::string & | modelString | ) |
Load the model from a full urdf string (xacro not supported)
modelString | The expanded urdf string |
void RobotModel::onRobotInitializedCallback | ( | std::function< void(void)> | cb | ) |
void RobotModel::print | ( | ) | const |
Print information about the model to cout.
void RobotModel::setBaseFrame | ( | const std::string & | fixed_base_or_free_floating_frame | ) |
Set the base frame / the free-floating frame that attaches the robot to the world frame For a humanoid it would be generally somewhere in the hips. For a fixed based robot it would be the frame between the robot and the table. This frame is also the frame of reference for the jacobians, transforms etc when refFrameName is not specified.
fixed_base_or_free_floating_frame | p_fixed_base_or_free_floating_frame:... |
void RobotModel::setGravity | ( | const Eigen::Vector3d & | global_gravity_vector | ) |
Return the global gravity vector.
global_gravity_vector | The global gravity vector (usually 0,0,-9.81) |
void RobotModel::setRobotState | ( | const Eigen::Matrix4d & | world_H_base, |
const Eigen::VectorXd & | jointPos, | ||
const Eigen::Matrix< double, 6, 1 > & | baseVel, | ||
const Eigen::VectorXd & | jointVel, | ||
const Eigen::Vector3d & | gravity | ||
) |
Set the complete robot state.
world_H_base | The pose of the robot w.r.t the world frame (identity for fixed-based robots) |
jointPos | The current joint positions |
baseVel | The base twist (zero for fixed-base robots) |
jointVel | The current joint velocities |
gravity | The world gravity |
void RobotModel::setRobotState | ( | const Eigen::VectorXd & | jointPos, |
const Eigen::VectorXd & | jointVel, | ||
const Eigen::Vector3d & | global_gravity_vector | ||
) |
Set the partial robot state.
jointPos | The current joint positions |
jointVel | THe current joint velocities |
global_gravity_vector | the world gravity |
void RobotModel::setRobotState | ( | const Eigen::VectorXd & | jointPos, |
const Eigen::VectorXd & | jointVel | ||
) |
Set the partial robot state. Useful for fixed based robots.
jointPos | The current joint positions |
jointVel | The current joint velocities |
|
protected |
|
protected |
|
protected |
|
protected |
|
protected |
|
protected |
|
protected |
|
protected |
|
protected |