wbc
wbc Namespace Reference

Classes

class  AccelerationScene
 Acceleration-based implementation of the WBC Scene. It sets up and solves the following optimization problem: More...
 
class  AccelerationSceneReducedTSID
 Acceleration-based implementation of the WBC Scene. It sets up and solves the following problem: More...
 
class  AccelerationSceneTSID
 Acceleration-based implementation of the WBC Scene. It sets up and solves the following problem: More...
 
struct  ActivationFunction
 
class  ActiveContact
 
class  ActiveContacts
 
class  CartesianAccelerationTask
 Implementation of a Cartesian acceleration task. More...
 
class  CartesianForcePIDController
 CartesianForcePIDController implements a PID Controller on a Wrench data type. More...
 
class  CartesianPosPDController
 The CartesianPosPDController class implements a PD Controller with feed forward on the RigidBodyStateSE3 type. The following control schemes are available: More...
 
class  CartesianPotentialFieldsController
 The PotentialFieldsController class implements a multi potential field controller in Cartesian space. More...
 
class  CartesianTask
 Abstract interface for a task in Cartesian space. More...
 
class  CartesianVelocityTask
 Implementation of a Cartesian velocity task. More...
 
class  CoMAccelerationTask
 Implementation of a CoM velocity task. More...
 
class  CoMVelocityTask
 Implementation of a CoM velocity task. More...
 
class  Constraint
 Abstract class to represent a generic hard (linear) constraint for a WBC optimization problem. the constraint belongs to one of three types: equality Ax = b inequality lb <= Ax <= ub bounds lb <= x <= ub. More...
 
class  ContactsAccelerationConstraint
 Abstract class to represent a generic hard constraint for a WBC optimization problem. More...
 
class  ContactsFrictionPointConstraint
 
class  ContactsFrictionSurfaceConstraint
 
class  ContactsVelocityConstraint
 Abstract class to represent a generic hard constraint for a WBC optimization problem. More...
 
class  EffortLimitsAccelerationConstraint
 Effort limits constraint for reduced TSID scene since torques are not part of qp, torque limits are enforced through acceleration and external forces limits using the linear dependecy given by the dynamic model. More...
 
