crocoddyl 1.9.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
 
Loading...
Searching...
No Matches
impulse-6d.hpp
1
2// BSD 3-Clause License
3//
4// Copyright (C) 2019-2021, LAAS-CNRS, University of Edinburgh
5// Copyright note valid unless otherwise stated in individual files.
6// All rights reserved.
8
9#ifndef CROCODDYL_MULTIBODY_IMPULSES_IMPULSE_6D_HPP_
10#define CROCODDYL_MULTIBODY_IMPULSES_IMPULSE_6D_HPP_
11
12#include <pinocchio/spatial/motion.hpp>
13#include <pinocchio/multibody/data.hpp>
14
15#include "crocoddyl/multibody/fwd.hpp"
16#include "crocoddyl/multibody/impulse-base.hpp"
17
18namespace crocoddyl {
19
20template <typename _Scalar>
22 public:
23 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
24
25 typedef _Scalar Scalar;
31 typedef typename MathBase::Vector2s Vector2s;
32 typedef typename MathBase::Vector3s Vector3s;
33 typedef typename MathBase::VectorXs VectorXs;
34 typedef typename MathBase::MatrixXs MatrixXs;
35
36 ImpulseModel6DTpl(boost::shared_ptr<StateMultibody> state, const std::size_t frame);
37 virtual ~ImpulseModel6DTpl();
38
39 virtual void calc(const boost::shared_ptr<ImpulseDataAbstract>& data, const Eigen::Ref<const VectorXs>& x);
40 virtual void calcDiff(const boost::shared_ptr<ImpulseDataAbstract>& data, const Eigen::Ref<const VectorXs>& x);
41 virtual void updateForce(const boost::shared_ptr<ImpulseDataAbstract>& data, const VectorXs& force);
42 virtual boost::shared_ptr<ImpulseDataAbstract> createData(pinocchio::DataTpl<Scalar>* const data);
43
44 std::size_t get_frame() const;
45
51 virtual void print(std::ostream& os) const;
52
53 protected:
54 using Base::state_;
55
56 private:
57 std::size_t frame_;
58};
59
60template <typename _Scalar>
61struct ImpulseData6DTpl : public ImpulseDataAbstractTpl<_Scalar> {
62 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
63 typedef _Scalar Scalar;
66 typedef typename MathBase::Matrix6xs Matrix6xs;
67
68 template <template <typename Scalar> class Model>
69 ImpulseData6DTpl(Model<Scalar>* const model, pinocchio::DataTpl<Scalar>* const data)
70 : Base(model, data),
71 fJf(6, model->get_state()->get_nv()),
72 v_partial_dq(6, model->get_state()->get_nv()),
73 v_partial_dv(6, model->get_state()->get_nv()) {
74 frame = model->get_frame();
75 jMf = model->get_state()->get_pinocchio()->frames[model->get_frame()].placement;
76 fXj = jMf.inverse().toActionMatrix();
77 fJf.setZero();
78 v_partial_dq.setZero();
79 v_partial_dv.setZero();
80 }
81
82 using Base::df_dx;
83 using Base::dv0_dq;
84 using Base::f;
85 using Base::frame;
86 using Base::Jc;
87 using Base::jMf;
88 using Base::pinocchio;
89
90 typename pinocchio::SE3Tpl<Scalar>::ActionMatrixType fXj;
91 Matrix6xs fJf;
92 Matrix6xs v_partial_dq;
93 Matrix6xs v_partial_dv;
94};
95
96} // namespace crocoddyl
97
98/* --- Details -------------------------------------------------------------- */
99/* --- Details -------------------------------------------------------------- */
100/* --- Details -------------------------------------------------------------- */
101#include "crocoddyl/multibody/impulses/impulse-6d.hxx"
102
103#endif // CROCODDYL_MULTIBODY_IMPULSES_IMPULSE_6D_HPP_
virtual void print(std::ostream &os) const
Print relevant information of the 6d impulse model.
State multibody representation.
Definition: multibody.hpp:31
pinocchio::FrameIndex frame
Frame index of the contact frame.
Definition: force-base.hpp:45
pinocchio::SE3Tpl< Scalar > jMf
Local frame placement of the contact frame.
Definition: force-base.hpp:46
pinocchio::FrameIndex frame
Frame index of the contact frame.
Definition: force-base.hpp:45
pinocchio::ForceTpl< Scalar > f
Definition: force-base.hpp:48
MatrixXs Jc
Contact Jacobian.
Definition: force-base.hpp:47
pinocchio::SE3Tpl< Scalar > jMf
Local frame placement of the contact frame.
Definition: force-base.hpp:46
MatrixXs df_dx
Jacobian of the contact forces.
Definition: force-base.hpp:50
pinocchio::DataTpl< Scalar > * pinocchio
Pinocchio data.
Definition: force-base.hpp:44