crocoddyl 1.9.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
 
Loading...
Searching...
No Matches
impulse-3d.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_3D_HPP_
10#define CROCODDYL_MULTIBODY_IMPULSES_IMPULSE_3D_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 ImpulseModel3DTpl(boost::shared_ptr<StateMultibody> state, const std::size_t frame);
37 virtual ~ImpulseModel3DTpl();
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 ImpulseData3DTpl : public ImpulseDataAbstractTpl<_Scalar> {
62 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
63
64 typedef _Scalar Scalar;
67 typedef typename MathBase::Matrix6xs Matrix6xs;
68
69 template <template <typename Scalar> class Model>
70 ImpulseData3DTpl(Model<Scalar>* const model, pinocchio::DataTpl<Scalar>* const data)
71 : Base(model, data),
72 fJf(6, model->get_state()->get_nv()),
73 v_partial_dq(6, model->get_state()->get_nv()),
74 v_partial_dv(6, model->get_state()->get_nv()) {
75 frame = model->get_frame();
76 jMf = model->get_state()->get_pinocchio()->frames[model->get_frame()].placement;
77 fXj = jMf.inverse().toActionMatrix();
78 fJf.setZero();
79 v_partial_dq.setZero();
80 v_partial_dv.setZero();
81 }
82
83 using Base::df_dx;
84 using Base::dv0_dq;
85 using Base::f;
86 using Base::frame;
87 using Base::Jc;
88 using Base::jMf;
89 using Base::pinocchio;
90
91 typename pinocchio::SE3Tpl<Scalar>::ActionMatrixType fXj;
92 Matrix6xs fJf;
93 Matrix6xs v_partial_dq;
94 Matrix6xs v_partial_dv;
95};
96
97} // namespace crocoddyl
98
99/* --- Details -------------------------------------------------------------- */
100/* --- Details -------------------------------------------------------------- */
101/* --- Details -------------------------------------------------------------- */
102#include "crocoddyl/multibody/impulses/impulse-3d.hxx"
103
104#endif // CROCODDYL_MULTIBODY_IMPULSES_IMPULSE_3D_HPP_
virtual void print(std::ostream &os) const
Print relevant information of the 3d 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