class  EiquadprogSolver
 The EiquadprogSolver class is a wrapper for the qp-solver eiquadprog (see https://github.com/stack-of-tasks/eiquadprog). It solves problems of shape: More...
 
class  HierarchicalLSSolver
 Implementation of the hierarchical weighted damped least squares solver (HWLS), similar to Schutter, J. et al. “Constraint-based Task Specification and Estimation for Sensor-Based Robot Systems in the Presence of Geometric Uncertainty.” The International Journal of Robotics Research 26 (2007): 433 - 455. It solves the following optimization problem. More...
 
struct  HierarchicalQP
 Describes a hierarchy of quadratic programs. More...
 
class  JointAccelerationTask
 Implementation of a Joint velocity task. More...
 
class  JointIntegrator
 The JointIntegrator class implements different numerical integrators. More...
 
class  JointLimitAvoidanceController
 
class  JointLimitsAccelerationConstraint
 Abstract class to represent a generic hard constraint for a WBC optimization problem. More...
 
class  JointLimitsVelocityConstraint
 Abstract class to represent a generic hard constraint for a WBC optimization problem. More...
 
class  JointPosPDController
 The JointPosPDController class implements a PD Controller with feed forward on the base-Joints type. The following control schemes are available: More...
 
class  JointTask
 Abstract interface for a task in joint space. More...
 
class  JointTorquePIDController
 
class  JointVelocityTask
 Implementation of a Joint velocity task. More...
 
class  JointWeights
 
class  OsqpSolver
 
class  PIDController
 The PIDController class implements an n-dimensional PID controller. More...
 
class  PIDCtrlParams
 
class  PlanarPotentialField
 Planar Potential field. The gradient will be constant on planes parallel to the plane defined by x0 (origin) and n (surface normal) More...
 
class  PluginLoader
 
class  PosPDController
 The PosPDController class implements the following two control scemes. More...
 
class  PotentialField
 Base class for potential fields. More...
 
struct  PotentialFieldInfo
 
class  PotentialFieldsController
 Base class for potential field controllers. More...
 
class  ProxQPSolver
 The ProxQPSolver class is a wrapper for the qp-solver prox-qp (see https://github.com/Simple-Robotics/proxsuite). It solves problems of shape: More...
 
class  QPOASESSolver
 The QPOASESSolver class is a wrapper for the qp-solver qpoases (see https://www.coin-or.org/qpOASES/doc/3.0/manual.pdf). It solves problems of shape: More...
 
class  QPSolver
 
struct  QPSolverConfig
 
struct  QPSolverFactory
 
struct  QPSolverRegistry
 
class  QPSwiftSolver
 
struct  QuadraticProgram
 Describes a quadratic program of the form. More...
 
class  RadialPotentialField
 Radial Potential field. The computed gradient will be constant on volumnes with constant radius around the center of the potential field: More...
 
class  RigidbodyDynamicsConstraint
 Abstract class to represent a generic hard constraint for a WBC optimization problem. More...
 
class  RobotModel
 Interface for all robot models. This has to provide all kinematics and dynamics information that is required for WBC. More...
 
struct  RobotModelConfig
 Robot Model configuration class. More...
 
struct  RobotModelFactory
 
class  RobotModelHyrodyn
 
class  RobotModelPinocchio
 
class  RobotModelRBDL
 
struct  RobotModelRegistry
 
class  Scene
 Base class for all wbc scenes. More...
 
struct  SceneConfig
 
struct  SceneFactory
 
struct  SceneRegistry
 
class  Task
 Abstract class to represent a generic task for a WBC optimization problem. More...
 
class  TaskConfig
 Defines a task in the whole body control problem. Valid Configurations are e.g. More...
 
class  TasksStatus
 
class  TaskStatus
 
class  URDFTools
 
class  VelocityScene
 Velocity-based implementation of the WBC Scene. It sets up and solves the following problem: More...
 
class  VelocitySceneQP
 Velocity-based implementation of the WBC Scene. It sets up and solves the following problem: More...
 

Typedefs

typedef std::shared_ptr< ContactsAccelerationConstraintContactsAccelerationConstraintPtr
 
typedef std::shared_ptr< ContactsVelocityConstraintContactsVelocityConstraintPtr
 
typedef std::shared_ptr< EffortLimitsAccelerationConstraintEffortLimitsAccelerationConstraintPtr
 
typedef std::shared_ptr< JointLimitsAccelerationConstraintJointLimitsAccelerationConstraintPtr
 
typedef std::shared_ptr< JointLimitsVelocityConstraintJointLimitsVelocityConstraintPtr
 
typedef std::shared_ptr< RigidbodyDynamicsConstraintRigidbodyDynamicsConstraintPtr
 
typedef std::shared_ptr< PlanarPotentialFieldPlanarPotentialFieldPtr
 
typedef std::shared_ptr< PotentialFieldPotentialFieldPtr
 
typedef std::shared_ptr< ConstraintConstraintPtr
 
typedef std::shared_ptr< QPSolverQPSolverPtr
 
typedef std::shared_ptr< RobotModelRobotModelPtr
 
typedef std::shared_ptr< SceneScenePtr
 
using TaskPtr = std::shared_ptr<Task>
 
using CartesianAccelerationTaskPtr = std::shared_ptr<CartesianAccelerationTask>
 
typedef std::shared_ptr< CartesianVelocityTaskCartesianVelocityTaskPtr
 
typedef std::shared_ptr< CoMAccelerationTaskCoMAccelerationTaskPtr
 
typedef std::shared_ptr< CoMVelocityTaskCoMVelocityTaskPtr
 
using JointAccelerationTaskPtr = std::shared_ptr<JointAccelerationTask>
 
typedef std::shared_ptr< JointVelocityTaskJointVelocityTaskPtr
 

Enumerations

enum  activationType {
  NO_ACTIVATION , STEP_ACTIVATION , LINEAR_ACTIVATION , QUADRATIC_ACTIVATION ,
  PROPORTIONAL_ACTIVATION
}
 
enum  TaskType { unset = -1 , jnt = 0 , cart = 1 , com = 2 }
 

Functions

template<typename T >
QPSolvercreateT ()
 
std::vector< std::string > operator+ (std::vector< std::string > a, std::vector< std::string > b)
 
template<typename T >
RobotModelcreateT ()
 
template<typename T , typename RobotModelPtr , typename QPSolverPtr >
ScenecreateT (RobotModelPtr robot_model, QPSolverPtr solver, const double dt)
 
void printRbs (base::samples::RigidBodyStateSE3 rbs)
 
base::samples::Joints makeRandomJointState (vector< string > joint_names)
 
base::samples::RigidBodyStateSE3 makeRandomFloatingBaseState ()
 
base::Twist operator- (const base::Pose &a, const base::Pose &b)
 
void testFK (RobotModelPtr robot_model, const string &tip_frame, bool verbose)
 
void testSpaceJacobian (RobotModelPtr robot_model, const string &tip_frame, bool verbose)
 
void testBodyJacobian (RobotModelPtr robot_model, const string &tip_frame, bool verbose)
 
void testCoMJacobian (RobotModelPtr robot_model, bool verbose)
 
void testDynamics (RobotModelPtr robot_model, bool verbose)
 
base::samples::Joints makeRandomJointState (std::vector< std::string > joint_names)
 
void testFK (RobotModelPtr robot_model, const std::string &tip_frame, bool verbose=false)
 
void testSpaceJacobian (RobotModelPtr robot_model, const std::string &tip_frame, bool verbose=false)
 
void testBodyJacobian (RobotModelPtr robot_model, const std::string &tip_frame, bool verbose=false)
 
base::Vector6d operator+ (base::Vector6d a, base::Acceleration b)
 
base::Vector6d operator- (base::Vector6d a, base::Acceleration b)
 
int svd_eigen_decomposition (const base::MatrixXd &A, base::MatrixXd &U, base::VectorXd &S, base::MatrixXd &V, base::VectorXd &tmp, int maxiter, double epsilon)
 
double PYTHAG (double a, double b)
 
double SIGN (double a, double b)
 

Typedef Documentation

◆ CartesianAccelerationTaskPtr

◆ CartesianVelocityTaskPtr

◆ CoMAccelerationTaskPtr

◆ CoMVelocityTaskPtr

typedef std::shared_ptr<CoMVelocityTask> wbc::CoMVelocityTaskPtr

◆ ConstraintPtr

typedef std::shared_ptr<Constraint> wbc::ConstraintPtr

◆ ContactsAccelerationConstraintPtr

◆ ContactsVelocityConstraintPtr

◆ EffortLimitsAccelerationConstraintPtr

◆ JointAccelerationTaskPtr

◆ JointLimitsAccelerationConstraintPtr

◆ JointLimitsVelocityConstraintPtr

◆ JointVelocityTaskPtr

typedef std::shared_ptr<JointVelocityTask> wbc::JointVelocityTaskPtr

◆ PlanarPotentialFieldPtr

◆ PotentialFieldPtr

typedef std::shared_ptr<PotentialField> wbc::PotentialFieldPtr

◆ QPSolverPtr

typedef std::shared_ptr<QPSolver> wbc::QPSolverPtr

◆ RigidbodyDynamicsConstraintPtr

◆ RobotModelPtr

typedef std::shared_ptr<RobotModel> wbc::RobotModelPtr

◆ ScenePtr

typedef std::shared_ptr<Scene> wbc::ScenePtr

◆ TaskPtr

using wbc::TaskPtr = std::shared_ptr<Task>

Enumeration Type Documentation

◆ activationType

Enumerator
NO_ACTIVATION 
STEP_ACTIVATION 

Activation values will always be one

LINEAR_ACTIVATION 

If input > threshold, activation will be one, else 0

QUADRATIC_ACTIVATION 

If input < threshold, activation will be increase linearly the input, otherwise 1

PROPORTIONAL_ACTIVATION 

If input < threshold, activation will be increase quadratically the input, otherwise 1

◆ TaskType

Task Type. Two types of tasks are possible:

  • Cartesian tasks: The motion between two coordinate frames (root, tip) will be constrained. This can be used for operational space control, e.g. Cartesian force/position control, obstacle avoidance, ...
  • Joint tasks: The motion for the given joints will be constrained. This can be used for joint space control, e.g. avoiding the joint limits, maintaining a certain elbow position, joint position control, ...
Enumerator
unset 
jnt 
cart 
com 

Function Documentation

◆ createT() [1/3]

template<typename T >
QPSolver * wbc::createT ( )

◆ createT() [2/3]

template<typename T >
RobotModel * wbc::createT ( )

◆ createT() [3/3]

template<typename T , typename RobotModelPtr , typename QPSolverPtr >
Scene * wbc::createT ( RobotModelPtr robot_model,
QPSolverPtr solver,
const double dt )

◆ makeRandomFloatingBaseState()

base::samples::RigidBodyStateSE3 wbc::makeRandomFloatingBaseState ( )

◆ makeRandomJointState() [1/2]

base::samples::Joints wbc::makeRandomJointState ( std::vector< std::string > joint_names)

◆ makeRandomJointState() [2/2]

base::samples::Joints wbc::makeRandomJointState ( vector< string > joint_names)

◆ operator+() [1/2]

base::Vector6d wbc::operator+ ( base::Vector6d a,
base::Acceleration b )

◆ operator+() [2/2]

std::vector< std::string > wbc::operator+ ( std::vector< std::string > a,
std::vector< std::string > b )

◆ operator-() [1/2]

base::Vector6d wbc::operator- ( base::Vector6d a,
base::Acceleration b )

◆ operator-() [2/2]

base::Twist wbc::operator- ( const base::Pose & a,
const base::Pose & b )

◆ printRbs()

void wbc::printRbs ( base::samples::RigidBodyStateSE3 rbs)

◆ PYTHAG()

double wbc::PYTHAG ( double a,
double b )
inline

◆ SIGN()

double wbc::SIGN ( double a,
double b )
inline

◆ svd_eigen_decomposition()

int wbc::svd_eigen_decomposition ( const base::MatrixXd & A,
base::MatrixXd & U,
base::VectorXd & S,
base::MatrixXd & V,
base::VectorXd & tmp,
int maxiter,
double epsilon )

◆ testBodyJacobian() [1/2]

void wbc::testBodyJacobian ( RobotModelPtr robot_model,
const std::string & tip_frame,
bool verbose = false )

◆ testBodyJacobian() [2/2]

void wbc::testBodyJacobian ( RobotModelPtr robot_model,
const string & tip_frame,
bool verbose )

◆ testCoMJacobian()

void wbc::testCoMJacobian ( RobotModelPtr robot_model,
bool verbose )

◆ testDynamics()

void wbc::testDynamics ( RobotModelPtr robot_model,
bool verbose )

◆ testFK() [1/2]

void wbc::testFK ( RobotModelPtr robot_model,
const std::string & tip_frame,
bool verbose = false )

◆ testFK() [2/2]

void wbc::testFK ( RobotModelPtr robot_model,
const string & tip_frame,
bool verbose )

◆ testSpaceJacobian() [1/2]

void wbc::testSpaceJacobian ( RobotModelPtr robot_model,
const std::string & tip_frame,
bool verbose = false )

◆ testSpaceJacobian() [2/2]

void wbc::testSpaceJacobian ( RobotModelPtr robot_model,
const string & tip_frame,
bool verbose )