|
odometry
|
#include <MotionModel.hpp>
Public Types | |
| enum | methodContactPoint { LOWEST_POINT = 0, COMBINATORICS = 1 } |
| typedef boost::shared_ptr< KinematicModel< _Scalar, _RobotTrees, _RobotJointDoF, _SlipDoF, _ContactDoF > > | kinematics_ptr |
Public Member Functions | |
| MotionModel () | |
| Default constructor. More... | |
| MotionModel (bool &status, MotionModel::methodContactPoint method, kinematics_ptr robotModel) | |
| Constructor. More... | |
| ~MotionModel () | |
| Default destructor. More... | |
| void | updateKinematics (const Eigen::Matrix< _Scalar, MODEL_DOF, 1 > &modelPositions) |
| Updates the Kinematic Model of the robot. More... | |
| virtual void | getKinematics (std::vector< Eigen::Affine3d > ¤tFkRobot, std::vector< base::Matrix6d > ¤tFkCov) |
| Returns the kinematic of the robot. More... | |
| virtual std::vector< int > | getPointsInContact () |
| Return the current points in contact. More... | |
| virtual double | navSolver (const Eigen::Matrix< _Scalar, MODEL_DOF, 1 > &modelPositions, Eigen::Matrix< _Scalar, 6, 1 > &cartesianVelocities, Eigen::Matrix< _Scalar, MODEL_DOF, 1 > &modelVelocities, Eigen::Matrix< _Scalar, 6, 6 > &cartesianVelCov, Eigen::Matrix< _Scalar, MODEL_DOF, MODEL_DOF > &modelVelCov, Eigen::Matrix< _Scalar, 6 *_RobotTrees, 6 *_RobotTrees > Weight) |
| virtual double | navSolverNoAngVelo (const Eigen::Matrix< _Scalar, MODEL_DOF, 1 > &modelPositions, Eigen::Matrix< _Scalar, 6, 1 > &cartesianVelocities, Eigen::Matrix< _Scalar, MODEL_DOF, 1 > &modelVelocities, Eigen::Matrix< _Scalar, 6, 6 > &cartesianVelCov, Eigen::Matrix< _Scalar, MODEL_DOF, MODEL_DOF > &modelVelCov, Eigen::Matrix< _Scalar, 6 *_RobotTrees, 6 *_RobotTrees > Weight) |
| virtual base::Vector6d | slipSolver (void) |
| Solver for the Slip equations TO-DO. More... | |
Public Attributes | |
| methodContactPoint | contactSelection |
| kinematics_ptr | robotModel |
| std::vector< Eigen::Affine3d > | fkRobot |
| std::vector< base::Matrix6d > | fkCov |
| std::vector< TreeContactPoint > | contactPoints |
Static Public Attributes | |
| static const unsigned int | MAX_CHAIN_DOF = _RobotJointDoF+_SlipDoF+_ContactDoF |
| static const unsigned int | MODEL_DOF = _RobotJointDoF+_RobotTrees*(_SlipDoF+_ContactDoF) |
| static const int | NO_CONTACT = -1 |
Protected Member Functions | |
| void | lowestPointInContact () |
| computes the current set of points in contact choosing the lowest point. More... | |
| void | combinatoricsPointInContact () |
| Computes the points in contact using combinatorics. More... | |
| void | selectPointsInContact (MotionModel::methodContactPoint method) |
| this method computes the point in contact depending on the method. More... | |
| void | navEquations (const Eigen::Matrix< _Scalar, 6, 1 > &cartesianVelocities, const Eigen::Matrix< _Scalar, MODEL_DOF, 1 > &modelVelocities, const Eigen::Matrix< _Scalar, 6 *_RobotTrees, MODEL_DOF > &J, const Eigen::Matrix< _Scalar, 6, 6 > &cartesianVelCov, const Eigen::Matrix< _Scalar, MODEL_DOF, MODEL_DOF > &modelVelCov, Eigen::Matrix< _Scalar, 6 *_RobotTrees, 3+_RobotTrees+(_RobotTrees *_ContactDoF)> &unknownA, Eigen::Matrix< _Scalar, 3+_RobotTrees+(_RobotTrees *_ContactDoF), 1 > &unknownx, Eigen::Matrix< _Scalar, 6 *_RobotTrees, 3+_RobotJointDoF > &knownB, Eigen::Matrix< _Scalar, 3+_RobotJointDoF, 1 > &knowny, Eigen::Matrix< _Scalar, 6 *_RobotTrees, 6 *_RobotTrees > &Weight) |
| Forms the Navigation Equations for the navigation kinematics. More... | |
| void | navEquationsNoAngVelo (const Eigen::Matrix< _Scalar, 6, 1 > &cartesianVelocities, const Eigen::Matrix< _Scalar, MODEL_DOF, 1 > &modelVelocities, const Eigen::Matrix< _Scalar, 6 *_RobotTrees, MODEL_DOF > &J, const Eigen::Matrix< _Scalar, 6, 6 > &cartesianVelCov, const Eigen::Matrix< _Scalar, MODEL_DOF, MODEL_DOF > &modelVelCov, Eigen::Matrix< _Scalar, 6 *_RobotTrees, 6+_RobotTrees+(_RobotTrees *_ContactDoF)> &unknownA, Eigen::Matrix< _Scalar, 6+_RobotTrees+(_RobotTrees *_ContactDoF), 1 > &unknownx, Eigen::Matrix< _Scalar, 6 *_RobotTrees, _RobotJointDoF > &knownB, Eigen::Matrix< _Scalar, _RobotJointDoF, 1 > &knowny, Eigen::Matrix< _Scalar, 6 *_RobotTrees, 6 *_RobotTrees > &Weight) |
| Forms the Navigation Equations for the navigation kinematics(angular velocities are unknow variables) More... | |
Motion Model solver class
| _Scalar | is the typename of your desired implementation (float, double, etc..) |
| _RobotTrees | is the number of independent Trees connected to your desired Body Center. Trees is understood as connection of kinematics chains. As an example: Asguard hybrid wheels model as Trees. Therefore Asguard has 4 Trees. Each Tree has 5 open kinematics chains, one per each foot which are potential points in contact with the ground. |
| _RobotJointDoF | Complete number of DoF in the Joint Space of your robot.This is every joint passive or active that your robot (or the part of your robot related to odometry) has. This is required for the Jacobian in a general form. Therefore, this would be part of the number of columns in the Kinematic Model Jacobian. |
| _SlipDoF | The number of DoF than you want to model the slip of the contact point. If you are not interested to model slip velocity/displacement set it to zero. Otherwise, a common slip model has 3DoF (in X an Y direction and Z rotation) |
| _ContactDoF | This is the angle of contact between the ground and the contact point. If this is not interesting for you model (i.e. the robot moves in indoor environment) set it to zero. Otherwise, for navigation on uneven terrains, it is normally modeled as 1DoF along the axis of the pitch angle of your robot. |
Note that it is very important how you specify the number of Trees according to the contact points. It is assumed that one Tree can only have one single point of contact with the ground at the same time. Therefore, if your real/physical kinematic Tree can have more than one contact point at a time (e.g: two) the Tree needs to be split according to it (e.g: two Trees). Take Asguard wheel as an example. If we want to model the wheel as two feet can have point in contact, two Trees needs to be created in the wheel (virtualy increasing the number of Trees).
| typedef boost::shared_ptr< KinematicModel <_Scalar, _RobotTrees, _RobotJointDoF, _SlipDoF, _ContactDoF> > odometry::MotionModel< _Scalar, _RobotTrees, _RobotJointDoF, _SlipDoF, _ContactDoF >::kinematics_ptr |
Pointer to the kinematic model
| enum odometry::MotionModel::methodContactPoint |
An Enum Store the different algorithm to find the point which are in contact. In the case of having external means to define the point in contact with the ground (i.e: map representation) this is not very important. If the point in contact is computed only using the current kinematic configuration of the robot, then it is important.
| Enumerator | |
|---|---|
| LOWEST_POINT | |
| COMBINATORICS | |
|
inline |
Default constructor.
|
inline |
Constructor.
Constructs the Motion Model object with the parameters.
| [out] | status,true | if everything went well. |
| [in] | method | of the points in contact selection algorithm |
| [in] | pointer | to the object with the Kinematic Model of the robot. |
Size the contactPoints variable
Assign the robot model
Know the number of contact points per tree
Assign the type of method to select the contact points actually in contact
Fill the internal contact point variable
Number of contact points
By default set the contact point zero.
Print information message
|
inline |
Default destructor.
|
inlineprotected |
Computes the points in contact using combinatorics.
Probabilistic combinatoric method to compute the points in contact for the robot. One point in contact per Tree.
TO-DO
|
inlinevirtual |
Returns the kinematic of the robot.
| [out] | robot | forward kinematics as a vector of affine transformations (one per Tree) |
| [out] | covariance | of the transformation. |
|
inlinevirtual |
Return the current points in contact.
|
inlineprotected |
computes the current set of points in contact choosing the lowest point.
This function uses the lowest point w.r.t the local (robot body frame) Z axis to compute the points in contact for the robot. One point per Tree. It stores the result in the contact point member of the class.
For all the trees of the model
More than one contact point per this tree
If there is only one contact point then it is that one making contact
This tree does not have contact
|
inlineprotected |
Forms the Navigation Equations for the navigation kinematics.
It assumes that known quantities are angular rotations (Cartesian) and joint velocities. non-slip in the X and Y axis. Only slip in z-axis (non-holonomic constrain) The unknown quantities are position of the robot in Cartesian, the contact angle and the z value of the slip vector. The system of equations is unknownA * unknownx = knownB * knowny The parameters are stored in the general dimension for the problem, therefore unknown quantities are set to NaN in the parameters of the method.
| [in] | cartesianVelocities | Eigen vector with the cartesian velocities (w.r.t local body frame). |
| [in] | modelVelocities | Eigen vector with the velocities of the model(joint, slip and contact angle) |
| [in] | J | robot jacobian matrix. |
| [in] | cartesianVelCov | uncertainty in the measurement of the cartesianVelocities vector. |
| [in] | modelVelCov | uncertainty in the measurement of the modelVelocities vector. |
| [out] | unknownA | is the matrix of unknow quantities. |
| [out] | unknownx | in the vector of unknow quantities. |
| [out] | knownB | is the matrix of know quantities |
| [out] | knowny | is the matrix of know quantities |
| [out] | Weight | matrix with the inverse of the noise for the equation (Weighted Least-Squares). |
Compute the composite rover equation matrix I
Form the unknownA matrix
Form the unknownx
Non-holonomic constraint Slip vector
Contact angles per each Tree
Form the knownB matrix
Form the knowny
|
inlineprotected |
Forms the Navigation Equations for the navigation kinematics(angular velocities are unknow variables)
It assumes that known quantities are joint velocities. No-slip in the X and Y axis. Only slip in z-axis (non-holonomic constrain) The unknown quantities are position and orientation of the robot in Cartesian, the contact angle and the z value of the slip vector. The system of equations is unknownA * unknownx = knownB * knowny The parameters are stored in the general dimension for the problem, therefore unknown quantities are set to NaN in the parameters of the method.
| [in] | cartesianVelocities | Eigen vector with the Cartesian velocities (w.r.t local body frame). |
| [in] | modelVelocities | Eigen vector with the velocities of the model(joint, slip and contact angle) |
| [in] | J | robot jacobian matrix. |
| [in] | cartesianVelCov | uncertainty in the measurement of the cartesianVelocities vector. |
| [in] | modelVelCov | uncertainty in the measurement of the modelVelocities vector. |
| [out] | unknownA | is the matrix of unknown quantities. |
| [out] | unknownx | in the vector of unknown quantities. |
| [out] | knownB | is the matrix of know quantities |
| [out] | knowny | is the matrix of know quantities |
| [out] | Weight | matrix with the inverse of the noise for the equation (Weighted Least-Squares). |
Compute the composite rover equation matrix I
Form the unknownA matrix
Form the unknownx
Non-holonomic constraint Slip vector
Contact angles per each Tree
Form the knownB matrix
Form the knowny
|
inlinevirtual |
Solver for the Navigation equations
This method get the robot Jacobian matrix and computes the Navigation kinematics equations to finally solve the Weighting Least-Square method.
The Navigation kinematics equation: A * x = B * y This method(navigation solver) perform the solution to the system above, solving:
x = (A^T * W * A)^(-1) * A^T * W * B * y
| [in] | modelPositions,position | values of the model (joints, slip and contact angle) |
| [in,out] | cartesianVelocities,velocities | of the robot Cartesian space (w.r.t local body frame). |
| [in] | modelVelocities,velocity | values of the model 9joints, slip and contact angle) |
| [in] | cartesianVelCov,uncertainty | in the measurement of the robot (local body frame) Cartesian velocities. |
| [in] | modelVelCov,uncertainty | in the measurement of the robot model velocities (joint space). |
| [in] | Weight,trees | Weighting matrix (which robot's tree has more weight to the computation). |
Initialize variables
Copy Eigen to vector
Solve the Robot Jacobian Matrix
Form the Composite Navigation Equations and Noise Covariance
Solve the Motion Model by Least-Squares (navigation kinematics)
Error of the solution
Error covariance matrix
Save the results in the parameters (previous NaN values are now just known quantities)
Angular velocity estimated noise (experimental). Get the uncertainty of the angular velocity from the LS covariance
There is one per each _RobotTrees. errorCov is a 6*_RobotTrees x 6*_RobotTrees matrix dimension
Non-holonomic constraint Slip vector
Contact angles per each Tree
|
inlinevirtual |
Solver for the Navigation equations(no angular velocities are provided)
This method get the robot Jacobian matrix and computes the Navigation kinematics equations to finally solve the Weighting Least-Square method.
The Navigation kinematics equation: A * x = B * y This method(navigation solver) perform the solution to the system above, solving:
x = (A^T * W * A)^(-1) * A^T * W * B * y
| [in] | modelPositions,position | values of the model (joints, slip and contact angle) |
| [in,out] | cartesianVelocities,velocities | of the robot Cartesian space (w.r.t. local body frame). |
| [in] | modelVelocities,velocity | values of the model 9joints, slip and contact angle) |
| [in] | cartesianVelCov,uncertainty | in the measurement of the robot (local body frame) Cartesian velocities. |
| [in] | modelVelCov,uncertainty | in the measurement of the robot model velocities (joint space). |
| [in] | Weight,trees | Weighting matrix (which robot's tree has more weight to the computation). |
Initialize variables
Copy Eigen to vector
Solve the Robot Jacobian Matrix
Form the Composite Navigation Equations and Noise Covariance
Solve the Motion Model by Least-Squares (navigation kinematics)
Error of the solution
For the Error covariance matrix
Save the results in the parameters (previous NaN values are now just known quantities)
Non-holonomic constraint Slip vector
Contact angles per each Tree
|
inlineprotected |
this method computes the point in contact depending on the method.
| method | enum variable with the desired method to compute the points in contact |
Select the method for the points in contact
|
inlinevirtual |
Solver for the Slip equations TO-DO.
|
inline |
Updates the Kinematic Model of the robot.
It computes the Forward Kinematics of the robot and update the points in contacts.
| [in] | modelPositions,the | vector with the model positions (joints, slip, contact angle). |
Copy Eigen to vector
Solve the forward kinematics for the complete Robot (all the chains)
Select the contact point
Set the contact point to the kinematic model
| std::vector<TreeContactPoint> odometry::MotionModel< _Scalar, _RobotTrees, _RobotJointDoF, _SlipDoF, _ContactDoF >::contactPoints |
Number and point in contact per each tree of the model
| methodContactPoint odometry::MotionModel< _Scalar, _RobotTrees, _RobotJointDoF, _SlipDoF, _ContactDoF >::contactSelection |
Method to select the contact points
| std::vector<base::Matrix6d> odometry::MotionModel< _Scalar, _RobotTrees, _RobotJointDoF, _SlipDoF, _ContactDoF >::fkCov |
Uncertainty of the forward kinematics (if any)
| std::vector<Eigen::Affine3d> odometry::MotionModel< _Scalar, _RobotTrees, _RobotJointDoF, _SlipDoF, _ContactDoF >::fkRobot |
Forward kinematics of the robot chains
|
static |
|
static |
This is also the order of storage of the values. Columns of the robot Jacobian (J)
|
static |
The potentical contact point of the kineatic chain does not have contact
| kinematics_ptr odometry::MotionModel< _Scalar, _RobotTrees, _RobotJointDoF, _SlipDoF, _ContactDoF >::robotModel |
Kinematic model of a robot
1.8.11