hpp-rbprm 4.15.1
Implementation of RB-PRM planner using hpp.
Loading...
Searching...
No Matches
stability.hh
Go to the documentation of this file.
1//
2// Copyright (c) 2014 CNRS
3// Authors: Steve Tonneau (steve.tonneau@laas.fr)
4//
5// This file is part of hpp-rbprm.
6// hpp-rbprm is free software: you can redistribute it
7// and/or modify it under the terms of the GNU Lesser General Public
8// License as published by the Free Software Foundation, either version
9// 3 of the License, or (at your option) any later version.
10//
11// hpp-rbprm is distributed in the hope that it will be
12// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
13// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
14// General Lesser Public License for more details. You should have
15// received a copy of the GNU Lesser General Public License along with
16// hpp-core If not, see
17// <http://www.gnu.org/licenses/>.
18
19#ifndef HPP_RBPRM_STABILITY_HH
20#define HPP_RBPRM_STABILITY_HH
21
22#include <hpp/centroidal-dynamics/centroidal_dynamics.hh>
23#include <hpp/pinocchio/device.hh>
26#include <map>
27#include <memory>
28
29namespace hpp {
30
31namespace rbprm {
32namespace stability {
33
34typedef Eigen::Matrix<pinocchio::value_type, Eigen::Dynamic, Eigen::Dynamic,
35 Eigen::RowMajor>
37typedef Eigen::Matrix<pinocchio::value_type, Eigen::Dynamic, 1> VectorX;
38
46
47double IsStable(const RbPrmFullBodyPtr_t fullbody, State& state,
48 fcl::Vec3f acc = fcl::Vec3f(0, 0, 0),
49 fcl::Vec3f com = fcl::Vec3f(0, 0, 0),
50 const centroidal_dynamics::EquilibriumAlgorithm =
51 centroidal_dynamics::EQUILIBRIUM_ALGORITHM_DLP);
52
58std::pair<MatrixXX, VectorX> ComputeCentroidalCone(
59 const RbPrmFullBodyPtr_t fullbody, State& state,
60 const core::value_type friction = 0.5);
61
62centroidal_dynamics::Equilibrium initLibrary(const RbPrmFullBodyPtr_t fullbody);
63
64centroidal_dynamics::Vector3 setupLibrary(
65 const RbPrmFullBodyPtr_t fullbody, State& state,
66 centroidal_dynamics::Equilibrium& sEq,
67 centroidal_dynamics::EquilibriumAlgorithm& alg,
68 core::value_type friction = 0.6, const double feetX = 0,
69 const double feetY = 0);
70
71} // namespace stability
72} // namespace rbprm
73} // namespace hpp
74#endif // HPP_RBPRM_STABILITY_HH
Eigen::Matrix< pinocchio::value_type, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor > MatrixXX
Definition: stability.hh:36
Eigen::Matrix< pinocchio::value_type, Eigen::Dynamic, 1 > VectorX
Definition: stability.hh:37
double IsStable(const RbPrmFullBodyPtr_t fullbody, State &state, fcl::Vec3f acc=fcl::Vec3f(0, 0, 0), fcl::Vec3f com=fcl::Vec3f(0, 0, 0), const centroidal_dynamics::EquilibriumAlgorithm=centroidal_dynamics::EQUILIBRIUM_ALGORITHM_DLP)
centroidal_dynamics::Vector3 setupLibrary(const RbPrmFullBodyPtr_t fullbody, State &state, centroidal_dynamics::Equilibrium &sEq, centroidal_dynamics::EquilibriumAlgorithm &alg, core::value_type friction=0.6, const double feetX=0, const double feetY=0)
centroidal_dynamics::Equilibrium initLibrary(const RbPrmFullBodyPtr_t fullbody)
std::pair< MatrixXX, VectorX > ComputeCentroidalCone(const RbPrmFullBodyPtr_t fullbody, State &state, const core::value_type friction=0.5)
shared_ptr< RbPrmFullBody > RbPrmFullBodyPtr_t
Definition: kinematics_constraints.hh:12
Definition: algorithm.hh:26
Definition: rbprm-state.hh:40