GCC Code Coverage Report

Directory: ./
File: include/crocoddyl/multibody/residuals/pair-collision.hxx
Date: 2025-03-13 15:22:42
Exec Total Coverage
Lines: 33 36 91.7%
Branches: 19 58 32.8%
Line Branch Exec Source
1 ///////////////////////////////////////////////////////////////////////////////
2 // BSD 3-Clause License
3 //
4 // Copyright (C) 2021-2022, LAAS-CNRS, University of Edinburgh, INRIA
5 // Copyright note valid unless otherwise stated in individual files.
6 // All rights reserved.
7 ///////////////////////////////////////////////////////////////////////////////
8
9 #ifdef PINOCCHIO_WITH_HPP_FCL
10
11 #include <pinocchio/algorithm/geometry.hpp>
12 #include <pinocchio/algorithm/jacobian.hpp>
13 #include <pinocchio/multibody/fcl.hpp>
14
15 #include "crocoddyl/core/utils/exception.hpp"
16
17 namespace crocoddyl {
18
19 template <typename Scalar>
20 64 ResidualModelPairCollisionTpl<Scalar>::ResidualModelPairCollisionTpl(
21 std::shared_ptr<StateMultibody> state, const std::size_t nu,
22 std::shared_ptr<GeometryModel> geom_model,
23 const pinocchio::PairIndex pair_id, const pinocchio::JointIndex joint_id)
24 : Base(state, 3, nu, true, false, false),
25
1/2
✓ Branch 3 taken 64 times.
✗ Branch 4 not taken.
 64 pin_model_(*state->get_pinocchio()),
26 64 geom_model_(geom_model),
27 64 pair_id_(pair_id),
28
1/2
✓ Branch 2 taken 64 times.
✗ Branch 3 not taken.
 64 joint_id_(joint_id) {
29
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 64 times.
 64 if (static_cast<pinocchio::FrameIndex>(geom_model->collisionPairs.size()) <=
30 pair_id) {
31 ✗ throw_pretty(
32 "Invalid argument: "
33 << "the pair index is wrong (it does not exist in the geometry model)");
34 }
35
1/2
✗ Branch 3 not taken.
✓ Branch 4 taken 64 times.
 64 if (static_cast<pinocchio::FrameIndex>(state->get_pinocchio()->njoints) <=
36 joint_id) {
37 ✗ throw_pretty(
38 "Invalid argument: "
39 << "the joint index is wrong (it does not exist in the robot)");
40 }
41 64 }
42
43 template <typename Scalar>
44 128 ResidualModelPairCollisionTpl<Scalar>::~ResidualModelPairCollisionTpl() {}
45
46 template <typename Scalar>
47 2234 void ResidualModelPairCollisionTpl<Scalar>::calc(
48 const std::shared_ptr<ResidualDataAbstract> &data,
49 const Eigen::Ref<const VectorXs> &x, const Eigen::Ref<const VectorXs> &) {
50 2234 Data *d = static_cast<Data *>(data.get());
51
52 const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> q =
53
1/2
✓ Branch 3 taken 2234 times.
✗ Branch 4 not taken.
 2234 x.head(state_->get_nq());
54
55 // computes the distance for the collision pair pair_id_
56
1/2
✓ Branch 1 taken 2234 times.
✗ Branch 2 not taken.
 2234 pinocchio::updateGeometryPlacements(pin_model_, *d->pinocchio,
57 2234 *geom_model_.get(), d->geometry, q);
58
1/2
✓ Branch 2 taken 2234 times.
✗ Branch 3 not taken.
 2234 pinocchio::computeDistance(*geom_model_.get(), d->geometry, pair_id_);
59
60 // calculate residual
61
2/4
✓ Branch 3 taken 2234 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 2234 times.
✗ Branch 8 not taken.
 2234 data->r = d->geometry.distanceResults[pair_id_].nearest_points[0] -
62 2234 d->geometry.distanceResults[pair_id_].nearest_points[1];
63 2234 }
64
65 template <typename Scalar>
66 42 void ResidualModelPairCollisionTpl<Scalar>::calcDiff(
67 const std::shared_ptr<ResidualDataAbstract> &data,
68 const Eigen::Ref<const VectorXs> &, const Eigen::Ref<const VectorXs> &) {
69 42 Data *d = static_cast<Data *>(data.get());
70
71 42 const std::size_t nv = state_->get_nv();
72
73 // calculate the vector from the joint jointId to the collision p1, expressed
74 // in world frame
75
1/2
✓ Branch 3 taken 42 times.
✗ Branch 4 not taken.
 42 d->d = d->geometry.distanceResults[pair_id_].nearest_points[0] -
76 42 d->pinocchio->oMi[joint_id_].translation();
77 42 pinocchio::getJointJacobian(pin_model_, *d->pinocchio, joint_id_,
78 42 pinocchio::LOCAL_WORLD_ALIGNED, d->J);
79
80 // calculate the Jacobian at p1
81
3/6
✓ Branch 1 taken 42 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 42 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 42 times.
✗ Branch 8 not taken.
 42 d->J.template topRows<3>().noalias() +=
82
3/6
✓ Branch 2 taken 42 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 42 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 42 times.
✗ Branch 9 not taken.
 42 pinocchio::skew(d->d).transpose() * d->J.template bottomRows<3>();
83
84 // compute the residual derivatives
85
2/4
✓ Branch 2 taken 42 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 42 times.
✗ Branch 6 not taken.
 42 d->Rx.topLeftCorner(3, nv) = d->J.template topRows<3>();
86 42 }
87
88 template <typename Scalar>
89 std::shared_ptr<ResidualDataAbstractTpl<Scalar> >
90 2716 ResidualModelPairCollisionTpl<Scalar>::createData(
91 DataCollectorAbstract *const data) {
92 ✗ return std::allocate_shared<Data>(Eigen::aligned_allocator<Data>(), this,
93
1/2
✓ Branch 2 taken 2716 times.
✗ Branch 3 not taken.
 2716 data);
94 }
95
96 template <typename Scalar>
97 const pinocchio::GeometryModel &
98 2716 ResidualModelPairCollisionTpl<Scalar>::get_geometry() const {
99 2716 return *geom_model_.get();
100 }
101
102 } // namespace crocoddyl
103
104 #endif // PINOCCHIO_WITH_HPP_FCL
105