| Directory: | ./ |
|---|---|
| File: | src/relative-com.cc |
| Date: | 2025-05-05 12:19:30 |
| Exec | Total | Coverage | |
|---|---|---|---|
| Lines: | 40 | 84 | 47.6% |
| Branches: | 46 | 252 | 18.3% |
| Line | Branch | Exec | Source |
|---|---|---|---|
| 1 | // | ||
| 2 | // Copyright (c) 2014 CNRS | ||
| 3 | // Authors: Florent Lamiraux | ||
| 4 | // | ||
| 5 | // | ||
| 6 | |||
| 7 | // Redistribution and use in source and binary forms, with or without | ||
| 8 | // modification, are permitted provided that the following conditions are | ||
| 9 | // met: | ||
| 10 | // | ||
| 11 | // 1. Redistributions of source code must retain the above copyright | ||
| 12 | // notice, this list of conditions and the following disclaimer. | ||
| 13 | // | ||
| 14 | // 2. Redistributions in binary form must reproduce the above copyright | ||
| 15 | // notice, this list of conditions and the following disclaimer in the | ||
| 16 | // documentation and/or other materials provided with the distribution. | ||
| 17 | // | ||
| 18 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS | ||
| 19 | // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT | ||
| 20 | // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR | ||
| 21 | // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT | ||
| 22 | // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, | ||
| 23 | // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT | ||
| 24 | // LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, | ||
| 25 | // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY | ||
| 26 | // THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT | ||
| 27 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE | ||
| 28 | // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH | ||
| 29 | // DAMAGE. | ||
| 30 | |||
| 31 | #include <boost/serialization/vector.hpp> | ||
| 32 | #include <hpp/constraints/macros.hh> | ||
| 33 | #include <hpp/constraints/relative-com.hh> | ||
| 34 | #include <hpp/pinocchio/center-of-mass-computation.hh> | ||
| 35 | #include <hpp/pinocchio/configuration.hh> | ||
| 36 | #include <hpp/pinocchio/device.hh> | ||
| 37 | #include <hpp/pinocchio/joint.hh> | ||
| 38 | #include <hpp/pinocchio/liegroup-element.hh> | ||
| 39 | #include <hpp/pinocchio/serialization.hh> | ||
| 40 | #include <hpp/pinocchio/util.hh> | ||
| 41 | #include <hpp/util/debug.hh> | ||
| 42 | #include <hpp/util/indent.hh> | ||
| 43 | #include <hpp/util/serialization.hh> | ||
| 44 | #include <pinocchio/serialization/eigen.hpp> | ||
| 45 | |||
| 46 | namespace hpp { | ||
| 47 | namespace constraints { | ||
| 48 | |||
| 49 | namespace { | ||
| 50 | 1 | static size_type size(std::vector<bool> mask) { | |
| 51 | 1 | size_type res = 0; | |
| 52 |
2/2✓ Branch 4 taken 3 times.
✓ Branch 5 taken 1 times.
|
4 | for (std::vector<bool>::iterator it = mask.begin(); it != mask.end(); ++it) |
| 53 |
1/2✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
|
3 | if (*it) ++res; |
| 54 | 1 | return res; | |
| 55 | } | ||
| 56 | } // namespace | ||
| 57 | |||
| 58 | 1 | RelativeComPtr_t RelativeCom::create(const std::string& name, | |
| 59 | const DevicePtr_t& robot, | ||
| 60 | const JointPtr_t& joint, | ||
| 61 | const vector3_t reference, | ||
| 62 | std::vector<bool> mask) { | ||
| 63 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | CenterOfMassComputationPtr_t comc = CenterOfMassComputation::create(robot); |
| 64 |
2/4✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
|
1 | comc->add(robot->rootJoint()); |
| 65 |
3/6✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
|
2 | return create(name, robot, comc, joint, reference, mask); |
| 66 | 1 | } | |
| 67 | |||
| 68 | ✗ | RelativeComPtr_t RelativeCom::create(const DevicePtr_t& robot, | |
| 69 | const CenterOfMassComputationPtr_t& comc, | ||
| 70 | const JointPtr_t& joint, | ||
| 71 | const vector3_t reference, | ||
| 72 | std::vector<bool> mask) { | ||
| 73 | ✗ | return create("RelativeCom", robot, comc, joint, reference, mask); | |
| 74 | } | ||
| 75 | |||
| 76 | 1 | RelativeComPtr_t RelativeCom::create(const std::string& name, | |
| 77 | const DevicePtr_t& robot, | ||
| 78 | const CenterOfMassComputationPtr_t& comc, | ||
| 79 | const JointPtr_t& joint, | ||
| 80 | const vector3_t reference, | ||
| 81 | std::vector<bool> mask) { | ||
| 82 |
3/6✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
|
1 | RelativeCom* ptr = new RelativeCom(robot, comc, joint, reference, mask, name); |
| 83 | 1 | RelativeComPtr_t shPtr(ptr); | |
| 84 | 1 | return shPtr; | |
| 85 | } | ||
| 86 | |||
| 87 | 1 | RelativeCom::RelativeCom(const DevicePtr_t& robot, | |
| 88 | const CenterOfMassComputationPtr_t& comc, | ||
| 89 | const JointPtr_t& joint, const vector3_t reference, | ||
| 90 | 1 | std::vector<bool> mask, const std::string& name) | |
| 91 | : DifferentiableFunction(robot->configSize(), robot->numberDof(), | ||
| 92 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
|
2 | LiegroupSpace::Rn(size(mask)), name), |
| 93 | 1 | robot_(robot), | |
| 94 | 1 | comc_(comc), | |
| 95 | 1 | joint_(joint), | |
| 96 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | reference_(reference), |
| 97 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | mask_(mask), |
| 98 | 1 | nominalCase_(false), | |
| 99 |
5/10✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
✓ Branch 22 taken 1 times.
✗ Branch 23 not taken.
|
4 | jacobian_(3, robot->numberDof() - robot->extraConfigSpace().dimension()) { |
| 100 |
7/14✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
|
1 | if (mask[0] && mask[1] && mask[2]) nominalCase_ = true; |
| 101 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | jacobian_.setZero(); |
| 102 | 1 | } | |
| 103 | |||
| 104 | ✗ | std::ostream& RelativeCom::print(std::ostream& o) const { | |
| 105 | ✗ | return o << "RelativeCom: " << name() << incindent << iendl | |
| 106 | ✗ | << "Joint: " << (joint_ ? joint_->name() : "World") << iendl | |
| 107 | ✗ | << "Reference: " << one_line(reference_) << iendl << "mask: "; | |
| 108 | for (size_type i = 0; i < 3; ++i) o << mask_[i] << ", "; | ||
| 109 | return o << decindent; | ||
| 110 | } | ||
| 111 | |||
| 112 | 2 | void RelativeCom::impl_compute(LiegroupElementRef result, | |
| 113 | ConfigurationIn_t argument) const { | ||
| 114 |
1/2✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
2 | pinocchio::DeviceSync device(robot_); |
| 115 |
2/4✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
|
2 | device.currentConfiguration(argument); |
| 116 |
1/2✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
2 | device.computeForwardKinematics(pinocchio::JOINT_POSITION); |
| 117 |
2/4✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
|
2 | comc_->compute(device.d(), hpp::pinocchio::COM); |
| 118 |
2/4✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
|
2 | const Transform3s& M = joint_->currentTransformation(device.d()); |
| 119 |
1/2✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
|
2 | const vector3_t& x = comc_->com(device.d()); |
| 120 |
1/2✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
2 | const matrix3_t& R = M.rotation(); |
| 121 |
1/2✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
2 | const vector3_t& t = M.translation(); |
| 122 | |||
| 123 |
1/2✓ Branch 0 taken 2 times.
✗ Branch 1 not taken.
|
2 | if (nominalCase_) |
| 124 |
5/10✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 2 times.
✗ Branch 11 not taken.
✓ Branch 14 taken 2 times.
✗ Branch 15 not taken.
|
2 | result.vector() = R.transpose() * (x - t) - reference_; |
| 125 | else { | ||
| 126 | ✗ | const vector3_t res(R.transpose() * (x - t) - reference_); | |
| 127 | ✗ | size_t index = 0; | |
| 128 | ✗ | for (size_t i = 0; i < 3; ++i) | |
| 129 | ✗ | if (mask_[i]) { | |
| 130 | ✗ | result.vector()[index] = res[i]; | |
| 131 | ✗ | index++; | |
| 132 | } | ||
| 133 | } | ||
| 134 | 2 | } | |
| 135 | |||
| 136 | ✗ | void RelativeCom::impl_jacobian(matrixOut_t jacobian, | |
| 137 | ConfigurationIn_t arg) const { | ||
| 138 | ✗ | pinocchio::DeviceSync device(robot_); | |
| 139 | ✗ | device.currentConfiguration(arg); | |
| 140 | ✗ | device.computeForwardKinematics(pinocchio::JOINT_POSITION | | |
| 141 | pinocchio::JACOBIAN); | ||
| 142 | ✗ | comc_->compute(device.d(), hpp::pinocchio::COMPUTE_ALL); | |
| 143 | ✗ | const ComJacobian_t& Jcom = comc_->jacobian(device.d()); | |
| 144 | ✗ | const JointJacobian_t& Jjoint(joint_->jacobian(device.d())); | |
| 145 | ✗ | const Transform3s& M = joint_->currentTransformation(device.d()); | |
| 146 | ✗ | const matrix3_t& R(M.rotation()); | |
| 147 | ✗ | const vector3_t& x(comc_->com(device.d())); | |
| 148 | ✗ | const vector3_t& t(M.translation()); | |
| 149 | |||
| 150 | // Right part | ||
| 151 | ✗ | jacobian.rightCols(jacobian.cols() - Jjoint.cols()).setZero(); | |
| 152 | // Left part | ||
| 153 | // J = 0RTj ( Jcom + [ x - 0tj ]x 0Rj jJwj - 0Rj jJtj) | ||
| 154 | ✗ | jacobian_ = R.transpose() * Jcom; | |
| 155 | ✗ | jacobian_.noalias() += | |
| 156 | ✗ | (R.transpose() * R.colwise().cross(t - x)) * Jjoint.bottomRows<3>(); | |
| 157 | |||
| 158 | ✗ | if (nominalCase_) { | |
| 159 | ✗ | jacobian.leftCols(Jjoint.cols()).noalias() = | |
| 160 | ✗ | jacobian_ - Jjoint.topRows<3>(); | |
| 161 | } else { | ||
| 162 | ✗ | size_t index = 0; | |
| 163 | ✗ | for (size_t i = 0; i < 3; ++i) | |
| 164 | ✗ | if (mask_[i]) { | |
| 165 | ✗ | jacobian.row(index).head(Jjoint.cols()) = | |
| 166 | ✗ | jacobian_.row(i) - Jjoint.row(i); | |
| 167 | ✗ | index++; | |
| 168 | } | ||
| 169 | } | ||
| 170 | hppDnum(info, "Jcom = " << std::endl << Jcom); | ||
| 171 | hppDnum(info, "Jw = " << std::endl << Jjoint.bottomRows<3>()); | ||
| 172 | hppDnum(info, "Jv = " << std::endl << Jjoint.topRows<3>()); | ||
| 173 | } | ||
| 174 | |||
| 175 | template <class Archive> | ||
| 176 | ✗ | void RelativeCom::serialize(Archive& ar, const unsigned int version) { | |
| 177 | using namespace boost::serialization; | ||
| 178 | (void)version; | ||
| 179 | ✗ | ar& make_nvp("base", base_object<DifferentiableFunction>(*this)); | |
| 180 | ✗ | ar& BOOST_SERIALIZATION_NVP(robot_); | |
| 181 | ✗ | ar& BOOST_SERIALIZATION_NVP(comc_); | |
| 182 | ✗ | ar& BOOST_SERIALIZATION_NVP(joint_); | |
| 183 | ✗ | ar& BOOST_SERIALIZATION_NVP(reference_); | |
| 184 | ✗ | ar& BOOST_SERIALIZATION_NVP(mask_); | |
| 185 | if (!Archive::is_saving::value) | ||
| 186 | ✗ | nominalCase_ = (mask_[0] && mask_[1] && mask_[2]); | |
| 187 | } | ||
| 188 | |||
| 189 | HPP_SERIALIZATION_IMPLEMENT(RelativeCom); | ||
| 190 | } // namespace constraints | ||
| 191 | } // namespace hpp | ||
| 192 | |||
| 193 | 12 | BOOST_CLASS_EXPORT(hpp::constraints::RelativeCom) | |
| 194 |