10#ifndef CROCODDYL_MULTIBODY_IMPULSES_IMPULSE_3D_HPP_
11#define CROCODDYL_MULTIBODY_IMPULSES_IMPULSE_3D_HPP_
13#include "crocoddyl/multibody/fwd.hpp"
14#include "crocoddyl/multibody/impulse-base.hpp"
18template <
typename _Scalar>
21 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
24 typedef _Scalar Scalar;
30 typedef typename MathBase::Vector2s Vector2s;
31 typedef typename MathBase::Vector3s Vector3s;
32 typedef typename MathBase::VectorXs VectorXs;
33 typedef typename MathBase::MatrixXs Matrix3s;
34 typedef typename MathBase::MatrixXs MatrixXs;
44 std::shared_ptr<StateMultibody> state,
const pinocchio::FrameIndex
id,
45 const pinocchio::ReferenceFrame type = pinocchio::ReferenceFrame::LOCAL);
54 virtual void calc(
const std::shared_ptr<ImpulseDataAbstract>& data,
55 const Eigen::Ref<const VectorXs>& x)
override;
63 virtual void calcDiff(
const std::shared_ptr<ImpulseDataAbstract>& data,
64 const Eigen::Ref<const VectorXs>& x)
override;
72 virtual void updateForce(
const std::shared_ptr<ImpulseDataAbstract>& data,
73 const VectorXs& force)
override;
79 pinocchio::DataTpl<Scalar>*
const data)
override;
90 template <
typename NewScalar>
98 virtual void print(std::ostream& os)
const override;
106template <
typename _Scalar>
108 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
110 typedef _Scalar Scalar;
113 typedef typename MathBase::Vector3s Vector3s;
114 typedef typename MathBase::Matrix3s Matrix3s;
115 typedef typename MathBase::Matrix3xs Matrix3xs;
116 typedef typename MathBase::Matrix6xs Matrix6xs;
117 typedef typename pinocchio::ForceTpl<Scalar> Force;
119 template <
template <
typename Scalar>
class Model>
121 pinocchio::DataTpl<Scalar>*
const data)
123 f_local(Force::Zero()),
124 dv0_local_dq(3, model->get_state()->get_nv()),
125 fJf(6, model->get_state()->get_nv()),
126 v_partial_dq(6, model->get_state()->get_nv()),
127 v_partial_dv(6, model->get_state()->get_nv()),
128 fJf_df(3, model->get_state()->get_nv()) {
129 frame = model->get_id();
131 model->get_state()->get_pinocchio()->frames[model->get_id()].placement;
132 fXj =
jMf.inverse().toActionMatrix();
134 dv0_local_dq.setZero();
136 v_partial_dq.setZero();
137 v_partial_dv.setZero();
139 v0_world_skew.setZero();
156 Matrix3xs dv0_local_dq;
158 Matrix6xs v_partial_dq;
159 Matrix6xs v_partial_dv;
161 Matrix3s v0_world_skew;
171#include "crocoddyl/multibody/impulses/impulse-3d.hxx"
ImpulseModel3DTpl< NewScalar > cast() const
Cast the impulse-3d model to a different scalar type.
virtual std::shared_ptr< ImpulseDataAbstract > createData(pinocchio::DataTpl< Scalar > *const data) override
Create the 6d impulse data.
virtual void updateForce(const std::shared_ptr< ImpulseDataAbstract > &data, const VectorXs &force) override
Convert the force into a stack of spatial forces.
virtual void calc(const std::shared_ptr< ImpulseDataAbstract > &data, const Eigen::Ref< const VectorXs > &x) override
Compute the 6d impulse Jacobian.
ImpulseModel3DTpl(std::shared_ptr< StateMultibody > state, const pinocchio::FrameIndex id, const pinocchio::ReferenceFrame type=pinocchio::ReferenceFrame::LOCAL)
Initialize the 3d impulse model.
virtual void calcDiff(const std::shared_ptr< ImpulseDataAbstract > &data, const Eigen::Ref< const VectorXs > &x) override
Compute the derivatives of the 6d impulse holonomic constraint.
virtual void print(std::ostream &os) const override
Print relevant information of the 3d impulse model.
pinocchio::ReferenceFrame type_
Type of contact.
pinocchio::FrameIndex id_
Reference frame id of the contact.
State multibody representation.
PinocchioData * pinocchio
Pinocchio data.
pinocchio::FrameIndex frame
Frame index of the contact frame.
SE3 jMf
Local frame placement of the contact frame.
MatrixXs Jc
Contact Jacobian.
Force f
Contact force expressed in the coordinate defined by type.