Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	include/crocoddyl/multibody/states/multibody.hxx	Lines:	105	140	75.0 %
Date:	2024-02-13 11:12:33	Branches:	146	639	22.8 %


///////////////////////////////////////////////////////////////////////////////
// BSD 3-Clause License
//
// Copyright (C) 2019, LAAS-CNRS
// Copyright note valid unless otherwise stated in individual files.
// All rights reserved.
///////////////////////////////////////////////////////////////////////////////

#include <pinocchio/algorithm/joint-configuration.hpp>

#include "crocoddyl/core/utils/exception.hpp"
#include "crocoddyl/multibody/states/multibody.hpp"

namespace crocoddyl {

template <typename Scalar>
StateMultibodyTpl<Scalar>::StateMultibodyTpl(
    boost::shared_ptr<PinocchioModel> model)
    : Base(model->nq + model->nv, 2 * model->nv),
      pinocchio_(model),
      x0_(VectorXs::Zero(model->nq + model->nv)) {
  x0_.head(nq_) = pinocchio::neutral(*pinocchio_.get());

  // In a multibody system, we could define the first joint using Lie groups.
  // The current cases are free-flyer (SE3) and spherical (S03).
  // Instead simple represents any joint that can model within the Euclidean
  // manifold. The rest of joints use Euclidean algebra. We use this fact for
  // computing Jdiff.

  // Define internally the limits of the first joint

  const std::size_t nq0 = model->joints[1].nq();

  lb_.head(nq0) =
      -std::numeric_limits<Scalar>::infinity() * VectorXs::Ones(nq0);
  ub_.head(nq0) = std::numeric_limits<Scalar>::infinity() * VectorXs::Ones(nq0);
  lb_.segment(nq0, nq_ - nq0) = pinocchio_->lowerPositionLimit.tail(nq_ - nq0);
  ub_.segment(nq0, nq_ - nq0) = pinocchio_->upperPositionLimit.tail(nq_ - nq0);
  lb_.tail(nv_) = -pinocchio_->velocityLimit;
  ub_.tail(nv_) = pinocchio_->velocityLimit;
  Base::update_has_limits();
}

template <typename Scalar>
StateMultibodyTpl<Scalar>::StateMultibodyTpl()
    : Base(), x0_(VectorXs::Zero(0)) {}

template <typename Scalar>
StateMultibodyTpl<Scalar>::~StateMultibodyTpl() {}

template <typename Scalar>
typename MathBaseTpl<Scalar>::VectorXs StateMultibodyTpl<Scalar>::zero() const {
  return x0_;
}

template <typename Scalar>
typename MathBaseTpl<Scalar>::VectorXs StateMultibodyTpl<Scalar>::rand() const {
  VectorXs xrand = VectorXs::Random(nx_);
  xrand.head(nq_) = pinocchio::randomConfiguration(*pinocchio_.get());
  return xrand;
}

template <typename Scalar>
void StateMultibodyTpl<Scalar>::diff(const Eigen::Ref<const VectorXs>& x0,
                                     const Eigen::Ref<const VectorXs>& x1,
                                     Eigen::Ref<VectorXs> dxout) const {
  if (static_cast<std::size_t>(x0.size()) != nx_) {
    throw_pretty("Invalid argument: "
                 << "x0 has wrong dimension (it should be " +
                        std::to_string(nx_) + ")");
  }
  if (static_cast<std::size_t>(x1.size()) != nx_) {
    throw_pretty("Invalid argument: "
                 << "x1 has wrong dimension (it should be " +
                        std::to_string(nx_) + ")");
  }
  if (static_cast<std::size_t>(dxout.size()) != ndx_) {
    throw_pretty("Invalid argument: "
                 << "dxout has wrong dimension (it should be " +
                        std::to_string(ndx_) + ")");
  }

  pinocchio::difference(*pinocchio_.get(), x0.head(nq_), x1.head(nq_),
                        dxout.head(nv_));
  dxout.tail(nv_) = x1.tail(nv_) - x0.tail(nv_);
}

template <typename Scalar>
void StateMultibodyTpl<Scalar>::integrate(const Eigen::Ref<const VectorXs>& x,
                                          const Eigen::Ref<const VectorXs>& dx,
                                          Eigen::Ref<VectorXs> xout) const {
  if (static_cast<std::size_t>(x.size()) != nx_) {
    throw_pretty("Invalid argument: "
                 << "x has wrong dimension (it should be " +
                        std::to_string(nx_) + ")");
  }
  if (static_cast<std::size_t>(dx.size()) != ndx_) {
    throw_pretty("Invalid argument: "
                 << "dx has wrong dimension (it should be " +
                        std::to_string(ndx_) + ")");
  }
  if (static_cast<std::size_t>(xout.size()) != nx_) {
    throw_pretty("Invalid argument: "
                 << "xout has wrong dimension (it should be " +
                        std::to_string(nx_) + ")");
  }

  pinocchio::integrate(*pinocchio_.get(), x.head(nq_), dx.head(nv_),
                       xout.head(nq_));
  xout.tail(nv_) = x.tail(nv_) + dx.tail(nv_);
}

template <typename Scalar>
void StateMultibodyTpl<Scalar>::Jdiff(const Eigen::Ref<const VectorXs>& x0,
                                      const Eigen::Ref<const VectorXs>& x1,
                                      Eigen::Ref<MatrixXs> Jfirst,
                                      Eigen::Ref<MatrixXs> Jsecond,
                                      const Jcomponent firstsecond) const {
  assert_pretty(
      is_a_Jcomponent(firstsecond),
      ("firstsecond must be one of the Jcomponent {both, first, second}"));
  if (static_cast<std::size_t>(x0.size()) != nx_) {
    throw_pretty("Invalid argument: "
                 << "x0 has wrong dimension (it should be " +
                        std::to_string(nx_) + ")");
  }
  if (static_cast<std::size_t>(x1.size()) != nx_) {
    throw_pretty("Invalid argument: "
                 << "x1 has wrong dimension (it should be " +
                        std::to_string(nx_) + ")");
  }

  if (firstsecond == first) {
    if (static_cast<std::size_t>(Jfirst.rows()) != ndx_ ||
        static_cast<std::size_t>(Jfirst.cols()) != ndx_) {
      throw_pretty("Invalid argument: "
                   << "Jfirst has wrong dimension (it should be " +
                          std::to_string(ndx_) + "," + std::to_string(ndx_) +
                          ")");
    }

    pinocchio::dDifference(*pinocchio_.get(), x0.head(nq_), x1.head(nq_),
                           Jfirst.topLeftCorner(nv_, nv_), pinocchio::ARG0);
    Jfirst.bottomRightCorner(nv_, nv_).diagonal().array() = (Scalar)-1;
  } else if (firstsecond == second) {
    if (static_cast<std::size_t>(Jsecond.rows()) != ndx_ ||
        static_cast<std::size_t>(Jsecond.cols()) != ndx_) {
      throw_pretty("Invalid argument: "
                   << "Jsecond has wrong dimension (it should be " +
                          std::to_string(ndx_) + "," + std::to_string(ndx_) +
                          ")");
    }
    pinocchio::dDifference(*pinocchio_.get(), x0.head(nq_), x1.head(nq_),
                           Jsecond.topLeftCorner(nv_, nv_), pinocchio::ARG1);
    Jsecond.bottomRightCorner(nv_, nv_).diagonal().array() = (Scalar)1;
  } else {  // computing both
    if (static_cast<std::size_t>(Jfirst.rows()) != ndx_ ||
        static_cast<std::size_t>(Jfirst.cols()) != ndx_) {
      throw_pretty("Invalid argument: "
                   << "Jfirst has wrong dimension (it should be " +
                          std::to_string(ndx_) + "," + std::to_string(ndx_) +
                          ")");
    }
    if (static_cast<std::size_t>(Jsecond.rows()) != ndx_ ||
        static_cast<std::size_t>(Jsecond.cols()) != ndx_) {
      throw_pretty("Invalid argument: "
                   << "Jsecond has wrong dimension (it should be " +
                          std::to_string(ndx_) + "," + std::to_string(ndx_) +
                          ")");
    }
    pinocchio::dDifference(*pinocchio_.get(), x0.head(nq_), x1.head(nq_),
                           Jfirst.topLeftCorner(nv_, nv_), pinocchio::ARG0);
    pinocchio::dDifference(*pinocchio_.get(), x0.head(nq_), x1.head(nq_),
                           Jsecond.topLeftCorner(nv_, nv_), pinocchio::ARG1);
    Jfirst.bottomRightCorner(nv_, nv_).diagonal().array() = (Scalar)-1;
    Jsecond.bottomRightCorner(nv_, nv_).diagonal().array() = (Scalar)1;
  }
}

template <typename Scalar>
void StateMultibodyTpl<Scalar>::Jintegrate(const Eigen::Ref<const VectorXs>& x,
                                           const Eigen::Ref<const VectorXs>& dx,
                                           Eigen::Ref<MatrixXs> Jfirst,
                                           Eigen::Ref<MatrixXs> Jsecond,
                                           const Jcomponent firstsecond,
                                           const AssignmentOp op) const {
  assert_pretty(
      is_a_Jcomponent(firstsecond),
      ("firstsecond must be one of the Jcomponent {both, first, second}"));
  assert_pretty(is_a_AssignmentOp(op),
                ("op must be one of the AssignmentOp {settop, addto, rmfrom}"));
  if (firstsecond == first || firstsecond == both) {
    if (static_cast<std::size_t>(Jfirst.rows()) != ndx_ ||
        static_cast<std::size_t>(Jfirst.cols()) != ndx_) {
      throw_pretty("Invalid argument: "
                   << "Jfirst has wrong dimension (it should be " +
                          std::to_string(ndx_) + "," + std::to_string(ndx_) +
                          ")");
    }
    switch (op) {
      case setto:
        pinocchio::dIntegrate(*pinocchio_.get(), x.head(nq_), dx.head(nv_),
                              Jfirst.topLeftCorner(nv_, nv_), pinocchio::ARG0,
                              pinocchio::SETTO);
        Jfirst.bottomRightCorner(nv_, nv_).diagonal().array() = (Scalar)1;
        break;
      case addto:
        pinocchio::dIntegrate(*pinocchio_.get(), x.head(nq_), dx.head(nv_),
                              Jfirst.topLeftCorner(nv_, nv_), pinocchio::ARG0,
                              pinocchio::ADDTO);
        Jfirst.bottomRightCorner(nv_, nv_).diagonal().array() += (Scalar)1;
        break;
      case rmfrom:
        pinocchio::dIntegrate(*pinocchio_.get(), x.head(nq_), dx.head(nv_),
                              Jfirst.topLeftCorner(nv_, nv_), pinocchio::ARG0,
                              pinocchio::RMTO);
        Jfirst.bottomRightCorner(nv_, nv_).diagonal().array() -= (Scalar)1;
        break;
      default:
        throw_pretty(
            "Invalid argument: allowed operators: setto, addto, rmfrom");
        break;
    }
  }
  if (firstsecond == second || firstsecond == both) {
    if (static_cast<std::size_t>(Jsecond.rows()) != ndx_ ||
        static_cast<std::size_t>(Jsecond.cols()) != ndx_) {
      throw_pretty("Invalid argument: "
                   << "Jsecond has wrong dimension (it should be " +
                          std::to_string(ndx_) + "," + std::to_string(ndx_) +
                          ")");
    }
    switch (op) {
      case setto:
        pinocchio::dIntegrate(*pinocchio_.get(), x.head(nq_), dx.head(nv_),
                              Jsecond.topLeftCorner(nv_, nv_), pinocchio::ARG1,
                              pinocchio::SETTO);
        Jsecond.bottomRightCorner(nv_, nv_).diagonal().array() = (Scalar)1;
        break;
      case addto:
        pinocchio::dIntegrate(*pinocchio_.get(), x.head(nq_), dx.head(nv_),
                              Jsecond.topLeftCorner(nv_, nv_), pinocchio::ARG1,
                              pinocchio::ADDTO);
        Jsecond.bottomRightCorner(nv_, nv_).diagonal().array() += (Scalar)1;
        break;
      case rmfrom:
        pinocchio::dIntegrate(*pinocchio_.get(), x.head(nq_), dx.head(nv_),
                              Jsecond.topLeftCorner(nv_, nv_), pinocchio::ARG1,
                              pinocchio::RMTO);
        Jsecond.bottomRightCorner(nv_, nv_).diagonal().array() -= (Scalar)1;
        break;
      default:
        throw_pretty(
            "Invalid argument: allowed operators: setto, addto, rmfrom");
        break;
    }
  }
}

template <typename Scalar>
void StateMultibodyTpl<Scalar>::JintegrateTransport(
    const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& dx,
    Eigen::Ref<MatrixXs> Jin, const Jcomponent firstsecond) const {
  assert_pretty(
      is_a_Jcomponent(firstsecond),
      ("firstsecond must be one of the Jcomponent {both, first, second}"));

  switch (firstsecond) {
    case first:
      pinocchio::dIntegrateTransport(*pinocchio_.get(), x.head(nq_),
                                     dx.head(nv_), Jin.topRows(nv_),
                                     pinocchio::ARG0);
      break;
    case second:
      pinocchio::dIntegrateTransport(*pinocchio_.get(), x.head(nq_),
                                     dx.head(nv_), Jin.topRows(nv_),
                                     pinocchio::ARG1);
      break;
    default:
      throw_pretty(
          "Invalid argument: firstsecond must be either first or second. both "
          "not supported for this operation.");
      break;
  }
}

template <typename Scalar>
const boost::shared_ptr<pinocchio::ModelTpl<Scalar> >&
StateMultibodyTpl<Scalar>::get_pinocchio() const {
  return pinocchio_;
}

}  // namespace crocoddyl


