wbc
|
Interface for all robot models. This has to provide all kinematics and dynamics information that is required for WBC. Conventions and usage: More...
#include <RobotModel.hpp>
Classes | |
struct | Matrix |
struct | Pose |
struct | RigidBodyState |
struct | SpatialAcceleration |
struct | Twist |
struct | Vector |
Public Member Functions | |
RobotModel () | |
virtual | ~RobotModel () |
virtual bool | configure (const RobotModelConfig &cfg)=0 |
This will read the robot model from the given URDF file and initialize all members accordingly. | |
void | update (const Eigen::VectorXd &joint_positions, const Eigen::VectorXd &joint_velocities) |
Update kinematics/dynamics for fixed base robots. Joint acceleration will be assumed zero. | |
virtual void | update (const Eigen::VectorXd &joint_positions, const Eigen::VectorXd &joint_velocities, const Eigen::VectorXd &joint_accelerations) |
Update kinematics/dynamics for fixed base robots. | |
void | update (const Eigen::VectorXd &joint_positions, const Eigen::VectorXd &joint_velocities, const types::Pose &fb_pose, const types::Twist &fb_twist) |
Update kinematics/dynamics for floating base robots. Joint and spatial acceleration will be assumed zero. | |
virtual void | update (const Eigen::VectorXd &joint_positions, const Eigen::VectorXd &joint_velocities, const Eigen::VectorXd &joint_accelerations, const types::Pose &fb_pose, const types::Twist &fb_twist, const types::SpatialAcceleration &fb_acc)=0 |
Update kinematics/dynamics. | |
const types::JointState & | jointState () |
virtual const types::Pose & | pose (const std::string &frame_id)=0 |
virtual const types::Twist & | twist (const std::string &frame_id)=0 |
virtual const types::SpatialAcceleration & | acceleration (const std::string &frame_id)=0 |
virtual const Eigen::MatrixXd & | spaceJacobian (const std::string &frame_id)=0 |
Returns the Space Jacobian for the given frame. The order of the Jacobian's columns will be the same as in the model (alphabetic). Note that the linear part of the jacobian will be aligned to world frame (floating base robot) or robot root link (fixed base), and the angular part will be in body coordinates. This is refered to as "hybrid" representation. | |
virtual const Eigen::MatrixXd & | bodyJacobian (const std::string &frame_id)=0 |
Returns the Body Jacobian for the given frame. The order of the Jacobian's columns will be the same as in the model (alphabetic). | |
virtual const Eigen::MatrixXd & | comJacobian ()=0 |
Returns the CoM Jacobian for the robot, which maps the robot joint velocities to linear spatial velocities of the CoM expressed in world frame (floating base robot) or robot root link (fixed base). The order of the columns will be the same as the configured joint order of the robot. | |
virtual const types::SpatialAcceleration & | spatialAccelerationBias (const std::string &frame_id)=0 |
Returns the spatial acceleration bias, i.e. the term Jdot*qdot. The linear part of the acceleration will be aligned to world frame (floating base robot) or robot root link (fixed base), and the angular part will be in body coordinates. This is refered to as "hybrid" representation. | |
virtual const Eigen::MatrixXd & | jointSpaceInertiaMatrix ()=0 |
Returns the joint space mass-inertia matrix, which is nj x nj, where nj is the number of joints + number of floating coordinates, i.e. 6 in case of a floating base robot. | |
virtual const Eigen::VectorXd & | biasForces ()=0 |
Returns the bias force vector, which is nj x 1, where nj is the number of joints + number of floating coordinates, i.e. 6 in case of a floating base robot. | |
const std::vector< std::string > & | jointNames () |
Return all joint names excluding the floating base. This will be. | |
const std::vector< std::string > & | actuatedJointNames () |
Return only actuated joint names. | |
const std::vector< std::string > & | independentJointNames () |
Return only independent joint names. | |
uint | jointIndex (const std::string &joint_name) |
Get the internal index for a joint name. | |
const std::string & | baseFrame () |
Get the base frame of the robot, i.e. the root of the URDF model. | |
const std::string & | worldFrame () |
Get the world frame id. Will be "world" in case of floating base robot and equal to base_frame in case of fixed base robot. | |
const types::JointLimits | jointLimits () |
Return current joint limits. | |
const Eigen::MatrixXd & | selectionMatrix () |
Return current selection matrix, mapping actuated to all dof. | |
bool | hasLink (const std::string &link_name) |
Return True if given link name is available in robot model, false otherwise. | |
bool | hasJoint (const std::string &joint_name) |
Return True if given joint name is available in robot model, false otherwise. | |
bool | hasActuatedJoint (const std::string &joint_name) |
Return True if given joint name is an actuated joint in robot model, false otherwise. | |
virtual const types::RigidBodyState & | centerOfMass ()=0 |
Return centers of mass expressed in world frame. | |
void | setContacts (const std::vector< types::Contact > &contacts) |
Set new contact points. | |
const std::vector< types::Contact > & | getContacts () |
get current contact points | |
uint | nj () |
Return number of joints. | |
uint | na () |
Return number of actuated joints. | |
uint | nfb () |
Return dof of the floating base, will be either 0 (floating_base == false) or 6 (floating_base = = false) | |
uint | nac () |
uint | nc () |
Return total number of contact points. | |
const Eigen::VectorXd & | getQ () |
Return full system position vector, incl. floating base. Convention [floating_base_pos, floating_base_ori, joint_positions]: [fb_x,fb_y,fb_z, fb_qx,fb_qy,fb_qz,fb_qw, q_1 ... q_n)]. | |
const Eigen::VectorXd & | getQd () |
Return full system velocity vector, incl. floating base. Convention [floating_base_vel, floating_base_rot_vel, joint_velocities]: [fb_vx,fb_vx,fb_vx, fb_wx,fb_wy,fb_wz, qd_1 ... qd_n)]. | |
const Eigen::VectorXd & | getQdd () |
Return full system acceleration vector, incl. floating base. Convention [floating_base_acc, floating_base_rot_acc, joint_accelerations]: [fb_dvx,fb_dvx,fb_dvx, fb_dwx,fb_dwy,fb_dwz, qdd_1 ... qdd_n)]. | |
void | setGravityVector (const Eigen::Vector3d &g) |
Set the current gravity vector. Default is (0,0,-9.81) | |
const types::RigidBodyState & | floatingBaseState () |
Get current status of floating base. | |
const RobotModelConfig & | getRobotModelConfig () |
Get current robot model config. | |
bool | hasFloatingBase () |
Is is a floating base robot? | |
urdf::ModelInterfaceSharedPtr | loadRobotURDF (const std::string &file_or_string) |
Load URDF model from either file or string. | |
urdf::ModelInterfaceSharedPtr | getURDFModel () |
Return current URDF model. | |
void | setJointWeights (const Eigen::VectorXd &weights) |
set Joint weights by given name | |
const Eigen::VectorXd & | getJointWeights () const |
Get Joint weights as Named vector. | |
virtual const Eigen::VectorXd & | inverseDynamics (const Eigen::VectorXd &qdd_ref=Eigen::VectorXd(), const std::vector< types::Wrench > &f_ext=std::vector< types::Wrench >())=0 |
Compute tau from internal state. | |
Protected Member Functions | |
void | clear () |
void | resetData () |
void | updateData () |
Interface for all robot models. This has to provide all kinematics and dynamics information that is required for WBC. Conventions and usage:
wbc::RobotModel::RobotModel | ( | ) |
|
inlinevirtual |
|
pure virtual |
Returns the spatial acceleration of the body defined by frame_id. Twist will be in "local-world-aligned" (hybrid) representation, i.e., with respect to a frame attached to the given body, but aligned to world coordinates (floating base robot) or robot root link (fixed base)
Implemented in wbc::RobotModelPinocchio, and wbc::RobotModelRBDL.
|
inline |
Return only actuated joint names.
|
inline |
Get the base frame of the robot, i.e. the root of the URDF model.
|
pure virtual |
Returns the bias force vector, which is nj x 1, where nj is the number of joints + number of floating coordinates, i.e. 6 in case of a floating base robot.
Implemented in wbc::RobotModelHyrodyn, wbc::RobotModelPinocchio, and wbc::RobotModelRBDL.
|
pure virtual |
Returns the Body Jacobian for the given frame. The order of the Jacobian's columns will be the same as in the model (alphabetic).
frame_id | Reference frame of the Jacobian. Has to be a valid link in the robot model. |
Implemented in wbc::RobotModelPinocchio, and wbc::RobotModelRBDL.
|
pure virtual |
Return centers of mass expressed in world frame.
Implemented in wbc::RobotModelHyrodyn, wbc::RobotModelPinocchio, and wbc::RobotModelRBDL.
|
protected |
|
pure virtual |
Returns the CoM Jacobian for the robot, which maps the robot joint velocities to linear spatial velocities of the CoM expressed in world frame (floating base robot) or robot root link (fixed base). The order of the columns will be the same as the configured joint order of the robot.
Implemented in wbc::RobotModelHyrodyn, wbc::RobotModelPinocchio, and wbc::RobotModelRBDL.
|
pure virtual |
This will read the robot model from the given URDF file and initialize all members accordingly.
cfg | Model configuration. See RobotModelConfig.hpp for details |
Implemented in wbc::RobotModelHyrodyn, wbc::RobotModelPinocchio, and wbc::RobotModelRBDL.
|
inline |
Get current status of floating base.
|
inline |
get current contact points
|
inline |
Get Joint weights as Named vector.
|
inline |
Return full system position vector, incl. floating base. Convention [floating_base_pos, floating_base_ori, joint_positions]: [fb_x,fb_y,fb_z, fb_qx,fb_qy,fb_qz,fb_qw, q_1 ... q_n)].
|
inline |
Return full system velocity vector, incl. floating base. Convention [floating_base_vel, floating_base_rot_vel, joint_velocities]: [fb_vx,fb_vx,fb_vx, fb_wx,fb_wy,fb_wz, qd_1 ... qd_n)].
|
inline |
Return full system acceleration vector, incl. floating base. Convention [floating_base_acc, floating_base_rot_acc, joint_accelerations]: [fb_dvx,fb_dvx,fb_dvx, fb_dwx,fb_dwy,fb_dwz, qdd_1 ... qdd_n)].
|
inline |
Get current robot model config.
|
inline |
Return current URDF model.
bool wbc::RobotModel::hasActuatedJoint | ( | const std::string & | joint_name | ) |
Return True if given joint name is an actuated joint in robot model, false otherwise.
|
inline |
Is is a floating base robot?
bool wbc::RobotModel::hasJoint | ( | const std::string & | joint_name | ) |
Return True if given joint name is available in robot model, false otherwise.
bool wbc::RobotModel::hasLink | ( | const std::string & | link_name | ) |
Return True if given link name is available in robot model, false otherwise.
|
inline |
Return only independent joint names.
|
pure virtual |
Compute tau from internal state.
qdd_ref | (Optional) Desired reference joint acceleration (including floating base), if empty actual acceleration will be used. |
f_ext | (Optional) Contact wrenches, if not empty, size has to match the number of contact points. All wrenches will be in local coordinates, but aligned wrt. world |
Implemented in wbc::RobotModelPinocchio, and wbc::RobotModelRBDL.
uint wbc::RobotModel::jointIndex | ( | const std::string & | joint_name | ) |
Get the internal index for a joint name.
|
inline |
Return current joint limits.
|
inline |
Return all joint names excluding the floating base. This will be.
|
pure virtual |
Returns the joint space mass-inertia matrix, which is nj x nj, where nj is the number of joints + number of floating coordinates, i.e. 6 in case of a floating base robot.
Implemented in wbc::RobotModelHyrodyn, wbc::RobotModelPinocchio, and wbc::RobotModelRBDL.
|
inline |
Returns the current joint state (indepenent joints)
urdf::ModelInterfaceSharedPtr wbc::RobotModel::loadRobotURDF | ( | const std::string & | file_or_string | ) |
Load URDF model from either file or string.
|
inline |
Return number of actuated joints.
uint wbc::RobotModel::nac | ( | ) |
Return number of active contacts
|
inline |
Return total number of contact points.
|
inline |
Return dof of the floating base, will be either 0 (floating_base == false) or 6 (floating_base = = false)
|
inline |
Return number of joints.
|
pure virtual |
Returns the pose of the body defined by frame_id. Pose will be computed with respect to world frame (floating base robot) or robot root link (fixed base)
Implemented in wbc::RobotModelPinocchio, and wbc::RobotModelRBDL.
|
protected |
|
inline |
Return current selection matrix, mapping actuated to all dof.
void wbc::RobotModel::setContacts | ( | const std::vector< types::Contact > & | contacts | ) |
Set new contact points.
|
inline |
Set the current gravity vector. Default is (0,0,-9.81)
void wbc::RobotModel::setJointWeights | ( | const Eigen::VectorXd & | weights | ) |
set Joint weights by given name
|
pure virtual |
Returns the Space Jacobian for the given frame. The order of the Jacobian's columns will be the same as in the model (alphabetic). Note that the linear part of the jacobian will be aligned to world frame (floating base robot) or robot root link (fixed base), and the angular part will be in body coordinates. This is refered to as "hybrid" representation.
frame_id | Tip frame of the Jacobian. Has to be a valid link in the robot model. |
Implemented in wbc::RobotModelPinocchio, and wbc::RobotModelRBDL.
|
pure virtual |
Returns the spatial acceleration bias, i.e. the term Jdot*qdot. The linear part of the acceleration will be aligned to world frame (floating base robot) or robot root link (fixed base), and the angular part will be in body coordinates. This is refered to as "hybrid" representation.
frame_id | Reference frame of the spatial acceleration. Has to be a valid link in the robot model. |
Implemented in wbc::RobotModelPinocchio, and wbc::RobotModelRBDL.
|
pure virtual |
Returns the twist of the body defined by frame_id. Twist will be in "local-world-aligned" (hybrid) representation, i.e., with respect to a frame attached to the given body, but aligned to world coordinates (floating base robot) or robot root link (fixed base)
Implemented in wbc::RobotModelPinocchio, and wbc::RobotModelRBDL.
void wbc::RobotModel::update | ( | const Eigen::VectorXd & | joint_positions, |
const Eigen::VectorXd & | joint_velocities ) |
Update kinematics/dynamics for fixed base robots. Joint acceleration will be assumed zero.
joint_positions | Positions of all independent joints. These have to be in the same order as used by the model (alphabetic). For getting the joint order, call jointNames(). |
joint_velocities | Velocities of all independent joints. These have to be in the same order as used by the model (alphabetic). For getting the joint order, call jointNames(). |
|
virtual |
Update kinematics/dynamics for fixed base robots.
joint_positions | Positions of all independent joints. These have to be in the same order as used by the model (alphabetic). For getting the joint order, call jointNames(). |
joint_velocities | Velocities of all independent joints. These have to be in the same order as used by the model (alphabetic). For getting the joint order, call jointNames(). |
joint_accelerations | Accelerations of all independent joints. These have to be in the same order as used by the model (alphabetic). For getting the joint order, call jointNames(). |
|
pure virtual |
Update kinematics/dynamics.
joint_positions | Positions of all independent joints. These have to be in the same order as used by the model (alphabetic). For getting the joint order, call jointNames(). |
joint_velocities | Velocities of all independent joints. These have to be in the same order as used by the model (alphabetic). For getting the joint order, call jointNames(). |
joint_acceleration | Velocities of all independent joints. These have to be in the same order as used by the model (alphabetic). For getting the joint order, call jointNames(). |
fb_pose | Pose of the floating base in world coordinates |
fb_twist | Twist of the floating base in "local-world-aligned" (hybrid) representation, i.e., with respect to a frame attached to the floating base (robot root), but aligned to world coordinates |
fb_acc | Spatial accelerationof the floating base in "local-world-aligned" (hybrid) representation, i.e., with respect to a frame attached to the floating base (robot root), but aligned to world coordinates |
Implemented in wbc::RobotModelPinocchio, and wbc::RobotModelRBDL.
void wbc::RobotModel::update | ( | const Eigen::VectorXd & | joint_positions, |
const Eigen::VectorXd & | joint_velocities, | ||
const types::Pose & | fb_pose, | ||
const types::Twist & | fb_twist ) |
Update kinematics/dynamics for floating base robots. Joint and spatial acceleration will be assumed zero.
joint_positions | Positions of all independent joints. These have to be in the same order as used by the model (alphabetic). For getting the joint order, call jointNames(). |
joint_velocities | Velocities of all independent joints. These have to be in the same order as used by the model (alphabetic). For getting the joint order, call jointNames(). |
fb_pose | Pose of the floating base in world coordinates |
fb_twist | Twist of the floating base in "local-world-aligned" (hybrid) representation, i.e., the frame is attached to the floating base (robot root), but aligned to world coordinates |
|
protected |
|
inline |
Get the world frame id. Will be "world" in case of floating base robot and equal to base_frame in case of fixed base robot.
|
protected |
|
protected |
|
protected |
|
protected |
|
protected |
|
protected |
|
protected |
|
protected |
|
protected |
|
protected |
|
protected |
|
protected |
|
protected |
|
protected |
|
protected |
|
protected |
|
protected |
|
protected |
|
protected |
|
protected |
|
protected |
|
protected |
|
protected |
|
protected |
|
protected |
|
protected |
|
protected |
|
protected |
|
protected |
|
protected |
|
protected |
|
protected |
|
protected |
|
protected |
|
protected |