|
threed_odometry
|
#include <MotionModel.hpp>
Public Member Functions | |
| MotionModel (const int _number_chains, const int _number_robot_joints, const int _number_slip_joints, const int _number_contact_joints) | |
| Constructor. More... | |
| ~MotionModel () | |
| Default destructor. More... | |
| virtual double | navSolver (const Eigen::Matrix< _Scalar, Eigen::Dynamic, 1 > &modelPositions, Eigen::Matrix< _Scalar, Eigen::Dynamic, 1 > &modelVelocities, const Eigen::Matrix< _Scalar, Eigen::Dynamic, Eigen::Dynamic > &J, Eigen::Matrix< _Scalar, 6, 1 > &cartesianVelocities, Eigen::Matrix< _Scalar, Eigen::Dynamic, Eigen::Dynamic > &modelVelCov, Eigen::Matrix< _Scalar, 6, 6 > &cartesianVelCov, const Eigen::Matrix< _Scalar, Eigen::Dynamic, Eigen::Dynamic > Weight, bool known_contact_angles=false) |
Public Attributes | |
| unsigned int | model_dof |
Protected Member Functions | |
| void | navEquations (const Eigen::Matrix< _Scalar, 6, 1 > &cartesianVelocities, const Eigen::Matrix< _Scalar, Eigen::Dynamic, 1 > &modelVelocities, const Eigen::Matrix< _Scalar, Eigen::Dynamic, Eigen::Dynamic > &J, Eigen::Matrix< _Scalar, Eigen::Dynamic, Eigen::Dynamic > &unknownA, Eigen::Matrix< _Scalar, Eigen::Dynamic, 1 > &unknownx, Eigen::Matrix< _Scalar, Eigen::Dynamic, Eigen::Dynamic > &knownB, Eigen::Matrix< _Scalar, Eigen::Dynamic, 1 > &knowny, bool known_contact_angles=false) |
| Forms the Navigation Equations for the navigation kinematics. More... | |
Protected Attributes | |
| int | number_chains |
| int | number_robot_joints |
| int | number_slip_joints |
| int | number_contact_joints |
Motion Model solver class
| _Scalar | is the typename of your desired implementation (float, double, etc..) |
Note that it is very important how you specify the number of Chains according to the contact points. It is assumed that one Chain can only have one single point of contact with the ground at the same time. Take Asguard wheel as an example. Each wheel is in reality five chains. However, Five points are not always in contact therefor on order to determine which (and how many) points are finally making the robot motion ones should use the weight matrix (Weight Least Square).
|
inline |
Constructor.
Constructs the Motion Model object with the parameters.
| number_chains | is the number of independent Chains connected to your desired Body Center. Trees is understood as connection of kinematics chains. As an example: Asguard hybrid wheels model as Chains. Therefore Asguard has five chain per Wheel, one per each foot which are potential points in contact with the ground. |
| number_robot_joints | Complete number of robot joints (DoF). This is every joint passive or active that your robot (or the part of your robot related to Odometry, which is the chassis) physically has. This is required for the Jacobian in a general form and to slip the Jacobian in sub-matrices. |
| number_slip_joints | The number of slip joints (DoF) than your robot has. For example if your robot has six contact points and 3DoF slip model per each contact point, therefore your robot has 18 slip joints. A common slip model has 3DoF (in X an Y direction and Z rotation) |
| number_contact_joints | This is the number of contact angles of your robot. For example, in case your robot has six contact points and 1DoF contact ange per each contact point. Your robot has 6 contact angle joints. In case it 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. |
|
inline |
Default destructor.
|
inlineprotected |
Forms the Navigation Equations for the navigation kinematics.
It assumes that known quantities are angular rotations (Cartesian), joint velocities and contact angles (depending on the known_contact_angles flag). There is 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 equation (Weighted Least-Squares). |
Compute the composite rover equation matrix I
Form the unknownA matrix
Form the unknownx vector
Non-holonomic constraint Slip vector
Contact angles per each Tree in the unknown par of the system
Form the knownB matrix
Form the knowny vector
Contact angles per each Tree in the known par of the system
|
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] | modelVelocities,velocity | values of the model (joints, slip and contact angle) |
| [in] | J,The | complete robot Jacobian matrix |
| [in,out] | cartesianVelocities,velocities | of the robot Cartesian space (w.r.t local body frame). |
| [in,out] | modelVelCov,uncertainty | in the measurement of the robot model velocities (joint space). |
| [in,out] | cartesianVelCov,uncertainty | in the measurement of the robot Cartesian velocities (w.r.t local body frame). |
| [in] | Weight,trees | Weighting matrix. It defines which robot's tree has more weight to the computation (0 will not have contribution to the movement, no-contact). |
| [in] | known_contact_angles,flags | to set to true when the contact angles is known, false otherwise. This affect to vectors and matrices dimensions. |
Resize the vector and matrices to the right dimensions
Resize the vector and matrices to the right dimensions
Initialize variables
Assert vectors sizes
Form the Composite Navigation Equations and Noise Covariance
Solve the Motion Model by Least-Squares (navigation kinematics)
Solve the system
Error of the solution
Error covariance matrix
Error in the solution. The size depends on the known_contact_angles
Least square error
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
Save the non-holonomic constraint, z-slip velocity vector
Save the estimated contact angle velocities per each Tree
| unsigned int threed_odometry::MotionModel< _Scalar >::model_dof |
|
protected |
|
protected |
|
protected |
|
protected |
1.8.11