| Directory: | ./ |
|---|---|
| File: | src/device-sync.cc |
| Date: | 2025-05-04 12:09:19 |
| Exec | Total | Coverage | |
|---|---|---|---|
| Lines: | 28 | 47 | 59.6% |
| Branches: | 13 | 28 | 46.4% |
| Line | Branch | Exec | Source |
|---|---|---|---|
| 1 | // | ||
| 2 | // Copyright (c) 2016 CNRS | ||
| 3 | // Author: NMansard | ||
| 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/thread/locks.hpp> | ||
| 32 | #include <hpp/pinocchio/device-sync.hh> | ||
| 33 | #include <hpp/pinocchio/device.hh> | ||
| 34 | #include <hpp/pinocchio/joint-collection.hh> | ||
| 35 | #include <pinocchio/multibody/data.hpp> | ||
| 36 | #include <pinocchio/multibody/geometry.hpp> | ||
| 37 | #include <pinocchio/multibody/model.hpp> | ||
| 38 | |||
| 39 | namespace hpp { | ||
| 40 | namespace pinocchio { | ||
| 41 | 2011 | bool AbstractDevice::currentConfiguration(ConfigurationIn_t configuration) { | |
| 42 | 2011 | DeviceData& data = d(); | |
| 43 |
1/2✗ Branch 2 not taken.
✓ Branch 3 taken 2011 times.
|
2011 | assert(configuration.size() == data.currentConfiguration_.size()); |
| 44 |
2/2✓ Branch 1 taken 1997 times.
✓ Branch 2 taken 11 times.
|
2011 | if (configuration != data.currentConfiguration_) { |
| 45 | 1997 | data.invalidate(); | |
| 46 | 1996 | data.currentConfiguration_ = configuration; | |
| 47 | 1999 | return true; | |
| 48 | } | ||
| 49 | 11 | return false; | |
| 50 | } | ||
| 51 | |||
| 52 | ✗ | bool AbstractDevice::currentVelocity(vectorIn_t v) { | |
| 53 | ✗ | DeviceData& data = d(); | |
| 54 | ✗ | if (v != data.currentVelocity_) { | |
| 55 | ✗ | data.invalidate(); | |
| 56 | ✗ | data.currentVelocity_ = v; | |
| 57 | ✗ | return true; | |
| 58 | } | ||
| 59 | ✗ | return false; | |
| 60 | } | ||
| 61 | |||
| 62 | ✗ | bool AbstractDevice::currentAcceleration(vectorIn_t a) { | |
| 63 | ✗ | DeviceData& data = d(); | |
| 64 | ✗ | if (a != data.currentAcceleration_) { | |
| 65 | ✗ | data.invalidate(); | |
| 66 | ✗ | data.currentAcceleration_ = a; | |
| 67 | ✗ | return true; | |
| 68 | } | ||
| 69 | ✗ | return false; | |
| 70 | } | ||
| 71 | |||
| 72 | ✗ | const value_type& AbstractDevice::mass() const { return data().mass[0]; } | |
| 73 | |||
| 74 | ✗ | const vector3_t& AbstractDevice::positionCenterOfMass() const { | |
| 75 | ✗ | return data().com[0]; | |
| 76 | } | ||
| 77 | |||
| 78 | ✗ | const ComJacobian_t& AbstractDevice::jacobianCenterOfMass() const { | |
| 79 | ✗ | return data().Jcom; | |
| 80 | } | ||
| 81 | |||
| 82 | 31 | AbstractDevice::AbstractDevice() | |
| 83 |
4/8✓ Branch 2 taken 31 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 31 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 31 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 31 times.
✗ Branch 13 not taken.
|
31 | : model_(new Model()), geomModel_(new GeomModel()) {} |
| 84 | |||
| 85 | 1005 | AbstractDevice::AbstractDevice(const ModelPtr_t& m, const GeomModelPtr_t& gm) | |
| 86 | 1005 | : model_(m), geomModel_(gm) {} | |
| 87 | |||
| 88 | 1003 | DeviceSync::DeviceSync(const DevicePtr_t& d, bool acquireLock) | |
| 89 |
1/2✓ Branch 5 taken 1006 times.
✗ Branch 6 not taken.
|
1003 | : AbstractDevice(d->modelPtr(), d->geomModelPtr()), device_(d), d_(NULL) { |
| 90 |
2/4✓ Branch 0 taken 1004 times.
✗ Branch 1 not taken.
✓ Branch 3 taken 1006 times.
✗ Branch 4 not taken.
|
1004 | if (acquireLock) lock(); |
| 91 | 1006 | } | |
| 92 | |||
| 93 | 2008 | DeviceSync::~DeviceSync() { | |
| 94 |
1/2✓ Branch 1 taken 1001 times.
✗ Branch 2 not taken.
|
1996 | if (isLocked()) unlock(); |
| 95 | 2018 | } | |
| 96 | |||
| 97 | 1004 | void DeviceSync::lock() { | |
| 98 |
1/2✓ Branch 1 taken 1003 times.
✗ Branch 2 not taken.
|
1004 | if (!isLocked()) { |
| 99 | 1003 | d_ = device_->datas_.acquire(); | |
| 100 | } else { | ||
| 101 | hppDout(warning, | ||
| 102 | "Cannot lock a locked DeviceSync. You may a concurrency error."); | ||
| 103 | } | ||
| 104 | 1006 | } | |
| 105 | |||
| 106 | 987 | void DeviceSync::unlock() { | |
| 107 |
1/2✓ Branch 1 taken 993 times.
✗ Branch 2 not taken.
|
987 | if (isLocked()) { |
| 108 | 993 | device_->datas_.release(d_); | |
| 109 | 1006 | d_ = NULL; | |
| 110 | } else { | ||
| 111 | hppDout( | ||
| 112 | warning, | ||
| 113 | "Cannot unlock an unlocked DeviceSync. You may a concurrency error."); | ||
| 114 | } | ||
| 115 | 1004 | } | |
| 116 | } // namespace pinocchio | ||
| 117 | } // namespace hpp | ||
| 118 |