From 7ef67d73c664b6ef0bbacdbfe5592f2c0373d9e3 Mon Sep 17 00:00:00 2001 From: Devansh Date: Wed, 23 Jun 2021 15:41:42 -0400 Subject: [PATCH 01/18] Added integration tests for FT sensor (without noise model) --- include/ignition/sensors/ForceTorqueSensor.hh | 110 +++++++ src/CMakeLists.txt | 3 + src/ForceTorqueSensor.cc | 219 +++++++++++++ test/integration/CMakeLists.txt | 2 + test/integration/force_torque_plugin.cc | 310 ++++++++++++++++++ 5 files changed, 644 insertions(+) create mode 100644 include/ignition/sensors/ForceTorqueSensor.hh create mode 100644 src/ForceTorqueSensor.cc create mode 100644 test/integration/force_torque_plugin.cc diff --git a/include/ignition/sensors/ForceTorqueSensor.hh b/include/ignition/sensors/ForceTorqueSensor.hh new file mode 100644 index 00000000..c4d71da8 --- /dev/null +++ b/include/ignition/sensors/ForceTorqueSensor.hh @@ -0,0 +1,110 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#ifndef IGNITION_SENSORS_FORCETORQUESENSOR_HH_ +#define IGNITION_SENSORS_FORCETORQUESENSOR_HH_ + +#include + +#include + +#include +#include +#include + +#include + +#include + +#include "ignition/sensors/Sensor.hh" + +namespace ignition +{ + namespace sensors + { + // Inline bracket to help doxygen filtering. + inline namespace IGNITION_SENSORS_VERSION_NAMESPACE { + // + /// \brief forward declarations + class ForceTorqueSensorPrivate; + + /// \brief Force Torque Sensor Class + /// + /// A force-torque Sensor that reports force and torque applied on a joint. + + class IGNITION_SENSORS_FORCE_TORQUE_VISIBLE ForceTorqueSensor : public Sensor + { + /// \brief constructor + public: ForceTorqueSensor(); + + /// \brief destructor + public: virtual ~ForceTorqueSensor(); + + /// \brief Load the sensor based on data from an sdf::Sensor object. + /// \param[in] _sdf SDF Sensor parameters. + /// \return true if loading was successful + public: virtual bool Load(const sdf::Sensor &_sdf) override; + + /// \brief Load the sensor with SDF parameters. + /// \param[in] _sdf SDF Sensor parameters. + /// \return true if loading was successful + public: virtual bool Load(sdf::ElementPtr _sdf) override; + + /// \brief Initialize values in the sensor + /// \return True on success + public: virtual bool Init() override; + + /// \brief Update the sensor and generate data + /// \param[in] _now The current time + /// \return true if the update was successfull + public: virtual bool IGN_DEPRECATED(4) Update( + const ignition::common::Time &_now) override; + + /// \brief Update the sensor and generate data + /// \param[in] _now The current time + /// \return true if the update was successfull + public: virtual bool Update( + const std::chrono::steady_clock::duration &_now) override; + + /// \brief Get the current joint force. + /// \return The latested measured force. + public: math::Vector3d Force() const; + + /// \brief Set the force vector. + /// \param[in] _force force vector in newton. + public: void SetForce(const math::Vector3d &_force); + + /// \brief Get the current joint torque. + /// \return The latested measured torque. + public: math::Vector3d Torque() const; + + /// \brief Set the torque vector. + /// \param[in] _torque torque vector in newton. + public: void SetTorque(const math::Vector3d &_torque); + + + IGN_COMMON_WARN_IGNORE__DLL_INTERFACE_MISSING + /// \brief Data pointer for private data + /// \internal + private: std::unique_ptr dataPtr; + IGN_COMMON_WARN_RESUME__DLL_INTERFACE_MISSING + }; + } + } +} + +#endif diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 1eb3bb4a..3b53c21c 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -111,6 +111,9 @@ ign_add_component(magnetometer SOURCES ${magnetometer_sources} GET_TARGET_NAME m set(imu_sources ImuSensor.cc) ign_add_component(imu SOURCES ${imu_sources} GET_TARGET_NAME imu_target) +set(force_torque_sources ForceTorqueSensor.cc) +ign_add_component(force_torque SOURCES ${force_torque_sources} GET_TARGET_NAME force_torque_target) + set(altimeter_sources AltimeterSensor.cc) ign_add_component(altimeter SOURCES ${altimeter_sources} GET_TARGET_NAME altimeter_target) diff --git a/src/ForceTorqueSensor.cc b/src/ForceTorqueSensor.cc new file mode 100644 index 00000000..038aea49 --- /dev/null +++ b/src/ForceTorqueSensor.cc @@ -0,0 +1,219 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#ifdef _WIN32 +#pragma warning(push) +#pragma warning(disable: 4005) +#pragma warning(disable: 4251) +#endif +#include +#ifdef _WIN32 +#pragma warning(pop) +#endif + +#include +#include + +#include "ignition/sensors/ForceTorqueSensor.hh" +#include "ignition/sensors/Noise.hh" +#include "ignition/sensors/SensorFactory.hh" +#include "ignition/sensors/SensorTypes.hh" + +using namespace ignition; +using namespace sensors; + +/// \brief Private data for ForceTorqueSensor +class ignition::sensors::ForceTorqueSensorPrivate +{ + /// \brief node to create publisher + public: transport::Node node; + + /// \brief publisher to publish Wrench messages. + public: transport::Node::Publisher pub; + + /// \brief true if Load() has been called and was successful + public: bool initialized = false; + + /// \brief Noise free force. + public: ignition::math::Vector3d force; + + /// \brief Noise free torque. + public: ignition::math::Vector3d torque; + + /// \brief Flag for if time has been initialized + public: bool timeInitialized = false; + + /// \brief Previous update time step. + public: std::chrono::steady_clock::duration prevStep + {std::chrono::steady_clock::duration::zero()}; + +}; + +////////////////////////////////////////////////// +ForceTorqueSensor::ForceTorqueSensor() + : dataPtr(new ForceTorqueSensorPrivate()) +{ +} + +////////////////////////////////////////////////// +ForceTorqueSensor::~ForceTorqueSensor() +{ +} + +////////////////////////////////////////////////// +bool ForceTorqueSensor::Init() +{ + return this->Sensor::Init(); +} + +////////////////////////////////////////////////// +bool ForceTorqueSensor::Load(const sdf::Sensor &_sdf) +{ + if (!Sensor::Load(_sdf)) + return false; + + // Check if this is the right type + if (_sdf.Type() != sdf::SensorType::FORCE_TORQUE) + { + ignerr << "Attempting to a load a Force Torque sensor, but received " + << "a " << _sdf.TypeStr() << std::endl; + } + + if (_sdf.ForceTorqueSensor() == nullptr) + { + ignerr << "Attempting to a load a Force Torque sensor, but received " + << "a null sensor." << std::endl; + return false; + } + + if (this->Topic().empty()) + this->SetTopic("/forcetorque"); + + this->dataPtr->pub = + this->dataPtr->node.Advertise(this->Topic()); + + if (!this->dataPtr->pub) + { + ignerr << "Unable to create publisher on topic[" << this->Topic() << "].\n"; + return false; + } + + this->dataPtr->initialized = true; + return true; +} + +////////////////////////////////////////////////// +bool ForceTorqueSensor::Load(sdf::ElementPtr _sdf) +{ + sdf::Sensor sdfSensor; + sdfSensor.Load(_sdf); + return this->Load(sdfSensor); +} + +////////////////////////////////////////////////// +bool ForceTorqueSensor::Update( + const ignition::common::Time &_now) +{ + return this->Update(math::secNsecToDuration(_now.sec, _now.nsec)); +} + +////////////////////////////////////////////////// +bool ForceTorqueSensor::Update(const std::chrono::steady_clock::duration &_now) +{ + IGN_PROFILE("ForceTorqueSensor::Update"); + if (!this->dataPtr->initialized) + { + ignerr << "Not initialized, update ignored.\n"; + return false; + } + + // If time has gone backwards, reinitialize. + if (_now < this->dataPtr->prevStep) + { + this->dataPtr->timeInitialized = false; + } + + // Only compute dt if time is initialized and increasing. + double dt; + if (this->dataPtr->timeInitialized) + { + auto delay = std::chrono::duration_cast>( + _now - this->dataPtr->prevStep); + dt = delay.count(); + } + else + { + dt = 0.0; + } + + msgs::Wrench msg; + *msg.mutable_header()->mutable_stamp() = msgs::Convert(_now); + auto frame = msg.mutable_header()->add_data(); + frame->set_key("frame_id"); + frame->add_value(this->Name()); + + /* + TODO + - Check frame orientation + - Apply noises. + */ + + this->dataPtr->force.X() = 0.1; + this->dataPtr->force.Y() = 0.1; + this->dataPtr->force.Z() = 0.1; + + this->dataPtr->torque.X() = 0.1; + this->dataPtr->torque.Y() = 0.1; + this->dataPtr->torque.Z() = 0.1; + + msgs::Set(msg.mutable_force(), this->dataPtr->force); + msgs::Set(msg.mutable_torque(), this->dataPtr->torque); + + // publish + this->AddSequence(msg.mutable_header()); + this->dataPtr->pub.Publish(msg); + this->dataPtr->prevStep = _now; + this->dataPtr->timeInitialized = true; + return true; +} + +////////////////////////////////////////////////// +math::Vector3d ForceTorqueSensor::Force() const +{ + return this->dataPtr->force; +} + +////////////////////////////////////////////////// +void ForceTorqueSensor::SetForce(const math::Vector3d &_force) +{ + this->dataPtr->force = _force; +} + +////////////////////////////////////////////////// +math::Vector3d ForceTorqueSensor::Torque() const +{ + return this->dataPtr->torque; +} + +////////////////////////////////////////////////// +void ForceTorqueSensor::SetTorque(const math::Vector3d &_torque) +{ + this->dataPtr->torque = _torque; +} + + +IGN_SENSORS_REGISTER_SENSOR(ForceTorqueSensor) diff --git a/test/integration/CMakeLists.txt b/test/integration/CMakeLists.txt index 39ee1ef6..587f381d 100644 --- a/test/integration/CMakeLists.txt +++ b/test/integration/CMakeLists.txt @@ -14,6 +14,7 @@ set(tests logical_camera.cc magnetometer.cc imu.cc + force_torque_plugin.cc ) link_directories(${PROJECT_BINARY_DIR}/test) @@ -51,5 +52,6 @@ ign_build_tests(TYPE INTEGRATION ${PROJECT_LIBRARY_TARGET_NAME}-logical_camera ${PROJECT_LIBRARY_TARGET_NAME}-magnetometer ${PROJECT_LIBRARY_TARGET_NAME}-imu + ${PROJECT_LIBRARY_TARGET_NAME}-force_torque ) diff --git a/test/integration/force_torque_plugin.cc b/test/integration/force_torque_plugin.cc new file mode 100644 index 00000000..654c87fb --- /dev/null +++ b/test/integration/force_torque_plugin.cc @@ -0,0 +1,310 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#include + +#include + +#include +#include +#include + +#include "test_config.h" // NOLINT(build/include) +#include "TransportTestTools.hh" + +/// \brief Helper function to create a force torque sdf element. +sdf::ElementPtr ForceTorqueToSdf(const std::string &_name, + const ignition::math::Pose3d &_pose, const double _updateRate, + const std::string &_topic, const bool _alwaysOn, + const bool _visualize) +{ + std::ostringstream stream; + stream + << "" + << "" + << " " + << " " + << " " + << " " << _pose << "" + << " " << _topic << "" + << " "<< _updateRate <<"" + << " " << _alwaysOn <<"" + << " " << _visualize << "" + << " " + << " " + << " " + << ""; + + sdf::SDFPtr sdfParsed(new sdf::SDF()); + sdf::init(sdfParsed); + if (!sdf::readString(stream.str(), sdfParsed)) + return sdf::ElementPtr(); + + return sdfParsed->Root()->GetElement("model")->GetElement("link") + ->GetElement("sensor"); +} + +// /// \brief Helper function to create an altimeter sdf element with noise +// sdf::ElementPtr AltimeterToSdfWithNoise(const std::string &_name, +// const ignition::math::Pose3d &_pose, const double _updateRate, +// const std::string &_topic, const bool _alwaysOn, +// const bool _visualize, double _mean, double _stddev, double _bias) +// { +// std::ostringstream stream; +// stream +// << "" +// << "" +// << " " +// << " " +// << " " +// << " " << _pose << "" +// << " " << _topic << "" +// << " "<< _updateRate <<"" +// << " " << _alwaysOn <<"" +// << " " << _visualize << "" +// << " " +// << " " +// << " " +// << " " << _mean << "" +// << " " << _stddev << "" +// << " " << _bias << "" +// << " " +// << " " +// << " " +// << " " +// << " " << _mean << "" +// << " " << _stddev << "" +// << " " << _bias << "" +// << " " +// << " " +// << " " +// << " " +// << " " +// << " " +// << ""; + +// sdf::SDFPtr sdfParsed(new sdf::SDF()); +// sdf::init(sdfParsed); +// if (!sdf::readString(stream.str(), sdfParsed)) +// return sdf::ElementPtr(); + +// return sdfParsed->Root()->GetElement("model")->GetElement("link") +// ->GetElement("sensor"); +// } + +/// \brief Test force torque sensor +class ForceTorqueSensorTest: public testing::Test +{ + // Documentation inherited + protected: void SetUp() override + { + ignition::common::Console::SetVerbosity(4); + } +}; + +///////////////////////////////////////////////// +TEST_F(ForceTorqueSensorTest, CreateForceTorqueSensor) +{ + // Create SDF describing a force torque sensor + const std::string name = "TestForceTorqueSensor"; + const std::string topic = "/ignition/sensors/test/force_torque"; + + // const std::string topicNoise = "/ignition/sensors/test/altimeter_noise"; + const double updateRate = 30; + const bool alwaysOn = 1; + const bool visualize = 1; + + // Create sensor SDF + ignition::math::Pose3d sensorPose(ignition::math::Vector3d(0.25, 0.0, 0.5), + ignition::math::Quaterniond::Identity); + sdf::ElementPtr forcetorqueSdf = ForceTorqueToSdf(name, sensorPose, + updateRate, topic, alwaysOn, visualize); + + // sdf::ElementPtr altimeterSdfNoise = AltimeterToSdfWithNoise(name, sensorPose, + // updateRate, topicNoise, alwaysOn, visualize, 1.0, 0.2, 10.0); + + // create the sensor using sensor factory + ignition::sensors::SensorFactory sf; + std::unique_ptr sensor = + sf.CreateSensor(forcetorqueSdf); + ASSERT_NE(nullptr, sensor); + + EXPECT_EQ(name, sensor->Name()); + EXPECT_EQ(topic, sensor->Topic()); + EXPECT_DOUBLE_EQ(updateRate, sensor->UpdateRate()); + + // std::unique_ptr sensorNoise = + // sf.CreateSensor(altimeterSdfNoise); + // ASSERT_NE(nullptr, sensorNoise); + + // EXPECT_EQ(name, sensorNoise->Name()); + // EXPECT_EQ(topicNoise, sensorNoise->Topic()); + // EXPECT_DOUBLE_EQ(updateRate, sensorNoise->UpdateRate()); +} + +// ///////////////////////////////////////////////// +TEST_F(ForceTorqueSensorTest, SensorReadings) +{ + // Create SDF describing a a force torque sensor + const std::string name = "TestForceTorqueSensor"; + const std::string topic = "/ignition/sensors/test/force_torque"; + // const std::string topicNoise = "/ignition/sensors/test/altimeter_noise"; + const double updateRate = 30; + const bool alwaysOn = 1; + const bool visualize = 1; + + // Create sensor SDF + ignition::math::Pose3d sensorPose(ignition::math::Vector3d(0.25, 0.0, 0.5), + ignition::math::Quaterniond::Identity); + sdf::ElementPtr forcetorqueSdf = ForceTorqueToSdf(name, sensorPose, + updateRate, topic, alwaysOn, visualize); + + // sdf::ElementPtr altimeterSdfNoise = AltimeterToSdfWithNoise(name, sensorPose, + // updateRate, topicNoise, alwaysOn, visualize, 1.0, 0.2, 10.0); + + // create the sensor using sensor factory + // try creating without specifying the sensor type and then cast it + ignition::sensors::SensorFactory sf; + std::unique_ptr s = + sf.CreateSensor(forcetorqueSdf); + std::unique_ptr sensor( + dynamic_cast(s.release())); + + // Make sure the above dynamic cast worked. + ASSERT_NE(nullptr, sensor); + + // std::unique_ptr sNoise = + // sf.CreateSensor(altimeterSdfNoise); + // std::unique_ptr sensorNoise( + // dynamic_cast(sNoise.release())); + + // Make sure the above dynamic cast worked. + // ASSERT_NE(nullptr, sensorNoise); + + // verify initial readings + EXPECT_EQ(ignition::math::Vector3d::Zero, sensor->Force()); + EXPECT_EQ(ignition::math::Vector3d::Zero, sensor->Torque()); + + // verify initial readings + // EXPECT_DOUBLE_EQ(0.0, sensorNoise->force()); + // EXPECT_DOUBLE_EQ(0.0, sensorNoise->torque()); + + // set state and verify readings + + // TODO : Add Noise tests + ignition::math::Vector3d force(0,0,1); + sensor->SetForce(force); + EXPECT_EQ(force, sensor->Force()); + + ignition::math::Vector3d torque(0,0,1); + sensor->SetTorque(torque); + EXPECT_EQ(torque, sensor->Torque()); + + + // double pos = 2.0; + // sensor->SetPosition(pos); + // sensorNoise->SetPosition(pos); + // EXPECT_DOUBLE_EQ(pos - vertRef, sensor->VerticalPosition()); + // EXPECT_DOUBLE_EQ(pos - vertRef, sensorNoise->VerticalPosition()); + + // verify msg received on the topic + WaitForMessageTestHelper msgHelper(topic); + sensor->Update(std::chrono::steady_clock::duration(std::chrono::seconds(1))); + EXPECT_TRUE(msgHelper.WaitForMessage()) << msgHelper; + auto msg = msgHelper.Message(); + EXPECT_EQ(1, msg.header().stamp().sec()); + EXPECT_EQ(0, msg.header().stamp().nsec()); + + EXPECT_EQ(ignition::math::Vector3d(0.1, 0.1, 0.1), ignition::msgs::Convert(msg.force())); + EXPECT_EQ(ignition::math::Vector3d(0.1, 0.1, 0.1), ignition::msgs::Convert(msg.torque())); + + + // // verify msg with noise received on the topic + // WaitForMessageTestHelper + // msgHelperNoise(topicNoise); + // sensorNoise->Update(std::chrono::steady_clock::duration( + // std::chrono::seconds(1))); + // EXPECT_TRUE(msgHelperNoise.WaitForMessage()) << msgHelperNoise; + // auto msgNoise = msgHelperNoise.Message(); + // EXPECT_EQ(1, msg.header().stamp().sec()); + // EXPECT_EQ(0, msg.header().stamp().nsec()); + // EXPECT_DOUBLE_EQ(vertRef, msgNoise.vertical_reference()); + // EXPECT_FALSE(ignition::math::equal(pos - vertRef, + // msgNoise.vertical_position())); + // EXPECT_FALSE(ignition::math::equal(vertVel, msgNoise.vertical_velocity())); +} + +///////////////////////////////////////////////// +TEST_F(ForceTorqueSensorTest, Topic) +{ + const std::string name = "TestForceTorqueSensor"; + const double updateRate = 30; + const bool alwaysOn = 1; + const bool visualize = 1; + auto sensorPose = ignition::math::Pose3d(); + + // Factory + ignition::sensors::SensorFactory factory; + + // Default topic + { + const std::string topic; + auto forcetorqueSdf = ForceTorqueToSdf(name, sensorPose, + updateRate, topic, alwaysOn, visualize); + + auto sensor = factory.CreateSensor(forcetorqueSdf); + EXPECT_NE(nullptr, sensor); + + auto force_torque = + dynamic_cast(sensor.release()); + ASSERT_NE(nullptr, force_torque); + + EXPECT_EQ("/forcetorque", force_torque->Topic()); + } + + // Convert to valid topic + { + const std::string topic = "/topic with spaces/@~characters//"; + auto forcetorqueSdf = ForceTorqueToSdf(name, sensorPose, + updateRate, topic, alwaysOn, visualize); + + auto sensor = factory.CreateSensor(forcetorqueSdf); + EXPECT_NE(nullptr, sensor); + + auto force_torque = + dynamic_cast(sensor.release()); + ASSERT_NE(nullptr, force_torque); + + EXPECT_EQ("/topic_with_spaces/characters", force_torque->Topic()); + } + + // Invalid topic + { + const std::string topic = "@@@"; + auto forcetorqueSdf = ForceTorqueToSdf(name, sensorPose, + updateRate, topic, alwaysOn, visualize); + + auto sensor = factory.CreateSensor(forcetorqueSdf); + ASSERT_EQ(nullptr, sensor); + } +} + +int main(int argc, char **argv) +{ + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} From 04746c2845560fa78a428429cee9c6ac9e0e8e53 Mon Sep 17 00:00:00 2001 From: Devansh Date: Thu, 8 Jul 2021 15:17:37 -0400 Subject: [PATCH 02/18] added coordinate transformation --- src/ForceTorqueSensor.cc | 109 +++++++++++++++++++++++++++++++++------ 1 file changed, 94 insertions(+), 15 deletions(-) diff --git a/src/ForceTorqueSensor.cc b/src/ForceTorqueSensor.cc index 038aea49..81d998a8 100644 --- a/src/ForceTorqueSensor.cc +++ b/src/ForceTorqueSensor.cc @@ -48,11 +48,42 @@ class ignition::sensors::ForceTorqueSensorPrivate /// \brief true if Load() has been called and was successful public: bool initialized = false; - /// \brief Noise free force. - public: ignition::math::Vector3d force; + /// \brief Noise free body 1 force. + public: ignition::math::Vector3d body1Force; + + /// \brief Noise free body 2 force. + public: ignition::math::Vector3d body2Force; + + /// \brief Noise free body 1 torque. + public: ignition::math::Vector3d body1Torque; + + /// \brief Noise free body 2 torque. + public: ignition::math::Vector3d body2Torque; + + /// \brief Parent joint, from which we get force torque info. + public: physics::JointWrench parentJoint; + + /// \brief Which orientation we support for returning sensor measure + public: enum MeasureFrame + { + PARENT_LINK, + CHILD_LINK, + SENSOR + }; + + /// \brief Frame in which we return the measured force torque info. + public: MeasureFrame measureFrame; + + /// \brief Direction of the measure + /// True if the measured force torque is the one applied + /// by the parent on the child, false otherwise + public: bool parentToChild; + + /// \brief Rotation matrix than transforms a vector expressed in child + /// orientation in a vector expressed in joint orientation. + /// Necessary is the measure is specified in joint frame. + public: ignition::math::Matrix3d rotationSensorChild; - /// \brief Noise free torque. - public: ignition::math::Vector3d torque; /// \brief Flag for if time has been initialized public: bool timeInitialized = false; @@ -166,19 +197,67 @@ bool ForceTorqueSensor::Update(const std::chrono::steady_clock::duration &_now) frame->set_key("frame_id"); frame->add_value(this->Name()); - /* - TODO - - Check frame orientation - - Apply noises. - */ + // *** Need to Access force Torque *** + physics::JointWrench wrench = this->dataPtr->parentJoint->GetForceTorque()); + + // Get the force and torque in the appropriate frame. + ignition::math::Vector3d measuredForce; + ignition::math::Vector3d measuredTorque; + + if (this->dataPtr->measureFrame == PARENT_LINK) + { + if (this->dataPtr->parentToChild) + { + measuredForce = wrench.body1Force; + measuredTorque = wrench.body1Torque; + } + else + { + measuredForce = -1*wrench.body1Force; + measuredTorque = -1*wrench.body1Torque; + } + } + else if (this->dataPtr->measureFrame == CHILD_LINK) + { + if (!this->dataPtr->parentToChild) + { + measuredForce = wrench.body2Force; + measuredTorque = wrench.body2Torque; + } + else + { + measuredForce = -1*wrench.body2Force; + measuredTorque = -1*wrench.body2Torque; + } + } + else + { + ignerr << "measureFrame must be PARENT_LINK, CHILD_LINK or SENSOR\n"; + + if (!this->dataPtr->parentToChild) + { + measuredForce = this->dataPtr->rotationSensorChild * + wrench.body2Force; + measuredTorque = this->dataPtr->rotationSensorChild * + wrench.body2Torque; + } + else + { + measuredForce = this->dataPtr->rotationSensorChild * + (-1*wrench.body2Force); + measuredTorque = this->dataPtr->rotationSensorChild * + (-1*wrench.body2Torque); + } + + } - this->dataPtr->force.X() = 0.1; - this->dataPtr->force.Y() = 0.1; - this->dataPtr->force.Z() = 0.1; + // this->dataPtr->force.X() = 0.1; + // this->dataPtr->force.Y() = 0.1; + // this->dataPtr->force.Z() = 0.1; - this->dataPtr->torque.X() = 0.1; - this->dataPtr->torque.Y() = 0.1; - this->dataPtr->torque.Z() = 0.1; + // this->dataPtr->torque.X() = 0.1; + // this->dataPtr->torque.Y() = 0.1; + // this->dataPtr->torque.Z() = 0.1; msgs::Set(msg.mutable_force(), this->dataPtr->force); msgs::Set(msg.mutable_torque(), this->dataPtr->torque); From 543fe7550e0445c22a2bd324249719c27556be17 Mon Sep 17 00:00:00 2001 From: Devansh Date: Thu, 15 Jul 2021 14:35:30 -0400 Subject: [PATCH 03/18] Updated coordinate conversion and fixed errors --- src/ForceTorqueSensor.cc | 60 ++++++++++++++-------------------------- 1 file changed, 20 insertions(+), 40 deletions(-) diff --git a/src/ForceTorqueSensor.cc b/src/ForceTorqueSensor.cc index 81d998a8..29d4e668 100644 --- a/src/ForceTorqueSensor.cc +++ b/src/ForceTorqueSensor.cc @@ -48,20 +48,11 @@ class ignition::sensors::ForceTorqueSensorPrivate /// \brief true if Load() has been called and was successful public: bool initialized = false; - /// \brief Noise free body 1 force. - public: ignition::math::Vector3d body1Force; + /// \brief Noise free force + public: ignition::math::Vector3d force; - /// \brief Noise free body 2 force. - public: ignition::math::Vector3d body2Force; - - /// \brief Noise free body 1 torque. - public: ignition::math::Vector3d body1Torque; - - /// \brief Noise free body 2 torque. - public: ignition::math::Vector3d body2Torque; - - /// \brief Parent joint, from which we get force torque info. - public: physics::JointWrench parentJoint; + /// \brief Noise free torque + public: ignition::math::Vector3d torque; /// \brief Which orientation we support for returning sensor measure public: enum MeasureFrame @@ -197,37 +188,34 @@ bool ForceTorqueSensor::Update(const std::chrono::steady_clock::duration &_now) frame->set_key("frame_id"); frame->add_value(this->Name()); - // *** Need to Access force Torque *** - physics::JointWrench wrench = this->dataPtr->parentJoint->GetForceTorque()); - // Get the force and torque in the appropriate frame. ignition::math::Vector3d measuredForce; ignition::math::Vector3d measuredTorque; - if (this->dataPtr->measureFrame == PARENT_LINK) + if (this->dataPtr->measureFrame == ForceTorqueSensorPrivate::PARENT_LINK) { if (this->dataPtr->parentToChild) { - measuredForce = wrench.body1Force; - measuredTorque = wrench.body1Torque; + measuredForce = this->dataPtr->force; + measuredTorque = this->dataPtr->torque; } else { - measuredForce = -1*wrench.body1Force; - measuredTorque = -1*wrench.body1Torque; + measuredForce = -1*this->dataPtr->force; + measuredTorque = -1*this->dataPtr->torque; } } - else if (this->dataPtr->measureFrame == CHILD_LINK) + else if (this->dataPtr->measureFrame == ForceTorqueSensorPrivate::CHILD_LINK) { if (!this->dataPtr->parentToChild) { - measuredForce = wrench.body2Force; - measuredTorque = wrench.body2Torque; + measuredForce = this->dataPtr->force; + measuredTorque = this->dataPtr->torque; } else { - measuredForce = -1*wrench.body2Force; - measuredTorque = -1*wrench.body2Torque; + measuredForce = -1*this->dataPtr->force; + measuredTorque = -1*this->dataPtr->torque; } } else @@ -237,30 +225,22 @@ bool ForceTorqueSensor::Update(const std::chrono::steady_clock::duration &_now) if (!this->dataPtr->parentToChild) { measuredForce = this->dataPtr->rotationSensorChild * - wrench.body2Force; + this->dataPtr->force; measuredTorque = this->dataPtr->rotationSensorChild * - wrench.body2Torque; + this->dataPtr->torque; } else { measuredForce = this->dataPtr->rotationSensorChild * - (-1*wrench.body2Force); + (-1*this->dataPtr->force); measuredTorque = this->dataPtr->rotationSensorChild * - (-1*wrench.body2Torque); + (-1*this->dataPtr->torque); } } - // this->dataPtr->force.X() = 0.1; - // this->dataPtr->force.Y() = 0.1; - // this->dataPtr->force.Z() = 0.1; - - // this->dataPtr->torque.X() = 0.1; - // this->dataPtr->torque.Y() = 0.1; - // this->dataPtr->torque.Z() = 0.1; - - msgs::Set(msg.mutable_force(), this->dataPtr->force); - msgs::Set(msg.mutable_torque(), this->dataPtr->torque); + msgs::Set(msg.mutable_force(), measuredForce); + msgs::Set(msg.mutable_torque(), measuredTorque); // publish this->AddSequence(msg.mutable_header()); From cf50652dd98f3a3fe1281c8826636084740e55ff Mon Sep 17 00:00:00 2001 From: Devansh Chawla Date: Tue, 20 Jul 2021 14:42:04 -0400 Subject: [PATCH 04/18] Update include/ignition/sensors/ForceTorqueSensor.hh MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Alejandro Hernández Cordero --- include/ignition/sensors/ForceTorqueSensor.hh | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/include/ignition/sensors/ForceTorqueSensor.hh b/include/ignition/sensors/ForceTorqueSensor.hh index c4d71da8..95102e8d 100644 --- a/include/ignition/sensors/ForceTorqueSensor.hh +++ b/include/ignition/sensors/ForceTorqueSensor.hh @@ -24,10 +24,10 @@ #include #include + #include #include - #include #include "ignition/sensors/Sensor.hh" From 49a628d2dabb99f403480751babe7aa0f602cb01 Mon Sep 17 00:00:00 2001 From: Devansh Chawla Date: Tue, 20 Jul 2021 14:42:40 -0400 Subject: [PATCH 05/18] Update src/ForceTorqueSensor.cc MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Alejandro Hernández Cordero --- src/ForceTorqueSensor.cc | 1 - 1 file changed, 1 deletion(-) diff --git a/src/ForceTorqueSensor.cc b/src/ForceTorqueSensor.cc index 29d4e668..b08c3b0a 100644 --- a/src/ForceTorqueSensor.cc +++ b/src/ForceTorqueSensor.cc @@ -75,7 +75,6 @@ class ignition::sensors::ForceTorqueSensorPrivate /// Necessary is the measure is specified in joint frame. public: ignition::math::Matrix3d rotationSensorChild; - /// \brief Flag for if time has been initialized public: bool timeInitialized = false; From 895bbc044781570754b0490a33b62b17abb8fde7 Mon Sep 17 00:00:00 2001 From: Devansh Chawla Date: Tue, 20 Jul 2021 14:42:47 -0400 Subject: [PATCH 06/18] Update src/ForceTorqueSensor.cc MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Alejandro Hernández Cordero --- src/ForceTorqueSensor.cc | 1 - 1 file changed, 1 deletion(-) diff --git a/src/ForceTorqueSensor.cc b/src/ForceTorqueSensor.cc index b08c3b0a..b699e417 100644 --- a/src/ForceTorqueSensor.cc +++ b/src/ForceTorqueSensor.cc @@ -81,7 +81,6 @@ class ignition::sensors::ForceTorqueSensorPrivate /// \brief Previous update time step. public: std::chrono::steady_clock::duration prevStep {std::chrono::steady_clock::duration::zero()}; - }; ////////////////////////////////////////////////// From 507670fae8f405e26eb826fcffa11e8b8ce0c7d6 Mon Sep 17 00:00:00 2001 From: Devansh Chawla Date: Tue, 20 Jul 2021 14:42:53 -0400 Subject: [PATCH 07/18] Update src/ForceTorqueSensor.cc MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Alejandro Hernández Cordero --- src/ForceTorqueSensor.cc | 1 - 1 file changed, 1 deletion(-) diff --git a/src/ForceTorqueSensor.cc b/src/ForceTorqueSensor.cc index b699e417..9af7e47d 100644 --- a/src/ForceTorqueSensor.cc +++ b/src/ForceTorqueSensor.cc @@ -234,7 +234,6 @@ bool ForceTorqueSensor::Update(const std::chrono::steady_clock::duration &_now) measuredTorque = this->dataPtr->rotationSensorChild * (-1*this->dataPtr->torque); } - } msgs::Set(msg.mutable_force(), measuredForce); From 5971cfc2814179561765e9643c68688edb78f32e Mon Sep 17 00:00:00 2001 From: Devansh Chawla Date: Tue, 20 Jul 2021 14:43:10 -0400 Subject: [PATCH 08/18] Update test/integration/force_torque_plugin.cc MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Alejandro Hernández Cordero --- test/integration/force_torque_plugin.cc | 1 + 1 file changed, 1 insertion(+) diff --git a/test/integration/force_torque_plugin.cc b/test/integration/force_torque_plugin.cc index 654c87fb..65a61253 100644 --- a/test/integration/force_torque_plugin.cc +++ b/test/integration/force_torque_plugin.cc @@ -20,6 +20,7 @@ #include #include + #include #include From 75d4c0e25e08b7af56b34386fe520029e5c793bc Mon Sep 17 00:00:00 2001 From: Devansh Date: Tue, 10 Aug 2021 22:32:00 -0400 Subject: [PATCH 09/18] Fixed indentation --- include/ignition/sensors/ForceTorqueSensor.hh | 10 +++++----- src/ForceTorqueSensor.cc | 18 +++++++++--------- 2 files changed, 14 insertions(+), 14 deletions(-) diff --git a/include/ignition/sensors/ForceTorqueSensor.hh b/include/ignition/sensors/ForceTorqueSensor.hh index 95102e8d..5b8e4dfb 100644 --- a/include/ignition/sensors/ForceTorqueSensor.hh +++ b/include/ignition/sensors/ForceTorqueSensor.hh @@ -68,11 +68,11 @@ namespace ignition /// \return True on success public: virtual bool Init() override; - /// \brief Update the sensor and generate data - /// \param[in] _now The current time - /// \return true if the update was successfull - public: virtual bool IGN_DEPRECATED(4) Update( - const ignition::common::Time &_now) override; + // /// \brief Update the sensor and generate data + // /// \param[in] _now The current time + // /// \return true if the update was successfull + // public: virtual bool IGN_DEPRECATED(4) Update( + // const ignition::common::Time &_now) override; /// \brief Update the sensor and generate data /// \param[in] _now The current time diff --git a/src/ForceTorqueSensor.cc b/src/ForceTorqueSensor.cc index 9af7e47d..bccb2b05 100644 --- a/src/ForceTorqueSensor.cc +++ b/src/ForceTorqueSensor.cc @@ -55,25 +55,25 @@ class ignition::sensors::ForceTorqueSensorPrivate public: ignition::math::Vector3d torque; /// \brief Which orientation we support for returning sensor measure - public: enum MeasureFrame - { - PARENT_LINK, - CHILD_LINK, - SENSOR - }; + public: enum MeasureFrame + { + PARENT_LINK, + CHILD_LINK, + SENSOR + }; /// \brief Frame in which we return the measured force torque info. - public: MeasureFrame measureFrame; + public: MeasureFrame measureFrame; /// \brief Direction of the measure /// True if the measured force torque is the one applied /// by the parent on the child, false otherwise - public: bool parentToChild; + public: bool parentToChild; /// \brief Rotation matrix than transforms a vector expressed in child /// orientation in a vector expressed in joint orientation. /// Necessary is the measure is specified in joint frame. - public: ignition::math::Matrix3d rotationSensorChild; + public: ignition::math::Matrix3d rotationSensorChild; /// \brief Flag for if time has been initialized public: bool timeInitialized = false; From be5ce03c5d105a7c549965e39713d2f763e78ed0 Mon Sep 17 00:00:00 2001 From: Devansh Date: Thu, 12 Aug 2021 20:42:04 -0400 Subject: [PATCH 10/18] fixed sdf parsing Signed-off-by: Devansh --- include/ignition/sensors/ForceTorqueSensor.hh | 3 ++ src/ForceTorqueSensor.cc | 32 ++++++++----------- test/integration/force_torque_plugin.cc | 4 +-- 3 files changed, 19 insertions(+), 20 deletions(-) diff --git a/include/ignition/sensors/ForceTorqueSensor.hh b/include/ignition/sensors/ForceTorqueSensor.hh index 5b8e4dfb..7e81876a 100644 --- a/include/ignition/sensors/ForceTorqueSensor.hh +++ b/include/ignition/sensors/ForceTorqueSensor.hh @@ -80,6 +80,9 @@ namespace ignition public: virtual bool Update( const std::chrono::steady_clock::duration &_now) override; + public: bool Update( + const ignition::common::Time &_now); + /// \brief Get the current joint force. /// \return The latested measured force. public: math::Vector3d Force() const; diff --git a/src/ForceTorqueSensor.cc b/src/ForceTorqueSensor.cc index bccb2b05..f8d54e5d 100644 --- a/src/ForceTorqueSensor.cc +++ b/src/ForceTorqueSensor.cc @@ -53,22 +53,12 @@ class ignition::sensors::ForceTorqueSensorPrivate /// \brief Noise free torque public: ignition::math::Vector3d torque; - - /// \brief Which orientation we support for returning sensor measure - public: enum MeasureFrame - { - PARENT_LINK, - CHILD_LINK, - SENSOR - }; /// \brief Frame in which we return the measured force torque info. - public: MeasureFrame measureFrame; + public: sdf::ForceTorqueFrame measureFrame; - /// \brief Direction of the measure - /// True if the measured force torque is the one applied - /// by the parent on the child, false otherwise - public: bool parentToChild; + /// \brief Direction in which we return the measured force torque info. + public: sdf::ForceTorqueMeasureDirection measureDirection; /// \brief Rotation matrix than transforms a vector expressed in child /// orientation in a vector expressed in joint orientation. @@ -120,6 +110,12 @@ bool ForceTorqueSensor::Load(const sdf::Sensor &_sdf) return false; } + this->dataPtr->measureFrame = _sdf.ForceTorqueSensor()->Frame(); + this->dataPtr->measureDirection = _sdf.ForceTorqueSensor()->MeasureDirection(); + + + + if (this->Topic().empty()) this->SetTopic("/forcetorque"); @@ -190,9 +186,9 @@ bool ForceTorqueSensor::Update(const std::chrono::steady_clock::duration &_now) ignition::math::Vector3d measuredForce; ignition::math::Vector3d measuredTorque; - if (this->dataPtr->measureFrame == ForceTorqueSensorPrivate::PARENT_LINK) + if (this->dataPtr->measureFrame == sdf::ForceTorqueFrame::PARENT) { - if (this->dataPtr->parentToChild) + if (this->dataPtr->measureDirection == sdf::ForceTorqueMeasureDirection::PARENT_TO_CHILD) { measuredForce = this->dataPtr->force; measuredTorque = this->dataPtr->torque; @@ -203,9 +199,9 @@ bool ForceTorqueSensor::Update(const std::chrono::steady_clock::duration &_now) measuredTorque = -1*this->dataPtr->torque; } } - else if (this->dataPtr->measureFrame == ForceTorqueSensorPrivate::CHILD_LINK) + else if (this->dataPtr->measureFrame == sdf::ForceTorqueFrame::CHILD) { - if (!this->dataPtr->parentToChild) + if (this->dataPtr->measureDirection == sdf::ForceTorqueMeasureDirection::CHILD_TO_PARENT) { measuredForce = this->dataPtr->force; measuredTorque = this->dataPtr->torque; @@ -220,7 +216,7 @@ bool ForceTorqueSensor::Update(const std::chrono::steady_clock::duration &_now) { ignerr << "measureFrame must be PARENT_LINK, CHILD_LINK or SENSOR\n"; - if (!this->dataPtr->parentToChild) + if (this->dataPtr->measureDirection == sdf::ForceTorqueMeasureDirection::CHILD_TO_PARENT) { measuredForce = this->dataPtr->rotationSensorChild * this->dataPtr->force; diff --git a/test/integration/force_torque_plugin.cc b/test/integration/force_torque_plugin.cc index 65a61253..a83fbc94 100644 --- a/test/integration/force_torque_plugin.cc +++ b/test/integration/force_torque_plugin.cc @@ -230,8 +230,8 @@ TEST_F(ForceTorqueSensorTest, SensorReadings) EXPECT_EQ(1, msg.header().stamp().sec()); EXPECT_EQ(0, msg.header().stamp().nsec()); - EXPECT_EQ(ignition::math::Vector3d(0.1, 0.1, 0.1), ignition::msgs::Convert(msg.force())); - EXPECT_EQ(ignition::math::Vector3d(0.1, 0.1, 0.1), ignition::msgs::Convert(msg.torque())); + EXPECT_EQ(ignition::math::Vector3d(0, 0, 1), ignition::msgs::Convert(msg.force())); + EXPECT_EQ(ignition::math::Vector3d(0, 0, 1), ignition::msgs::Convert(msg.torque())); // // verify msg with noise received on the topic From 400675dd0136a0f1ea8de6e7ba8450c2e95da093 Mon Sep 17 00:00:00 2001 From: Devansh Date: Mon, 16 Aug 2021 06:34:57 -0400 Subject: [PATCH 11/18] added noise to FT values Signed-off-by: Devansh --- include/ignition/sensors/SensorTypes.hh | 24 ++++++++++++++++ src/ForceTorqueSensor.cc | 38 +++++++++++++++++++++++-- 2 files changed, 59 insertions(+), 3 deletions(-) diff --git a/include/ignition/sensors/SensorTypes.hh b/include/ignition/sensors/SensorTypes.hh index 479e671b..875ccb07 100644 --- a/include/ignition/sensors/SensorTypes.hh +++ b/include/ignition/sensors/SensorTypes.hh @@ -149,6 +149,30 @@ namespace ignition /// \sa Lidar LIDAR_NOISE = 14, + /// \brief Force body-frame X axis noise in N + /// \sa ForceTorqueSensor + FORCE_X_NOISE_N = 15, + + /// \brief Force body-frame Y axis noise in N + /// \sa ForceTorqueSensor + FORCE_Y_NOISE_N = 16, + + /// \brief Force body-frame Z axis noise in N + /// \sa ForceTorqueSensor + FORCE_Z_NOISE_N = 17, + + /// \brief Torque body-frame X axis noise in Nm + /// \sa ForceTorqueSensor + TORQUE_X_NOISE_N_M = 18, + + /// \brief Torque body-frame Y axis noise in Nm + /// \sa ForceTorqueSensor + TORQUE_Y_NOISE_N_M = 19, + + /// \brief Torque body-frame Z axis noise in Nm + /// \sa ForceTorqueSensor + TORQUE_Z_NOISE_N_M = 20, + /// \internal /// \brief Indicator used to create an iterator over the enum. Do not /// use this. diff --git a/src/ForceTorqueSensor.cc b/src/ForceTorqueSensor.cc index f8d54e5d..98748618 100644 --- a/src/ForceTorqueSensor.cc +++ b/src/ForceTorqueSensor.cc @@ -71,6 +71,9 @@ class ignition::sensors::ForceTorqueSensorPrivate /// \brief Previous update time step. public: std::chrono::steady_clock::duration prevStep {std::chrono::steady_clock::duration::zero()}; + + /// \brief Noise added to sensor data + public: std::map noises; }; ////////////////////////////////////////////////// @@ -113,9 +116,6 @@ bool ForceTorqueSensor::Load(const sdf::Sensor &_sdf) this->dataPtr->measureFrame = _sdf.ForceTorqueSensor()->Frame(); this->dataPtr->measureDirection = _sdf.ForceTorqueSensor()->MeasureDirection(); - - - if (this->Topic().empty()) this->SetTopic("/forcetorque"); @@ -128,6 +128,23 @@ bool ForceTorqueSensor::Load(const sdf::Sensor &_sdf) return false; } + const std::map noises = { + {FORCE_X_NOISE_N, _sdf.ForceTorqueSensor()->ForceXNoise()}, + {FORCE_Y_NOISE_N, _sdf.ForceTorqueSensor()->ForceYNoise()}, + {FORCE_Z_NOISE_N, _sdf.ForceTorqueSensor()->ForceZNoise()}, + {TORQUE_X_NOISE_N, _sdf.ForceTorqueSensor()->TorqueXNoise()}, + {TORQUE_Y_NOISE_N, _sdf.ForceTorqueSensor()->TorqueYNoise()}, + {TORQUE_Z_NOISE_N, _sdf.ForceTorqueSensor()->TorqueZNoise()}, + }; + + for (const auto & [noiseType, noiseSdf] : noises) + { + if (noiseSdf.Type() != sdf::NoiseType::NONE) + { + this->dataPtr->noises[noiseType] = NoiseFactory::NewNoiseModel(noiseSdf); + } + } + this->dataPtr->initialized = true; return true; } @@ -176,6 +193,21 @@ bool ForceTorqueSensor::Update(const std::chrono::steady_clock::duration &_now) dt = 0.0; } + // Convenience method to apply noise to a channel, if present. + auto applyNoise = [&](SensorNoiseType noiseType, double & value) + { + if (this->dataPtr->noises.find(noiseType) != this->dataPtr->noises.end()) { + value = this->dataPtr->noises[noiseType]->Apply(value, dt); + } + }; + + applyNoise(FORCE_X_NOISE_N, this->dataPtr->force.X()); + applyNoise(FORCE_Y_NOISE_N, this->dataPtr->force.Y()); + applyNoise(FORCE_Z_NOISE_N, this->dataPtr->force.Z()); + applyNoise(TORQUE_X_NOISE_N_M, this->dataPtr->torque.X()); + applyNoise(TORQUE_Y_NOISE_N_M, this->dataPtr->torque.Y()); + applyNoise(TORQUE_Z_NOISE_N_M, this->dataPtr->torque.Z()); + msgs::Wrench msg; *msg.mutable_header()->mutable_stamp() = msgs::Convert(_now); auto frame = msg.mutable_header()->add_data(); From b0b82131555874283af42006aa0bffd7bce5d53a Mon Sep 17 00:00:00 2001 From: Devansh Date: Mon, 16 Aug 2021 06:51:26 -0400 Subject: [PATCH 12/18] fixed errors Signed-off-by: Devansh --- src/ForceTorqueSensor.cc | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/ForceTorqueSensor.cc b/src/ForceTorqueSensor.cc index 98748618..318b7bde 100644 --- a/src/ForceTorqueSensor.cc +++ b/src/ForceTorqueSensor.cc @@ -132,9 +132,9 @@ bool ForceTorqueSensor::Load(const sdf::Sensor &_sdf) {FORCE_X_NOISE_N, _sdf.ForceTorqueSensor()->ForceXNoise()}, {FORCE_Y_NOISE_N, _sdf.ForceTorqueSensor()->ForceYNoise()}, {FORCE_Z_NOISE_N, _sdf.ForceTorqueSensor()->ForceZNoise()}, - {TORQUE_X_NOISE_N, _sdf.ForceTorqueSensor()->TorqueXNoise()}, - {TORQUE_Y_NOISE_N, _sdf.ForceTorqueSensor()->TorqueYNoise()}, - {TORQUE_Z_NOISE_N, _sdf.ForceTorqueSensor()->TorqueZNoise()}, + {TORQUE_X_NOISE_N_M, _sdf.ForceTorqueSensor()->TorqueXNoise()}, + {TORQUE_Y_NOISE_N_M, _sdf.ForceTorqueSensor()->TorqueYNoise()}, + {TORQUE_Z_NOISE_N_M, _sdf.ForceTorqueSensor()->TorqueZNoise()}, }; for (const auto & [noiseType, noiseSdf] : noises) From a2d37ea7b11a01697d86a46b4a1085a5b2016a97 Mon Sep 17 00:00:00 2001 From: Devansh Date: Wed, 25 Aug 2021 02:03:47 -0400 Subject: [PATCH 13/18] Updated plugin --- src/ForceTorqueSensor.cc | 1 + 1 file changed, 1 insertion(+) diff --git a/src/ForceTorqueSensor.cc b/src/ForceTorqueSensor.cc index 318b7bde..d5a13aa3 100644 --- a/src/ForceTorqueSensor.cc +++ b/src/ForceTorqueSensor.cc @@ -237,6 +237,7 @@ bool ForceTorqueSensor::Update(const std::chrono::steady_clock::duration &_now) { measuredForce = this->dataPtr->force; measuredTorque = this->dataPtr->torque; + ignerr << "Warning: ForceTorqueSensor::Update() " << std::endl; } else { From 725290098fa4f8f4ffc17f805000806e02ab667a Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Tue, 21 Sep 2021 22:11:36 -0500 Subject: [PATCH 14/18] Fix tests, add rotations to parent and child --- include/ignition/sensors/ForceTorqueSensor.hh | 46 ++- src/ForceTorqueSensor.cc | 171 ++++++---- test/integration/CMakeLists.txt | 2 +- test/integration/force_torque.cc | 285 ++++++++++++++++ test/integration/force_torque_plugin.cc | 311 ------------------ 5 files changed, 414 insertions(+), 401 deletions(-) create mode 100644 test/integration/force_torque.cc delete mode 100644 test/integration/force_torque_plugin.cc diff --git a/include/ignition/sensors/ForceTorqueSensor.hh b/include/ignition/sensors/ForceTorqueSensor.hh index 7e81876a..80868d76 100644 --- a/include/ignition/sensors/ForceTorqueSensor.hh +++ b/include/ignition/sensors/ForceTorqueSensor.hh @@ -45,8 +45,8 @@ namespace ignition /// \brief Force Torque Sensor Class /// /// A force-torque Sensor that reports force and torque applied on a joint. - - class IGNITION_SENSORS_FORCE_TORQUE_VISIBLE ForceTorqueSensor : public Sensor + class IGNITION_SENSORS_FORCE_TORQUE_VISIBLE ForceTorqueSensor + : public Sensor { /// \brief constructor public: ForceTorqueSensor(); @@ -68,37 +68,51 @@ namespace ignition /// \return True on success public: virtual bool Init() override; - // /// \brief Update the sensor and generate data - // /// \param[in] _now The current time - // /// \return true if the update was successfull - // public: virtual bool IGN_DEPRECATED(4) Update( - // const ignition::common::Time &_now) override; - /// \brief Update the sensor and generate data /// \param[in] _now The current time /// \return true if the update was successfull public: virtual bool Update( const std::chrono::steady_clock::duration &_now) override; - public: bool Update( - const ignition::common::Time &_now); - /// \brief Get the current joint force. - /// \return The latested measured force. + /// \return The latest measured force. public: math::Vector3d Force() const; - /// \brief Set the force vector. + /// \brief Set the force vector in sensor frame and where the force is + /// applied on the child (parent-to-child) /// \param[in] _force force vector in newton. public: void SetForce(const math::Vector3d &_force); /// \brief Get the current joint torque. - /// \return The latested measured torque. + /// \return The latest measured torque. public: math::Vector3d Torque() const; - /// \brief Set the torque vector. + /// \brief Set the torque vector in sensor frame and where the torque is + /// applied on the child (parent-to-child) /// \param[in] _torque torque vector in newton. public: void SetTorque(const math::Vector3d &_torque); - + + /// \brief Set the rotation of the joint parent relative to the sensor + /// frame. + /// \return The current rotation the parent in the sensor frame. + public: math::Quaterniond RotationParentInSensor() const; + + /// \brief Set the rotation of the joint parent relative to the sensor + /// frame. + /// \param[in] _rotParentInSensorRotation of parent in sensor frame. + public: void SetRotationParentInSensor( + const math::Quaterniond &_rotParentInSensor); + + /// \brief Set the rotation of the joint parent relative to the sensor + /// frame. + /// \return The current rotation the child in the sensor frame. + public: math::Quaterniond RotationChildInSensor() const; + + /// \brief Set the rotation of the joint child relative to the sensor + /// frame. + /// \param[in] _rotChildInSensor Rotation of child in sensor frame. + public: void SetRotationChildInSensor( + const math::Quaterniond &_rotChildInSensor); IGN_COMMON_WARN_IGNORE__DLL_INTERFACE_MISSING /// \brief Data pointer for private data diff --git a/src/ForceTorqueSensor.cc b/src/ForceTorqueSensor.cc index d5a13aa3..4de1a252 100644 --- a/src/ForceTorqueSensor.cc +++ b/src/ForceTorqueSensor.cc @@ -48,22 +48,33 @@ class ignition::sensors::ForceTorqueSensorPrivate /// \brief true if Load() has been called and was successful public: bool initialized = false; - /// \brief Noise free force - public: ignition::math::Vector3d force; + /// \brief Noise free force as set by SetForce + public: ignition::math::Vector3d force{0, 0, 0}; + + /// \brief Noise free torque as set by SetTorque + public: ignition::math::Vector3d torque{0, 0, 0}; - /// \brief Noise free torque - public: ignition::math::Vector3d torque; - /// \brief Frame in which we return the measured force torque info. public: sdf::ForceTorqueFrame measureFrame; /// \brief Direction in which we return the measured force torque info. public: sdf::ForceTorqueMeasureDirection measureDirection; - /// \brief Rotation matrix than transforms a vector expressed in child - /// orientation in a vector expressed in joint orientation. - /// Necessary is the measure is specified in joint frame. - public: ignition::math::Matrix3d rotationSensorChild; + /// \brief Rotation matrix that transforms a vector expressed in the parent + /// frame to a vector expressed in the sensor frame. + /// \note We store the rotation as a 3x3 matrix because matrix-vector + /// product is than quaternion-vector when there are a lot of vectors and the + /// rotation is not changing frequently. + public: ignition::math::Matrix3d rotationParentInSensor{ + ignition::math::Matrix3d::Identity}; + + /// \brief Rotation matrix that transforms a vector expressed in the child + /// frame to a vector expressed in the sensor frame. + /// \note We store the rotation as a 3x3 matrix because matrix-vector + /// product is than quaternion-vector when there are a lot of vectors and the + /// rotation is not changing frequently. + public: ignition::math::Matrix3d rotationChildInSensor{ + ignition::math::Matrix3d::Identity}; /// \brief Flag for if time has been initialized public: bool timeInitialized = false; @@ -83,9 +94,7 @@ ForceTorqueSensor::ForceTorqueSensor() } ////////////////////////////////////////////////// -ForceTorqueSensor::~ForceTorqueSensor() -{ -} +ForceTorqueSensor::~ForceTorqueSensor() = default; ////////////////////////////////////////////////// bool ForceTorqueSensor::Init() @@ -114,7 +123,8 @@ bool ForceTorqueSensor::Load(const sdf::Sensor &_sdf) } this->dataPtr->measureFrame = _sdf.ForceTorqueSensor()->Frame(); - this->dataPtr->measureDirection = _sdf.ForceTorqueSensor()->MeasureDirection(); + this->dataPtr->measureDirection = + _sdf.ForceTorqueSensor()->MeasureDirection(); if (this->Topic().empty()) this->SetTopic("/forcetorque"); @@ -157,13 +167,6 @@ bool ForceTorqueSensor::Load(sdf::ElementPtr _sdf) return this->Load(sdfSensor); } -////////////////////////////////////////////////// -bool ForceTorqueSensor::Update( - const ignition::common::Time &_now) -{ - return this->Update(math::secNsecToDuration(_now.sec, _now.nsec)); -} - ////////////////////////////////////////////////// bool ForceTorqueSensor::Update(const std::chrono::steady_clock::duration &_now) { @@ -192,78 +195,77 @@ bool ForceTorqueSensor::Update(const std::chrono::steady_clock::duration &_now) { dt = 0.0; } - - // Convenience method to apply noise to a channel, if present. - auto applyNoise = [&](SensorNoiseType noiseType, double & value) - { - if (this->dataPtr->noises.find(noiseType) != this->dataPtr->noises.end()) { - value = this->dataPtr->noises[noiseType]->Apply(value, dt); - } - }; - - applyNoise(FORCE_X_NOISE_N, this->dataPtr->force.X()); - applyNoise(FORCE_Y_NOISE_N, this->dataPtr->force.Y()); - applyNoise(FORCE_Z_NOISE_N, this->dataPtr->force.Z()); - applyNoise(TORQUE_X_NOISE_N_M, this->dataPtr->torque.X()); - applyNoise(TORQUE_Y_NOISE_N_M, this->dataPtr->torque.Y()); - applyNoise(TORQUE_Z_NOISE_N_M, this->dataPtr->torque.Z()); - - msgs::Wrench msg; - *msg.mutable_header()->mutable_stamp() = msgs::Convert(_now); - auto frame = msg.mutable_header()->add_data(); - frame->set_key("frame_id"); - frame->add_value(this->Name()); - // Get the force and torque in the appropriate frame. ignition::math::Vector3d measuredForce; ignition::math::Vector3d measuredTorque; if (this->dataPtr->measureFrame == sdf::ForceTorqueFrame::PARENT) { - if (this->dataPtr->measureDirection == sdf::ForceTorqueMeasureDirection::PARENT_TO_CHILD) - { - measuredForce = this->dataPtr->force; - measuredTorque = this->dataPtr->torque; - } - else - { - measuredForce = -1*this->dataPtr->force; - measuredTorque = -1*this->dataPtr->torque; - } + measuredForce = + this->dataPtr->rotationParentInSensor.Inverse() * this->dataPtr->force; + measuredTorque = + this->dataPtr->rotationParentInSensor.Inverse() * this->dataPtr->torque; + } else if (this->dataPtr->measureFrame == sdf::ForceTorqueFrame::CHILD) { - if (this->dataPtr->measureDirection == sdf::ForceTorqueMeasureDirection::CHILD_TO_PARENT) + measuredForce = + this->dataPtr->rotationChildInSensor.Inverse() * this->dataPtr->force; + measuredTorque = + this->dataPtr->rotationChildInSensor.Inverse() * this->dataPtr->torque; + + if (this->dataPtr->measureDirection == + sdf::ForceTorqueMeasureDirection::CHILD_TO_PARENT) { - measuredForce = this->dataPtr->force; - measuredTorque = this->dataPtr->torque; - ignerr << "Warning: ForceTorqueSensor::Update() " << std::endl; + measuredForce *= -1; + measuredTorque *= -1; } - else + } + else if (this->dataPtr->measureFrame == sdf::ForceTorqueFrame::SENSOR) + { + measuredForce = this->dataPtr->force; + measuredTorque = this->dataPtr->torque; + + if (this->dataPtr->measureDirection == + sdf::ForceTorqueMeasureDirection::CHILD_TO_PARENT) { - measuredForce = -1*this->dataPtr->force; - measuredTorque = -1*this->dataPtr->torque; + measuredForce *= -1; + measuredTorque *= -1; } } else { ignerr << "measureFrame must be PARENT_LINK, CHILD_LINK or SENSOR\n"; + } - if (this->dataPtr->measureDirection == sdf::ForceTorqueMeasureDirection::CHILD_TO_PARENT) - { - measuredForce = this->dataPtr->rotationSensorChild * - this->dataPtr->force; - measuredTorque = this->dataPtr->rotationSensorChild * - this->dataPtr->torque; - } - else + if (this->dataPtr->measureDirection == + sdf::ForceTorqueMeasureDirection::CHILD_TO_PARENT) + { + measuredForce *= -1; + measuredTorque *= -1; + } + + // Convenience method to apply noise to a channel, if present. + auto applyNoise = [&](SensorNoiseType noiseType, double &value) + { + if (this->dataPtr->noises.find(noiseType) != this->dataPtr->noises.end()) { - measuredForce = this->dataPtr->rotationSensorChild * - (-1*this->dataPtr->force); - measuredTorque = this->dataPtr->rotationSensorChild * - (-1*this->dataPtr->torque); + value = this->dataPtr->noises[noiseType]->Apply(value, dt); } - } + }; + + applyNoise(FORCE_X_NOISE_N, measuredForce.X()); + applyNoise(FORCE_Y_NOISE_N, measuredForce.Y()); + applyNoise(FORCE_Z_NOISE_N, measuredForce.Z()); + applyNoise(TORQUE_X_NOISE_N_M, measuredTorque.X()); + applyNoise(TORQUE_Y_NOISE_N_M, measuredTorque.Y()); + applyNoise(TORQUE_Z_NOISE_N_M, measuredTorque.Z()); + + msgs::Wrench msg; + *msg.mutable_header()->mutable_stamp() = msgs::Convert(_now); + auto frame = msg.mutable_header()->add_data(); + frame->set_key("frame_id"); + frame->add_value(this->Name()); msgs::Set(msg.mutable_force(), measuredForce); msgs::Set(msg.mutable_torque(), measuredTorque); @@ -300,5 +302,28 @@ void ForceTorqueSensor::SetTorque(const math::Vector3d &_torque) this->dataPtr->torque = _torque; } +////////////////////////////////////////////////// +math::Quaterniond ForceTorqueSensor::RotationParentInSensor() const +{ + return math::Quaterniond(this->dataPtr->rotationParentInSensor); +} + +////////////////////////////////////////////////// +void ForceTorqueSensor::SetRotationParentInSensor( + const math::Quaterniond &_rotParentInSensor) +{ + this->dataPtr->rotationParentInSensor = _rotParentInSensor; +} -IGN_SENSORS_REGISTER_SENSOR(ForceTorqueSensor) +////////////////////////////////////////////////// +void ForceTorqueSensor::SetRotationChildInSensor( + const math::Quaterniond &_rotChildInSensor) +{ + this->dataPtr->rotationChildInSensor = _rotChildInSensor; +} + +////////////////////////////////////////////////// +math::Quaterniond ForceTorqueSensor::RotationChildInSensor() const +{ + return math::Quaterniond(this->dataPtr->rotationChildInSensor); +} diff --git a/test/integration/CMakeLists.txt b/test/integration/CMakeLists.txt index 587f381d..c289f1c8 100644 --- a/test/integration/CMakeLists.txt +++ b/test/integration/CMakeLists.txt @@ -11,10 +11,10 @@ set(dri_tests set(tests air_pressure.cc altimeter.cc + force_torque.cc logical_camera.cc magnetometer.cc imu.cc - force_torque_plugin.cc ) link_directories(${PROJECT_BINARY_DIR}/test) diff --git a/test/integration/force_torque.cc b/test/integration/force_torque.cc new file mode 100644 index 00000000..4b36f674 --- /dev/null +++ b/test/integration/force_torque.cc @@ -0,0 +1,285 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#include + +#include + +#include + +#include +#include + +#include "test_config.h" // NOLINT(build/include) +#include "TransportTestTools.hh" + +/// \brief Helper function to create a force torque sensor sdf element. +void CreateForceTorqueToSdf(const std::string &_name, + const ignition::math::Pose3d &_pose, + const double _updateRate, const std::string &_topic, + const bool _alwaysOn, const bool _visualize, + const ignition::math::Vector3d &_forceNoiseMean, + const ignition::math::Vector3d &_forceNoiseStddev, + const ignition::math::Vector3d &_torqueNoiseMean, + const ignition::math::Vector3d &_torqueNoiseStddev, + sdf::ElementPtr &_sensorSdf) +{ + std::ostringstream stream; + stream + << "" + << "" + << " " + << " " + << " " + << " " + << " link1" + << " link2" + << " " + << " " << _pose << "" + << " " << _topic << "" + << " "<< _updateRate <<"" + << " " << _alwaysOn <<"" + << " " << _visualize << "" + << " " + << " child" + << " child_to_parent" + << " " + << " " + << " " + << " " << _forceNoiseMean.X() << "" + << " " << _forceNoiseStddev.X() << "" + << " " + << " " + << " " + << " " + << " " << _forceNoiseMean.Y() << "" + << " " << _forceNoiseStddev.Y() << "" + << " " + << " " + << " " + << " " + << " " << _forceNoiseMean.Z() << "" + << " " << _forceNoiseStddev.Z() << "" + << " " + << " " + << " " + << " " + << " " + << " " + << " " << _torqueNoiseMean.X() << "" + << " " << _torqueNoiseStddev.X() << "" + << " " + << " " + << " " + << " " + << " " << _torqueNoiseMean.Y() << "" + << " " << _torqueNoiseStddev.Y() << "" + << " " + << " " + << " " + << " " + << " " << _torqueNoiseMean.Z() << "" + << " " << _torqueNoiseStddev.Z() << "" + << " " + << " " + << " " + << " " + << " " + << " " + << " " + << ""; + + sdf::Root root; + sdf::Errors errors = root.LoadSdfString(stream.str()); + if (errors.empty()) + { + auto model = root.Model(); + ASSERT_NE(nullptr, model); + auto joint = model->JointByIndex(0); + ASSERT_NE(nullptr, joint); + auto sensor = joint->SensorByIndex(0); + ASSERT_NE(nullptr, sensor); + _sensorSdf = sensor->Element(); + } + else + { + _sensorSdf = nullptr; + } +} + +/// \brief Test force torque sensor +class ForceTorqueSensorTest: public testing::Test +{ + // Documentation inherited + protected: void SetUp() override + { + ignition::common::Console::SetVerbosity(4); + } +}; + +///////////////////////////////////////////////// +TEST_F(ForceTorqueSensorTest, CreateForceTorqueSensor) +{ + // Create SDF describing a force torque sensor + const std::string name = "TestForceTorqueSensor"; + const std::string topic = "/ignition/sensors/test/force_torque"; + + // const std::string topicNoise = "/ignition/sensors/test/altimeter_noise"; + const double updateRate = 30; + const bool alwaysOn = 1; + const bool visualize = 1; + + // Create sensor SDF + ignition::math::Pose3d sensorPose(ignition::math::Vector3d(0.25, 0.0, 0.5), + ignition::math::Quaterniond::Identity); + sdf::ElementPtr forcetorqueSdf; + CreateForceTorqueToSdf(name, sensorPose, updateRate, topic, alwaysOn, + visualize, {}, {}, {}, {}, forcetorqueSdf); + + ASSERT_NE(nullptr, forcetorqueSdf); + + // create the sensor using sensor factory + ignition::sensors::SensorFactory sf; + auto sensor = + sf.CreateSensor(forcetorqueSdf); + ASSERT_NE(nullptr, sensor); + + EXPECT_EQ(name, sensor->Name()); + EXPECT_EQ(topic, sensor->Topic()); + EXPECT_DOUBLE_EQ(updateRate, sensor->UpdateRate()); +} + +///////////////////////////////////////////////// +TEST_F(ForceTorqueSensorTest, SensorReadings) +{ + namespace math = ignition::math; + + // Create SDF describing a a force torque sensor + const std::string name = "TestForceTorqueSensor"; + const std::string topic = "/ignition/sensors/test/force_torque"; + // const std::string topicNoise = "/ignition/sensors/test/altimeter_noise"; + const double updateRate = 30; + const bool alwaysOn = 1; + const bool visualize = 1; + + const math::Vector3d forceNoiseMean{0.1, 0.2, 0.3}; + // Set the stddev to 0s so we can test the mean values + const math::Vector3d forceNoiseStddev{0.0, 0.0, 0.0}; + + const math::Vector3d torqueNoiseMean{0.5, 0.6, 0.7}; + // Set the stddev to 0s so we can test the mean values + const math::Vector3d torqueNoiseStddev{0.0, 0.0, 0.0}; + + // Create sensor SDF + math::Pose3d sensorPose{math::Vector3d(0.25, 0.0, 0.5), + math::Quaterniond::Identity}; + sdf::ElementPtr forcetorqueSdf; + CreateForceTorqueToSdf(name, sensorPose, updateRate, topic, alwaysOn, + visualize, forceNoiseMean, forceNoiseStddev, + torqueNoiseMean, torqueNoiseStddev, forcetorqueSdf); + + ASSERT_NE(nullptr, forcetorqueSdf); + + // create the sensor using sensor factory + // try creating without specifying the sensor type and then cast it + ignition::sensors::SensorFactory sf; + auto sensor = + sf.CreateSensor(forcetorqueSdf); + + ASSERT_NE(nullptr, sensor); + + // verify initial readings + EXPECT_EQ(math::Vector3d::Zero, sensor->Force()); + EXPECT_EQ(math::Vector3d::Zero, sensor->Torque()); + EXPECT_EQ(math::Quaterniond::Identity, sensor->RotationParentInSensor()); + EXPECT_EQ(math::Quaterniond::Identity, sensor->RotationChildInSensor()); + + // set state and verify readings + math::Vector3d force{0, 0, 1}; + sensor->SetForce(force); + EXPECT_EQ(force, sensor->Force()); + + math::Vector3d torque{0, 1, 0}; + sensor->SetTorque(torque); + EXPECT_EQ(torque, sensor->Torque()); + + // verify msg received on the topic + WaitForMessageTestHelper msgHelper(topic); + auto dt = std::chrono::steady_clock::duration(std::chrono::seconds(1)); + { + sensor->Update(dt); + EXPECT_TRUE(msgHelper.WaitForMessage()) << msgHelper; + auto msg = msgHelper.Message(); + EXPECT_EQ(1, msg.header().stamp().sec()); + EXPECT_EQ(0, msg.header().stamp().nsec()); + + EXPECT_EQ(force + forceNoiseMean, ignition::msgs::Convert(msg.force())); + EXPECT_EQ(torque + torqueNoiseMean, ignition::msgs::Convert(msg.torque())); + + // The Force() and Torque() functions return the noise-free forces and + // torques in the sensor frame + EXPECT_EQ(force, sensor->Force()); + EXPECT_EQ(torque, sensor->Torque()); + } + + // Set rotation of child + const math::Quaterniond rotChildInSensor{ + math::Vector3d{IGN_PI_4, IGN_PI_2, 0}}; + sensor->SetRotationChildInSensor(rotChildInSensor); + EXPECT_EQ(rotChildInSensor, sensor->RotationChildInSensor()); + { + sensor->Update(dt); + EXPECT_TRUE(msgHelper.WaitForMessage()) << msgHelper; + auto msg = msgHelper.Message(); + + EXPECT_EQ((rotChildInSensor.Inverse() * force) + forceNoiseMean, + ignition::msgs::Convert(msg.force())); + EXPECT_EQ((rotChildInSensor.Inverse() * torque) + torqueNoiseMean, + ignition::msgs::Convert(msg.torque())); + // The Force() and Torque() functions return the noise-free forces and + // torques in the sensor frame + EXPECT_EQ(force, sensor->Force()); + EXPECT_EQ(torque, sensor->Torque()); + } + + // Set rotation of parent. This should not affect the ouptut because the + // measurement frame is "child". + const math::Quaterniond rotParentInSensor{math::Vector3d{0.1, 0.2, 0.3}}; + sensor->SetRotationParentInSensor(rotParentInSensor); + EXPECT_EQ(rotParentInSensor, sensor->RotationParentInSensor()); + { + sensor->Update(dt); + EXPECT_TRUE(msgHelper.WaitForMessage()) << msgHelper; + auto msg = msgHelper.Message(); + + // Using rotChildInSensor since the measurement frame is still "child" + EXPECT_EQ((rotChildInSensor.Inverse() * force) + forceNoiseMean, + ignition::msgs::Convert(msg.force())); + EXPECT_EQ((rotChildInSensor.Inverse() * torque) + torqueNoiseMean, + ignition::msgs::Convert(msg.torque())); + // The Force() and Torque() functions return the noise-free forces and + // torques in the sensor frame + EXPECT_EQ(force, sensor->Force()); + EXPECT_EQ(torque, sensor->Torque()); + } +} + +int main(int argc, char **argv) +{ + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/test/integration/force_torque_plugin.cc b/test/integration/force_torque_plugin.cc deleted file mode 100644 index a83fbc94..00000000 --- a/test/integration/force_torque_plugin.cc +++ /dev/null @@ -1,311 +0,0 @@ -/* - * Copyright (C) 2021 Open Source Robotics Foundation - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - * -*/ - -#include - -#include - -#include - -#include -#include - -#include "test_config.h" // NOLINT(build/include) -#include "TransportTestTools.hh" - -/// \brief Helper function to create a force torque sdf element. -sdf::ElementPtr ForceTorqueToSdf(const std::string &_name, - const ignition::math::Pose3d &_pose, const double _updateRate, - const std::string &_topic, const bool _alwaysOn, - const bool _visualize) -{ - std::ostringstream stream; - stream - << "" - << "" - << " " - << " " - << " " - << " " << _pose << "" - << " " << _topic << "" - << " "<< _updateRate <<"" - << " " << _alwaysOn <<"" - << " " << _visualize << "" - << " " - << " " - << " " - << ""; - - sdf::SDFPtr sdfParsed(new sdf::SDF()); - sdf::init(sdfParsed); - if (!sdf::readString(stream.str(), sdfParsed)) - return sdf::ElementPtr(); - - return sdfParsed->Root()->GetElement("model")->GetElement("link") - ->GetElement("sensor"); -} - -// /// \brief Helper function to create an altimeter sdf element with noise -// sdf::ElementPtr AltimeterToSdfWithNoise(const std::string &_name, -// const ignition::math::Pose3d &_pose, const double _updateRate, -// const std::string &_topic, const bool _alwaysOn, -// const bool _visualize, double _mean, double _stddev, double _bias) -// { -// std::ostringstream stream; -// stream -// << "" -// << "" -// << " " -// << " " -// << " " -// << " " << _pose << "" -// << " " << _topic << "" -// << " "<< _updateRate <<"" -// << " " << _alwaysOn <<"" -// << " " << _visualize << "" -// << " " -// << " " -// << " " -// << " " << _mean << "" -// << " " << _stddev << "" -// << " " << _bias << "" -// << " " -// << " " -// << " " -// << " " -// << " " << _mean << "" -// << " " << _stddev << "" -// << " " << _bias << "" -// << " " -// << " " -// << " " -// << " " -// << " " -// << " " -// << ""; - -// sdf::SDFPtr sdfParsed(new sdf::SDF()); -// sdf::init(sdfParsed); -// if (!sdf::readString(stream.str(), sdfParsed)) -// return sdf::ElementPtr(); - -// return sdfParsed->Root()->GetElement("model")->GetElement("link") -// ->GetElement("sensor"); -// } - -/// \brief Test force torque sensor -class ForceTorqueSensorTest: public testing::Test -{ - // Documentation inherited - protected: void SetUp() override - { - ignition::common::Console::SetVerbosity(4); - } -}; - -///////////////////////////////////////////////// -TEST_F(ForceTorqueSensorTest, CreateForceTorqueSensor) -{ - // Create SDF describing a force torque sensor - const std::string name = "TestForceTorqueSensor"; - const std::string topic = "/ignition/sensors/test/force_torque"; - - // const std::string topicNoise = "/ignition/sensors/test/altimeter_noise"; - const double updateRate = 30; - const bool alwaysOn = 1; - const bool visualize = 1; - - // Create sensor SDF - ignition::math::Pose3d sensorPose(ignition::math::Vector3d(0.25, 0.0, 0.5), - ignition::math::Quaterniond::Identity); - sdf::ElementPtr forcetorqueSdf = ForceTorqueToSdf(name, sensorPose, - updateRate, topic, alwaysOn, visualize); - - // sdf::ElementPtr altimeterSdfNoise = AltimeterToSdfWithNoise(name, sensorPose, - // updateRate, topicNoise, alwaysOn, visualize, 1.0, 0.2, 10.0); - - // create the sensor using sensor factory - ignition::sensors::SensorFactory sf; - std::unique_ptr sensor = - sf.CreateSensor(forcetorqueSdf); - ASSERT_NE(nullptr, sensor); - - EXPECT_EQ(name, sensor->Name()); - EXPECT_EQ(topic, sensor->Topic()); - EXPECT_DOUBLE_EQ(updateRate, sensor->UpdateRate()); - - // std::unique_ptr sensorNoise = - // sf.CreateSensor(altimeterSdfNoise); - // ASSERT_NE(nullptr, sensorNoise); - - // EXPECT_EQ(name, sensorNoise->Name()); - // EXPECT_EQ(topicNoise, sensorNoise->Topic()); - // EXPECT_DOUBLE_EQ(updateRate, sensorNoise->UpdateRate()); -} - -// ///////////////////////////////////////////////// -TEST_F(ForceTorqueSensorTest, SensorReadings) -{ - // Create SDF describing a a force torque sensor - const std::string name = "TestForceTorqueSensor"; - const std::string topic = "/ignition/sensors/test/force_torque"; - // const std::string topicNoise = "/ignition/sensors/test/altimeter_noise"; - const double updateRate = 30; - const bool alwaysOn = 1; - const bool visualize = 1; - - // Create sensor SDF - ignition::math::Pose3d sensorPose(ignition::math::Vector3d(0.25, 0.0, 0.5), - ignition::math::Quaterniond::Identity); - sdf::ElementPtr forcetorqueSdf = ForceTorqueToSdf(name, sensorPose, - updateRate, topic, alwaysOn, visualize); - - // sdf::ElementPtr altimeterSdfNoise = AltimeterToSdfWithNoise(name, sensorPose, - // updateRate, topicNoise, alwaysOn, visualize, 1.0, 0.2, 10.0); - - // create the sensor using sensor factory - // try creating without specifying the sensor type and then cast it - ignition::sensors::SensorFactory sf; - std::unique_ptr s = - sf.CreateSensor(forcetorqueSdf); - std::unique_ptr sensor( - dynamic_cast(s.release())); - - // Make sure the above dynamic cast worked. - ASSERT_NE(nullptr, sensor); - - // std::unique_ptr sNoise = - // sf.CreateSensor(altimeterSdfNoise); - // std::unique_ptr sensorNoise( - // dynamic_cast(sNoise.release())); - - // Make sure the above dynamic cast worked. - // ASSERT_NE(nullptr, sensorNoise); - - // verify initial readings - EXPECT_EQ(ignition::math::Vector3d::Zero, sensor->Force()); - EXPECT_EQ(ignition::math::Vector3d::Zero, sensor->Torque()); - - // verify initial readings - // EXPECT_DOUBLE_EQ(0.0, sensorNoise->force()); - // EXPECT_DOUBLE_EQ(0.0, sensorNoise->torque()); - - // set state and verify readings - - // TODO : Add Noise tests - ignition::math::Vector3d force(0,0,1); - sensor->SetForce(force); - EXPECT_EQ(force, sensor->Force()); - - ignition::math::Vector3d torque(0,0,1); - sensor->SetTorque(torque); - EXPECT_EQ(torque, sensor->Torque()); - - - // double pos = 2.0; - // sensor->SetPosition(pos); - // sensorNoise->SetPosition(pos); - // EXPECT_DOUBLE_EQ(pos - vertRef, sensor->VerticalPosition()); - // EXPECT_DOUBLE_EQ(pos - vertRef, sensorNoise->VerticalPosition()); - - // verify msg received on the topic - WaitForMessageTestHelper msgHelper(topic); - sensor->Update(std::chrono::steady_clock::duration(std::chrono::seconds(1))); - EXPECT_TRUE(msgHelper.WaitForMessage()) << msgHelper; - auto msg = msgHelper.Message(); - EXPECT_EQ(1, msg.header().stamp().sec()); - EXPECT_EQ(0, msg.header().stamp().nsec()); - - EXPECT_EQ(ignition::math::Vector3d(0, 0, 1), ignition::msgs::Convert(msg.force())); - EXPECT_EQ(ignition::math::Vector3d(0, 0, 1), ignition::msgs::Convert(msg.torque())); - - - // // verify msg with noise received on the topic - // WaitForMessageTestHelper - // msgHelperNoise(topicNoise); - // sensorNoise->Update(std::chrono::steady_clock::duration( - // std::chrono::seconds(1))); - // EXPECT_TRUE(msgHelperNoise.WaitForMessage()) << msgHelperNoise; - // auto msgNoise = msgHelperNoise.Message(); - // EXPECT_EQ(1, msg.header().stamp().sec()); - // EXPECT_EQ(0, msg.header().stamp().nsec()); - // EXPECT_DOUBLE_EQ(vertRef, msgNoise.vertical_reference()); - // EXPECT_FALSE(ignition::math::equal(pos - vertRef, - // msgNoise.vertical_position())); - // EXPECT_FALSE(ignition::math::equal(vertVel, msgNoise.vertical_velocity())); -} - -///////////////////////////////////////////////// -TEST_F(ForceTorqueSensorTest, Topic) -{ - const std::string name = "TestForceTorqueSensor"; - const double updateRate = 30; - const bool alwaysOn = 1; - const bool visualize = 1; - auto sensorPose = ignition::math::Pose3d(); - - // Factory - ignition::sensors::SensorFactory factory; - - // Default topic - { - const std::string topic; - auto forcetorqueSdf = ForceTorqueToSdf(name, sensorPose, - updateRate, topic, alwaysOn, visualize); - - auto sensor = factory.CreateSensor(forcetorqueSdf); - EXPECT_NE(nullptr, sensor); - - auto force_torque = - dynamic_cast(sensor.release()); - ASSERT_NE(nullptr, force_torque); - - EXPECT_EQ("/forcetorque", force_torque->Topic()); - } - - // Convert to valid topic - { - const std::string topic = "/topic with spaces/@~characters//"; - auto forcetorqueSdf = ForceTorqueToSdf(name, sensorPose, - updateRate, topic, alwaysOn, visualize); - - auto sensor = factory.CreateSensor(forcetorqueSdf); - EXPECT_NE(nullptr, sensor); - - auto force_torque = - dynamic_cast(sensor.release()); - ASSERT_NE(nullptr, force_torque); - - EXPECT_EQ("/topic_with_spaces/characters", force_torque->Topic()); - } - - // Invalid topic - { - const std::string topic = "@@@"; - auto forcetorqueSdf = ForceTorqueToSdf(name, sensorPose, - updateRate, topic, alwaysOn, visualize); - - auto sensor = factory.CreateSensor(forcetorqueSdf); - ASSERT_EQ(nullptr, sensor); - } -} - -int main(int argc, char **argv) -{ - ::testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} From 67762c257346e355714059a2b61360177b479db4 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Wed, 22 Sep 2021 00:50:53 -0500 Subject: [PATCH 15/18] Fix duplicate sign changes Signed-off-by: Addisu Z. Taddese --- include/ignition/sensors/ForceTorqueSensor.hh | 2 +- src/ForceTorqueSensor.cc | 15 --------------- test/integration/force_torque.cc | 12 +++++------- 3 files changed, 6 insertions(+), 23 deletions(-) diff --git a/include/ignition/sensors/ForceTorqueSensor.hh b/include/ignition/sensors/ForceTorqueSensor.hh index 80868d76..303b51dc 100644 --- a/include/ignition/sensors/ForceTorqueSensor.hh +++ b/include/ignition/sensors/ForceTorqueSensor.hh @@ -87,7 +87,7 @@ namespace ignition /// \return The latest measured torque. public: math::Vector3d Torque() const; - /// \brief Set the torque vector in sensor frame and where the torque is + /// \brief Set the torque vector in sensor frame and where the torque is /// applied on the child (parent-to-child) /// \param[in] _torque torque vector in newton. public: void SetTorque(const math::Vector3d &_torque); diff --git a/src/ForceTorqueSensor.cc b/src/ForceTorqueSensor.cc index 4de1a252..05ade160 100644 --- a/src/ForceTorqueSensor.cc +++ b/src/ForceTorqueSensor.cc @@ -205,7 +205,6 @@ bool ForceTorqueSensor::Update(const std::chrono::steady_clock::duration &_now) this->dataPtr->rotationParentInSensor.Inverse() * this->dataPtr->force; measuredTorque = this->dataPtr->rotationParentInSensor.Inverse() * this->dataPtr->torque; - } else if (this->dataPtr->measureFrame == sdf::ForceTorqueFrame::CHILD) { @@ -213,25 +212,11 @@ bool ForceTorqueSensor::Update(const std::chrono::steady_clock::duration &_now) this->dataPtr->rotationChildInSensor.Inverse() * this->dataPtr->force; measuredTorque = this->dataPtr->rotationChildInSensor.Inverse() * this->dataPtr->torque; - - if (this->dataPtr->measureDirection == - sdf::ForceTorqueMeasureDirection::CHILD_TO_PARENT) - { - measuredForce *= -1; - measuredTorque *= -1; - } } else if (this->dataPtr->measureFrame == sdf::ForceTorqueFrame::SENSOR) { measuredForce = this->dataPtr->force; measuredTorque = this->dataPtr->torque; - - if (this->dataPtr->measureDirection == - sdf::ForceTorqueMeasureDirection::CHILD_TO_PARENT) - { - measuredForce *= -1; - measuredTorque *= -1; - } } else { diff --git a/test/integration/force_torque.cc b/test/integration/force_torque.cc index 4b36f674..580bb521 100644 --- a/test/integration/force_torque.cc +++ b/test/integration/force_torque.cc @@ -56,7 +56,7 @@ void CreateForceTorqueToSdf(const std::string &_name, << " " << _visualize << "" << " " << " child" - << " child_to_parent" + << " parent_to_child" << " " << " " << " " @@ -138,7 +138,6 @@ TEST_F(ForceTorqueSensorTest, CreateForceTorqueSensor) const std::string name = "TestForceTorqueSensor"; const std::string topic = "/ignition/sensors/test/force_torque"; - // const std::string topicNoise = "/ignition/sensors/test/altimeter_noise"; const double updateRate = 30; const bool alwaysOn = 1; const bool visualize = 1; @@ -171,7 +170,6 @@ TEST_F(ForceTorqueSensorTest, SensorReadings) // Create SDF describing a a force torque sensor const std::string name = "TestForceTorqueSensor"; const std::string topic = "/ignition/sensors/test/force_torque"; - // const std::string topicNoise = "/ignition/sensors/test/altimeter_noise"; const double updateRate = 30; const bool alwaysOn = 1; const bool visualize = 1; @@ -230,7 +228,7 @@ TEST_F(ForceTorqueSensorTest, SensorReadings) EXPECT_EQ(force + forceNoiseMean, ignition::msgs::Convert(msg.force())); EXPECT_EQ(torque + torqueNoiseMean, ignition::msgs::Convert(msg.torque())); - // The Force() and Torque() functions return the noise-free forces and + // The Force() and Torque() functions return the noise-free forces and // torques in the sensor frame EXPECT_EQ(force, sensor->Force()); EXPECT_EQ(torque, sensor->Torque()); @@ -250,13 +248,13 @@ TEST_F(ForceTorqueSensorTest, SensorReadings) ignition::msgs::Convert(msg.force())); EXPECT_EQ((rotChildInSensor.Inverse() * torque) + torqueNoiseMean, ignition::msgs::Convert(msg.torque())); - // The Force() and Torque() functions return the noise-free forces and + // The Force() and Torque() functions return the noise-free forces and // torques in the sensor frame EXPECT_EQ(force, sensor->Force()); EXPECT_EQ(torque, sensor->Torque()); } - // Set rotation of parent. This should not affect the ouptut because the + // Set rotation of parent. This should not affect the output because the // measurement frame is "child". const math::Quaterniond rotParentInSensor{math::Vector3d{0.1, 0.2, 0.3}}; sensor->SetRotationParentInSensor(rotParentInSensor); @@ -271,7 +269,7 @@ TEST_F(ForceTorqueSensorTest, SensorReadings) ignition::msgs::Convert(msg.force())); EXPECT_EQ((rotChildInSensor.Inverse() * torque) + torqueNoiseMean, ignition::msgs::Convert(msg.torque())); - // The Force() and Torque() functions return the noise-free forces and + // The Force() and Torque() functions return the noise-free forces and // torques in the sensor frame EXPECT_EQ(force, sensor->Force()); EXPECT_EQ(torque, sensor->Torque()); From f615e18534c194fc2aac047c67868b1c86c45a7c Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Wed, 22 Sep 2021 02:05:15 -0500 Subject: [PATCH 16/18] Test force/torque sensor frame and measurement directions Signed-off-by: Addisu Z. Taddese --- test/integration/force_torque.cc | 133 +++++++++++++++++++------------ 1 file changed, 80 insertions(+), 53 deletions(-) diff --git a/test/integration/force_torque.cc b/test/integration/force_torque.cc index 580bb521..4be3ce17 100644 --- a/test/integration/force_torque.cc +++ b/test/integration/force_torque.cc @@ -32,6 +32,8 @@ void CreateForceTorqueToSdf(const std::string &_name, const ignition::math::Pose3d &_pose, const double _updateRate, const std::string &_topic, const bool _alwaysOn, const bool _visualize, + const std::string &_frame, + const std::string &_measureDir, const ignition::math::Vector3d &_forceNoiseMean, const ignition::math::Vector3d &_forceNoiseStddev, const ignition::math::Vector3d &_torqueNoiseMean, @@ -55,8 +57,8 @@ void CreateForceTorqueToSdf(const std::string &_name, << " " << _alwaysOn <<"" << " " << _visualize << "" << " " - << " child" - << " parent_to_child" + << " " << _frame << "" + << " " << _measureDir << "" << " " << " " << " " @@ -122,7 +124,8 @@ void CreateForceTorqueToSdf(const std::string &_name, } /// \brief Test force torque sensor -class ForceTorqueSensorTest: public testing::Test +class ForceTorqueSensorTest + : public testing::TestWithParam> { // Documentation inherited protected: void SetUp() override @@ -147,7 +150,8 @@ TEST_F(ForceTorqueSensorTest, CreateForceTorqueSensor) ignition::math::Quaterniond::Identity); sdf::ElementPtr forcetorqueSdf; CreateForceTorqueToSdf(name, sensorPose, updateRate, topic, alwaysOn, - visualize, {}, {}, {}, {}, forcetorqueSdf); + visualize, "child", "child_to_parent", {}, {}, {}, {}, + forcetorqueSdf); ASSERT_NE(nullptr, forcetorqueSdf); @@ -163,10 +167,13 @@ TEST_F(ForceTorqueSensorTest, CreateForceTorqueSensor) } ///////////////////////////////////////////////// -TEST_F(ForceTorqueSensorTest, SensorReadings) +TEST_P(ForceTorqueSensorTest, SensorReadings) { namespace math = ignition::math; + std::string frame, measureDirection; + std::tie(frame, measureDirection) = GetParam(); + // Create SDF describing a a force torque sensor const std::string name = "TestForceTorqueSensor"; const std::string topic = "/ignition/sensors/test/force_torque"; @@ -186,9 +193,11 @@ TEST_F(ForceTorqueSensorTest, SensorReadings) math::Pose3d sensorPose{math::Vector3d(0.25, 0.0, 0.5), math::Quaterniond::Identity}; sdf::ElementPtr forcetorqueSdf; + CreateForceTorqueToSdf(name, sensorPose, updateRate, topic, alwaysOn, - visualize, forceNoiseMean, forceNoiseStddev, - torqueNoiseMean, torqueNoiseStddev, forcetorqueSdf); + visualize, frame, measureDirection, forceNoiseMean, + forceNoiseStddev, torqueNoiseMean, torqueNoiseStddev, + forcetorqueSdf); ASSERT_NE(nullptr, forcetorqueSdf); @@ -218,64 +227,82 @@ TEST_F(ForceTorqueSensorTest, SensorReadings) // verify msg received on the topic WaitForMessageTestHelper msgHelper(topic); auto dt = std::chrono::steady_clock::duration(std::chrono::seconds(1)); - { - sensor->Update(dt); - EXPECT_TRUE(msgHelper.WaitForMessage()) << msgHelper; - auto msg = msgHelper.Message(); - EXPECT_EQ(1, msg.header().stamp().sec()); - EXPECT_EQ(0, msg.header().stamp().nsec()); - - EXPECT_EQ(force + forceNoiseMean, ignition::msgs::Convert(msg.force())); - EXPECT_EQ(torque + torqueNoiseMean, ignition::msgs::Convert(msg.torque())); - - // The Force() and Torque() functions return the noise-free forces and - // torques in the sensor frame - EXPECT_EQ(force, sensor->Force()); - EXPECT_EQ(torque, sensor->Torque()); - } // Set rotation of child const math::Quaterniond rotChildInSensor{ math::Vector3d{IGN_PI_4, IGN_PI_2, 0}}; + const math::Quaterniond rotParentInSensor{ + math::Vector3d{0, IGN_PI_4, IGN_PI_4}}; + sensor->SetRotationChildInSensor(rotChildInSensor); EXPECT_EQ(rotChildInSensor, sensor->RotationChildInSensor()); - { - sensor->Update(dt); - EXPECT_TRUE(msgHelper.WaitForMessage()) << msgHelper; - auto msg = msgHelper.Message(); - - EXPECT_EQ((rotChildInSensor.Inverse() * force) + forceNoiseMean, - ignition::msgs::Convert(msg.force())); - EXPECT_EQ((rotChildInSensor.Inverse() * torque) + torqueNoiseMean, - ignition::msgs::Convert(msg.torque())); - // The Force() and Torque() functions return the noise-free forces and - // torques in the sensor frame - EXPECT_EQ(force, sensor->Force()); - EXPECT_EQ(torque, sensor->Torque()); - } - - // Set rotation of parent. This should not affect the output because the - // measurement frame is "child". - const math::Quaterniond rotParentInSensor{math::Vector3d{0.1, 0.2, 0.3}}; sensor->SetRotationParentInSensor(rotParentInSensor); EXPECT_EQ(rotParentInSensor, sensor->RotationParentInSensor()); + + sensor->Update(dt); + EXPECT_TRUE(msgHelper.WaitForMessage()) << msgHelper; + auto msg = msgHelper.Message(); + EXPECT_EQ(1, msg.header().stamp().sec()); + EXPECT_EQ(0, msg.header().stamp().nsec()); + + if (measureDirection == "parent_to_child") + { + if (frame == "child") + { + EXPECT_EQ((rotChildInSensor.Inverse() * force) + forceNoiseMean, + ignition::msgs::Convert(msg.force())); + EXPECT_EQ((rotChildInSensor.Inverse() * torque) + torqueNoiseMean, + ignition::msgs::Convert(msg.torque())); + } + else if (frame == "parent") + { + EXPECT_EQ((rotParentInSensor.Inverse() * force) + forceNoiseMean, + ignition::msgs::Convert(msg.force())); + EXPECT_EQ((rotParentInSensor.Inverse() * torque) + torqueNoiseMean, + ignition::msgs::Convert(msg.torque())); + } + else + { + EXPECT_EQ(force + forceNoiseMean, ignition::msgs::Convert(msg.force())); + EXPECT_EQ(torque + torqueNoiseMean, + ignition::msgs::Convert(msg.torque())); + } + } + else { - sensor->Update(dt); - EXPECT_TRUE(msgHelper.WaitForMessage()) << msgHelper; - auto msg = msgHelper.Message(); - - // Using rotChildInSensor since the measurement frame is still "child" - EXPECT_EQ((rotChildInSensor.Inverse() * force) + forceNoiseMean, - ignition::msgs::Convert(msg.force())); - EXPECT_EQ((rotChildInSensor.Inverse() * torque) + torqueNoiseMean, - ignition::msgs::Convert(msg.torque())); - // The Force() and Torque() functions return the noise-free forces and - // torques in the sensor frame - EXPECT_EQ(force, sensor->Force()); - EXPECT_EQ(torque, sensor->Torque()); + if (frame == "child") + { + EXPECT_EQ(-(rotChildInSensor.Inverse() * force) + forceNoiseMean, + ignition::msgs::Convert(msg.force())); + EXPECT_EQ(-(rotChildInSensor.Inverse() * torque) + torqueNoiseMean, + ignition::msgs::Convert(msg.torque())); + } + else if (frame == "parent") + { + EXPECT_EQ(-(rotParentInSensor.Inverse() * force) + forceNoiseMean, + ignition::msgs::Convert(msg.force())); + EXPECT_EQ(-(rotParentInSensor.Inverse() * torque) + torqueNoiseMean, + ignition::msgs::Convert(msg.torque())); + } + else + { + EXPECT_EQ(-force + forceNoiseMean, ignition::msgs::Convert(msg.force())); + EXPECT_EQ(-torque + torqueNoiseMean, + ignition::msgs::Convert(msg.torque())); + } } + // The Force() and Torque() functions return the noise-free forces and + // torques in the sensor frame + EXPECT_EQ(force, sensor->Force()); + EXPECT_EQ(torque, sensor->Torque()); } +INSTANTIATE_TEST_CASE_P( + FrameAndDirection, ForceTorqueSensorTest, + ::testing::Combine(::testing::Values("child", "parent", "sensor"), + ::testing::Values("parent_to_child", + "child_to_parent")),); + int main(int argc, char **argv) { ::testing::InitGoogleTest(&argc, argv); From 5f161997aff9f9f6b1eefb80b9e14a1241ef4348 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Wed, 22 Sep 2021 04:11:08 -0500 Subject: [PATCH 17/18] Codecheck Signed-off-by: Addisu Z. Taddese --- test/integration/force_torque.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/test/integration/force_torque.cc b/test/integration/force_torque.cc index 4be3ce17..45539443 100644 --- a/test/integration/force_torque.cc +++ b/test/integration/force_torque.cc @@ -301,7 +301,7 @@ INSTANTIATE_TEST_CASE_P( FrameAndDirection, ForceTorqueSensorTest, ::testing::Combine(::testing::Values("child", "parent", "sensor"), ::testing::Values("parent_to_child", - "child_to_parent")),); + "child_to_parent")), ); int main(int argc, char **argv) { From fe1ff32c96cb27f3c871a948b3d0eddb5d17edcf Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Wed, 22 Sep 2021 09:54:29 -0500 Subject: [PATCH 18/18] Use std::make_unique Signed-off-by: Michael Carroll --- src/ForceTorqueSensor.cc | 2 +- test/integration/air_pressure.cc | 1 - test/integration/altimeter.cc | 1 - test/integration/force_torque.cc | 1 - test/integration/imu.cc | 1 - test/integration/magnetometer.cc | 1 - 6 files changed, 1 insertion(+), 6 deletions(-) diff --git a/src/ForceTorqueSensor.cc b/src/ForceTorqueSensor.cc index 05ade160..8cb4d636 100644 --- a/src/ForceTorqueSensor.cc +++ b/src/ForceTorqueSensor.cc @@ -89,7 +89,7 @@ class ignition::sensors::ForceTorqueSensorPrivate ////////////////////////////////////////////////// ForceTorqueSensor::ForceTorqueSensor() - : dataPtr(new ForceTorqueSensorPrivate()) + : dataPtr(std::make_unique()) { } diff --git a/test/integration/air_pressure.cc b/test/integration/air_pressure.cc index 347b2649..f0670319 100644 --- a/test/integration/air_pressure.cc +++ b/test/integration/air_pressure.cc @@ -170,7 +170,6 @@ TEST_F(AirPressureSensorTest, SensorReadings) sensorPose, updateRate, topicNoise, alwaysOn, visualize, 1.0, 0.2, 10.0); // create the sensor using sensor factory - // try creating without specifying the sensor type and then cast it ignition::sensors::SensorFactory sf; auto sensor = sf.CreateSensor( airPressureSdf); diff --git a/test/integration/altimeter.cc b/test/integration/altimeter.cc index 653f07df..f568d185 100644 --- a/test/integration/altimeter.cc +++ b/test/integration/altimeter.cc @@ -176,7 +176,6 @@ TEST_F(AltimeterSensorTest, SensorReadings) updateRate, topicNoise, alwaysOn, visualize, 1.0, 0.2, 10.0); // create the sensor using sensor factory - // try creating without specifying the sensor type and then cast it ignition::sensors::SensorFactory sf; auto sensor = sf.CreateSensor(altimeterSdf); diff --git a/test/integration/force_torque.cc b/test/integration/force_torque.cc index 45539443..b28b3ebf 100644 --- a/test/integration/force_torque.cc +++ b/test/integration/force_torque.cc @@ -202,7 +202,6 @@ TEST_P(ForceTorqueSensorTest, SensorReadings) ASSERT_NE(nullptr, forcetorqueSdf); // create the sensor using sensor factory - // try creating without specifying the sensor type and then cast it ignition::sensors::SensorFactory sf; auto sensor = sf.CreateSensor(forcetorqueSdf); diff --git a/test/integration/imu.cc b/test/integration/imu.cc index 43c6a991..2b630443 100644 --- a/test/integration/imu.cc +++ b/test/integration/imu.cc @@ -111,7 +111,6 @@ TEST_F(ImuSensorTest, SensorReadings) updateRate, topic, alwaysOn, visualize); // create the sensor using sensor factory - // try creating without specifying the sensor type and then cast it ignition::sensors::SensorFactory sf; auto sensor = sf.CreateSensor(imuSdf); ASSERT_NE(nullptr, sensor); diff --git a/test/integration/magnetometer.cc b/test/integration/magnetometer.cc index a96fe766..4ba223f8 100644 --- a/test/integration/magnetometer.cc +++ b/test/integration/magnetometer.cc @@ -181,7 +181,6 @@ TEST_F(MagnetometerSensorTest, SensorReadings) sensorPose, updateRate, noiseTopic, alwaysOn, visualize, 1.0, 0.2, 10.0); // create the sensor using sensor factory - // try creating without specifying the sensor type and then cast it ignition::sensors::SensorFactory sf; auto sensor = sf.CreateSensor( magnetometerSdf);