Generated by: GCOVR (Version 4.2)

Line	Branch	Exec	Source
1			///////////////////////////////////////////////////////////////////////////////
2			// BSD 3-Clause License
3			//
4			// Copyright (C) 2019, LAAS-CNRS
5			// Copyright note valid unless otherwise stated in individual files.
6			// All rights reserved.
7			///////////////////////////////////////////////////////////////////////////////
8
9			#include <pinocchio/algorithm/joint-configuration.hpp>
10
11			#include "crocoddyl/core/utils/exception.hpp"
12			#include "crocoddyl/multibody/states/multibody.hpp"
13
14			namespace crocoddyl {
15
16			template <typename Scalar>
17		11084	StateMultibodyTpl<Scalar>::StateMultibodyTpl(
18			boost::shared_ptr<PinocchioModel> model)
19		11084	: Base(model->nq + model->nv, 2 * model->nv),
20			pinocchio_(model),
21	✓✗✓✗	22168	x0_(VectorXs::Zero(model->nq + model->nv)) {
22	✓✗✓✗ ✓✗	11084	x0_.head(nq_) = pinocchio::neutral(*pinocchio_.get());
23
24			// In a multibody system, we could define the first joint using Lie groups.
25			// The current cases are free-flyer (SE3) and spherical (S03).
26			// Instead simple represents any joint that can model within the Euclidean
27			// manifold. The rest of joints use Euclidean algebra. We use this fact for
28			// computing Jdiff.
29
30			// Define internally the limits of the first joint
31
32	✓✗	11084	const std::size_t nq0 = model->joints[1].nq();
33
34	✓✗✓✗ ✓✗✓✗	22168	lb_.head(nq0) =
35		11084	-std::numeric_limits<Scalar>::infinity() * VectorXs::Ones(nq0);
36	✓✗✓✗ ✓✗✓✗	11084	ub_.head(nq0) = std::numeric_limits<Scalar>::infinity() * VectorXs::Ones(nq0);
37	✓✗✓✗ ✓✗	11084	lb_.segment(nq0, nq_ - nq0) = pinocchio_->lowerPositionLimit.tail(nq_ - nq0);
38	✓✗✓✗ ✓✗	11084	ub_.segment(nq0, nq_ - nq0) = pinocchio_->upperPositionLimit.tail(nq_ - nq0);
39	✓✗✓✗ ✓✗	11084	lb_.tail(nv_) = -pinocchio_->velocityLimit;
40	✓✗✓✗	11084	ub_.tail(nv_) = pinocchio_->velocityLimit;
41	✓✗	11084	Base::update_has_limits();
42		11084	}
43
44			template <typename Scalar>
45			StateMultibodyTpl<Scalar>::StateMultibodyTpl()
46			: Base(), x0_(VectorXs::Zero(0)) {}
47
48			template <typename Scalar>
49		22172	StateMultibodyTpl<Scalar>::~StateMultibodyTpl() {}
50
51			template <typename Scalar>
52		3560	typename MathBaseTpl<Scalar>::VectorXs StateMultibodyTpl<Scalar>::zero() const {
53		3560	return x0_;
54			}
55
56			template <typename Scalar>
57		11884	typename MathBaseTpl<Scalar>::VectorXs StateMultibodyTpl<Scalar>::rand() const {
58	✓✗	11884	VectorXs xrand = VectorXs::Random(nx_);
59	✓✗✓✗ ✓✗	11884	xrand.head(nq_) = pinocchio::randomConfiguration(*pinocchio_.get());
60		11884	return xrand;
61			}
62
63			template <typename Scalar>
64		105907	void StateMultibodyTpl<Scalar>::diff(const Eigen::Ref<const VectorXs>& x0,
65			const Eigen::Ref<const VectorXs>& x1,
66			Eigen::Ref<VectorXs> dxout) const {
67	✗✓	105907	if (static_cast<std::size_t>(x0.size()) != nx_) {
68			throw_pretty("Invalid argument: "
69			<< "x0 has wrong dimension (it should be " +
70			std::to_string(nx_) + ")");
71			}
72	✗✓	105907	if (static_cast<std::size_t>(x1.size()) != nx_) {
73			throw_pretty("Invalid argument: "
74			<< "x1 has wrong dimension (it should be " +
75			std::to_string(nx_) + ")");
76			}
77	✗✓	105907	if (static_cast<std::size_t>(dxout.size()) != ndx_) {
78			throw_pretty("Invalid argument: "
79			<< "dxout has wrong dimension (it should be " +
80			std::to_string(ndx_) + ")");
81			}
82
83	✓✗✓✗ ✓✗	105907	pinocchio::difference(*pinocchio_.get(), x0.head(nq_), x1.head(nq_),
84		105907	dxout.head(nv_));
85	✓✗✓✗ ✓✗✓✗	105907	dxout.tail(nv_) = x1.tail(nv_) - x0.tail(nv_);
86		105907	}
87
88			template <typename Scalar>
89		147694	void StateMultibodyTpl<Scalar>::integrate(const Eigen::Ref<const VectorXs>& x,
90			const Eigen::Ref<const VectorXs>& dx,
91			Eigen::Ref<VectorXs> xout) const {
92	✗✓	147694	if (static_cast<std::size_t>(x.size()) != nx_) {
93			throw_pretty("Invalid argument: "
94			<< "x has wrong dimension (it should be " +
95			std::to_string(nx_) + ")");
96			}
97	✗✓	147694	if (static_cast<std::size_t>(dx.size()) != ndx_) {
98			throw_pretty("Invalid argument: "
99			<< "dx has wrong dimension (it should be " +
100			std::to_string(ndx_) + ")");
101			}
102	✗✓	147694	if (static_cast<std::size_t>(xout.size()) != nx_) {
103			throw_pretty("Invalid argument: "
104			<< "xout has wrong dimension (it should be " +
105			std::to_string(nx_) + ")");
106			}
107
108	✓✗✓✗ ✓✗	147694	pinocchio::integrate(*pinocchio_.get(), x.head(nq_), dx.head(nv_),
109		147694	xout.head(nq_));
110	✓✗✓✗ ✓✗✓✗	147694	xout.tail(nv_) = x.tail(nv_) + dx.tail(nv_);
111		147694	}
112
113			template <typename Scalar>
114		14159	void StateMultibodyTpl<Scalar>::Jdiff(const Eigen::Ref<const VectorXs>& x0,
115			const Eigen::Ref<const VectorXs>& x1,
116			Eigen::Ref<MatrixXs> Jfirst,
117			Eigen::Ref<MatrixXs> Jsecond,
118			const Jcomponent firstsecond) const {
119	✗✓✗✗ ✗✗✗✗ ✗✗	14159	assert_pretty(
120			is_a_Jcomponent(firstsecond),
121			("firstsecond must be one of the Jcomponent {both, first, second}"));
122	✗✓	14159	if (static_cast<std::size_t>(x0.size()) != nx_) {
123			throw_pretty("Invalid argument: "
124			<< "x0 has wrong dimension (it should be " +
125			std::to_string(nx_) + ")");
126			}
127	✗✓	14159	if (static_cast<std::size_t>(x1.size()) != nx_) {
128			throw_pretty("Invalid argument: "
129			<< "x1 has wrong dimension (it should be " +
130			std::to_string(nx_) + ")");
131			}
132
133	✓✓	14159	if (firstsecond == first) {
134	✓✗✗✓	24	if (static_cast<std::size_t>(Jfirst.rows()) != ndx_ \|\|
135	✗✓	12	static_cast<std::size_t>(Jfirst.cols()) != ndx_) {
136			throw_pretty("Invalid argument: "
137			<< "Jfirst has wrong dimension (it should be " +
138			std::to_string(ndx_) + "," + std::to_string(ndx_) +
139			")");
140			}
141
142	✓✗✓✗ ✓✗	12	pinocchio::dDifference(*pinocchio_.get(), x0.head(nq_), x1.head(nq_),
143		12	Jfirst.topLeftCorner(nv_, nv_), pinocchio::ARG0);
144	✓✗✓✗ ✓✗✓✗	12	Jfirst.bottomRightCorner(nv_, nv_).diagonal().array() = (Scalar)-1;
145	✓✓	14147	} else if (firstsecond == second) {
146	✓✗✗✓	28240	if (static_cast<std::size_t>(Jsecond.rows()) != ndx_ \|\|
147	✗✓	14120	static_cast<std::size_t>(Jsecond.cols()) != ndx_) {
148			throw_pretty("Invalid argument: "
149			<< "Jsecond has wrong dimension (it should be " +
150			std::to_string(ndx_) + "," + std::to_string(ndx_) +
151			")");
152			}
153	✓✗✓✗ ✓✗	14120	pinocchio::dDifference(*pinocchio_.get(), x0.head(nq_), x1.head(nq_),
154		14120	Jsecond.topLeftCorner(nv_, nv_), pinocchio::ARG1);
155	✓✗✓✗ ✓✗✓✗	14120	Jsecond.bottomRightCorner(nv_, nv_).diagonal().array() = (Scalar)1;
156			} else { // computing both
157	✓✗✗✓	54	if (static_cast<std::size_t>(Jfirst.rows()) != ndx_ \|\|
158	✗✓	27	static_cast<std::size_t>(Jfirst.cols()) != ndx_) {
159			throw_pretty("Invalid argument: "
160			<< "Jfirst has wrong dimension (it should be " +
161			std::to_string(ndx_) + "," + std::to_string(ndx_) +
162			")");
163			}
164	✓✗✗✓	54	if (static_cast<std::size_t>(Jsecond.rows()) != ndx_ \|\|
165	✗✓	27	static_cast<std::size_t>(Jsecond.cols()) != ndx_) {
166			throw_pretty("Invalid argument: "
167			<< "Jsecond has wrong dimension (it should be " +
168			std::to_string(ndx_) + "," + std::to_string(ndx_) +
169			")");
170			}
171	✓✗✓✗ ✓✗	27	pinocchio::dDifference(*pinocchio_.get(), x0.head(nq_), x1.head(nq_),
172		27	Jfirst.topLeftCorner(nv_, nv_), pinocchio::ARG0);
173	✓✗✓✗ ✓✗	27	pinocchio::dDifference(*pinocchio_.get(), x0.head(nq_), x1.head(nq_),
174		27	Jsecond.topLeftCorner(nv_, nv_), pinocchio::ARG1);
175	✓✗✓✗ ✓✗✓✗	27	Jfirst.bottomRightCorner(nv_, nv_).diagonal().array() = (Scalar)-1;
176	✓✗✓✗ ✓✗✓✗	27	Jsecond.bottomRightCorner(nv_, nv_).diagonal().array() = (Scalar)1;
177			}
178		14159	}
179
180			template <typename Scalar>
181		13263	void StateMultibodyTpl<Scalar>::Jintegrate(const Eigen::Ref<const VectorXs>& x,
182			const Eigen::Ref<const VectorXs>& dx,
183			Eigen::Ref<MatrixXs> Jfirst,
184			Eigen::Ref<MatrixXs> Jsecond,
185			const Jcomponent firstsecond,
186			const AssignmentOp op) const {
187	✗✓✗✗ ✗✗✗✗ ✗✗	13263	assert_pretty(
188			is_a_Jcomponent(firstsecond),
189			("firstsecond must be one of the Jcomponent {both, first, second}"));
190	✗✓✗✗ ✗✗✗✗ ✗✗	13263	assert_pretty(is_a_AssignmentOp(op),
191			("op must be one of the AssignmentOp {settop, addto, rmfrom}"));
192	✓✓✓✓	13263	if (firstsecond == first \|\| firstsecond == both) {
193	✓✗✗✓	26514	if (static_cast<std::size_t>(Jfirst.rows()) != ndx_ \|\|
194	✗✓	13257	static_cast<std::size_t>(Jfirst.cols()) != ndx_) {
195			throw_pretty("Invalid argument: "
196			<< "Jfirst has wrong dimension (it should be " +
197			std::to_string(ndx_) + "," + std::to_string(ndx_) +
198			")");
199			}
200	✓✓✗✗	13257	switch (op) {
201		194	case setto:
202	✓✗✓✗ ✓✗	194	pinocchio::dIntegrate(*pinocchio_.get(), x.head(nq_), dx.head(nv_),
203		194	Jfirst.topLeftCorner(nv_, nv_), pinocchio::ARG0,
204			pinocchio::SETTO);
205	✓✗✓✗ ✓✗✓✗	194	Jfirst.bottomRightCorner(nv_, nv_).diagonal().array() = (Scalar)1;
206		194	break;
207		13063	case addto:
208	✓✗✓✗ ✓✗	13063	pinocchio::dIntegrate(*pinocchio_.get(), x.head(nq_), dx.head(nv_),
209		13063	Jfirst.topLeftCorner(nv_, nv_), pinocchio::ARG0,
210			pinocchio::ADDTO);
211	✓✗✓✗ ✓✗✓✗	13063	Jfirst.bottomRightCorner(nv_, nv_).diagonal().array() += (Scalar)1;
212		13063	break;
213			case rmfrom:
214			pinocchio::dIntegrate(*pinocchio_.get(), x.head(nq_), dx.head(nv_),
215			Jfirst.topLeftCorner(nv_, nv_), pinocchio::ARG0,
216			pinocchio::RMTO);
217			Jfirst.bottomRightCorner(nv_, nv_).diagonal().array() -= (Scalar)1;
218			break;
219			default:
220			throw_pretty(
221			"Invalid argument: allowed operators: setto, addto, rmfrom");
222			break;
223			}
224			}
225	✓✓✓✓	13263	if (firstsecond == second \|\| firstsecond == both) {
226	✓✗✗✓	388	if (static_cast<std::size_t>(Jsecond.rows()) != ndx_ \|\|
227	✗✓	194	static_cast<std::size_t>(Jsecond.cols()) != ndx_) {
228			throw_pretty("Invalid argument: "
229			<< "Jsecond has wrong dimension (it should be " +
230			std::to_string(ndx_) + "," + std::to_string(ndx_) +
231			")");
232			}
233	✓✗✗✗	194	switch (op) {
234		194	case setto:
235	✓✗✓✗ ✓✗	194	pinocchio::dIntegrate(*pinocchio_.get(), x.head(nq_), dx.head(nv_),
236		194	Jsecond.topLeftCorner(nv_, nv_), pinocchio::ARG1,
237			pinocchio::SETTO);
238	✓✗✓✗ ✓✗✓✗	194	Jsecond.bottomRightCorner(nv_, nv_).diagonal().array() = (Scalar)1;
239		194	break;
240			case addto:
241			pinocchio::dIntegrate(*pinocchio_.get(), x.head(nq_), dx.head(nv_),
242			Jsecond.topLeftCorner(nv_, nv_), pinocchio::ARG1,
243			pinocchio::ADDTO);
244			Jsecond.bottomRightCorner(nv_, nv_).diagonal().array() += (Scalar)1;
245			break;
246			case rmfrom:
247			pinocchio::dIntegrate(*pinocchio_.get(), x.head(nq_), dx.head(nv_),
248			Jsecond.topLeftCorner(nv_, nv_), pinocchio::ARG1,
249			pinocchio::RMTO);
250			Jsecond.bottomRightCorner(nv_, nv_).diagonal().array() -= (Scalar)1;
251			break;
252			default:
253			throw_pretty(
254			"Invalid argument: allowed operators: setto, addto, rmfrom");
255			break;
256			}
257			}
258		13263	}
259
260			template <typename Scalar>
261		26138	void StateMultibodyTpl<Scalar>::JintegrateTransport(
262			const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& dx,
263			Eigen::Ref<MatrixXs> Jin, const Jcomponent firstsecond) const {
264	✗✓✗✗ ✗✗✗✗ ✗✗	26138	assert_pretty(
265			is_a_Jcomponent(firstsecond),
266			("firstsecond must be one of the Jcomponent {both, first, second}"));
267
268	✓✓✗	26138	switch (firstsecond) {
269		6	case first:
270	✓✗✓✗	6	pinocchio::dIntegrateTransport(*pinocchio_.get(), x.head(nq_),
271	✓✗	6	dx.head(nv_), Jin.topRows(nv_),
272			pinocchio::ARG0);
273		6	break;
274		26132	case second:
275	✓✗✓✗	26132	pinocchio::dIntegrateTransport(*pinocchio_.get(), x.head(nq_),
276	✓✗	26132	dx.head(nv_), Jin.topRows(nv_),
277			pinocchio::ARG1);
278		26132	break;
279			default:
280			throw_pretty(
281			"Invalid argument: firstsecond must be either first or second. both "
282			"not supported for this operation.");
283			break;
284			}
285		26138	}
286
287			template <typename Scalar>
288			const boost::shared_ptr<pinocchio::ModelTpl<Scalar> >&
289		1516411	StateMultibodyTpl<Scalar>::get_pinocchio() const {
290		1516411	return pinocchio_;
291			}
292
293			} // namespace crocoddyl