state_equation.hpp

Go to the documentation of this file.

#ifndef ROBOTOC_STATE_EQUATION_HPP_
#define ROBOTOC_STATE_EQUATION_HPP_
 
#include "Eigen/Core"
 
#include "robotoc/robot/robot.hpp"
#include "robotoc/robot/se3_jacobian_inverse.hpp"
#include "robotoc/core/split_solution.hpp"
#include "robotoc/core/split_direction.hpp"
#include "robotoc/core/split_kkt_residual.hpp"
#include "robotoc/core/split_kkt_matrix.hpp"
#include "robotoc/dynamics/state_equation_data.hpp"
 
 
namespace robotoc {
 
void evalStateEquation(const Robot& robot, const double dt, 
                       const SplitSolution& s, 
                       const Eigen::VectorXd& q_next, 
                       const Eigen::VectorXd& v_next, 
                       SplitKKTResidual& kkt_residual);
 
void evalStateEquation(const Robot& robot, const double dt, 
                       const SplitSolution& s, 
                       const SplitSolution& s_next, 
                       SplitKKTResidual& kkt_residual);
 
void linearizeStateEquation(const Robot& robot, const double dt, 
                            const Eigen::VectorXd& q_prev, 
                            const SplitSolution& s, const SplitSolution& s_next, 
                            StateEquationData& data, SplitKKTMatrix& kkt_matrix, 
                            SplitKKTResidual& kkt_residual);
 
void correctLinearizeStateEquation(const Robot& robot, const double dt, 
                                   const SplitSolution& s, 
                                   const SplitSolution& s_next, 
                                   StateEquationData& data, 
                                   SplitKKTMatrix& kkt_matrix, 
                                   SplitKKTResidual& kkt_residual);
 
void correctCostateDirection(StateEquationData& data, SplitDirection& d);
 
void computeInitialStateDirection(const Robot& robot, 
                                  const Eigen::VectorXd& q0, 
                                  const Eigen::VectorXd& v0, 
                                  const SplitSolution& s0, 
                                  const StateEquationData& data,
                                  SplitDirection& d0);
 
} // namespace robotoc 
 
#endif // ROBOTOC_STATE_EQUATION_HPP_