Skip to content

Commit

Permalink
Merge 4484116 into 4449749
Browse files Browse the repository at this point in the history
  • Loading branch information
Devansh Chawla authored Sep 21, 2021
2 parents 4449749 + 4484116 commit 6bfff87
Show file tree
Hide file tree
Showing 6 changed files with 299 additions and 5 deletions.
48 changes: 48 additions & 0 deletions include/sdf/ForceTorque.hh
View file
Original file line number Diff line number Diff line change
Expand Up @@ -79,6 +79,54 @@ namespace sdf
/// not been called.
public: sdf::ElementPtr Element() const;

/// \brief Get the force noise values in the measurement frame X-axis.
/// \return Noise values for the X-axis force.
public: const Noise &ForceXNoise() const;

/// \brief Set the force noise values in the measurement frame X-axis.
/// \param[in] _noise Noise values for the X-axis force.
public: void SetForceXNoise(const Noise &_noise);

/// \brief Get the force noise values in the measurement frame Y-axis.
/// \return Noise values for the Y-axis force.
public: const Noise &ForceYNoise() const;

/// \brief Set the force noise values in the measurement frame Y-axis.
/// \param[in] _noise Noise values for the Y-axis force.
public: void SetForceYNoise(const Noise &_noise);

/// \brief Get the force noise values in the measurement frame Z-axis.
/// \return Noise values for the Z-axis force.
public: const Noise &ForceZNoise() const;

/// \brief Set the force noise values in the measurement frame Z-axis.
/// \param[in] _noise Noise values for the Z-axis force.
public: void SetForceZNoise(const Noise &_noise);

/// \brief Get the torque noise values in the measurement frame X-axis.
/// \return Noise values for the X-axis torque.
public: const Noise &TorqueXNoise() const;

/// \brief Set the torque noise values in the measurement frame X-axis.
/// \param[in] _noise Noise values for the X-axis torque.
public: void SetTorqueXNoise(const Noise &_noise);

/// \brief Get the torque noise values in the measurement frame Y-axis.
/// \return Noise values for the Y-axis torque.
public: const Noise &TorqueYNoise() const;

/// \brief Set the torque noise values in the measurement frame Y-axis.
/// \param[in] _noise Noise values for the Y-axis torque.
public: void SetTorqueYNoise(const Noise &_noise);

/// \brief Get the torque noise values in the measurement frame Z-axis.
/// \return Noise values for the Z-axis torque.
public: const Noise &TorqueZNoise() const;

/// \brief Set the torque noise values in the measurement frame Z-axis.
/// \param[in] _noise Noise values for the Z-axis torque.
public: void SetTorqueZNoise(const Noise &_noise);

/// \brief Get the frame in which the wrench values are reported.
/// \return The frame of the wrench values.
public: ForceTorqueFrame Frame() const;
Expand Down
34 changes: 34 additions & 0 deletions sdf/1.9/forcetorque.sdf
View file
Original file line number Diff line number Diff line change
Expand Up @@ -17,4 +17,38 @@
"child_to_parent" if the measured wrench is the one applied by the child link on the parent link.
</description>
</element>

<element name="force" required="0">
<description>These elements are specific to measurement-frame force,
which is expressed in Newtons</description>
<element name="x" required="0">
<description>Force along the X axis</description>
<include filename="noise.sdf" required="0"/>
</element>
<element name="y" required="0">
<description>Force along the Y axis</description>
<include filename="noise.sdf" required="0"/>
</element>
<element name="z" required="0">
<description>Force along the Z axis</description>
<include filename="noise.sdf" required="0"/>
</element>
</element>

<element name="torque" required="0">
<description>These elements are specific to measurement-frame torque,
which is expressed in Newton-meters</description>
<element name="x" required="0">
<description>Torque about the X axis</description>
<include filename="noise.sdf" required="0"/>
</element>
<element name="y" required="0">
<description>Force about the Y axis</description>
<include filename="noise.sdf" required="0"/>
</element>
<element name="z" required="0">
<description>Torque about the Z axis</description>
<include filename="noise.sdf" required="0"/>
</element>
</element>
</element>
127 changes: 126 additions & 1 deletion src/ForceTorque.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -22,6 +22,30 @@ using namespace sdf;
/// \brief Private force torque data.
class sdf::ForceTorque::Implementation
{
/// \brief Noise values related to the body-frame force on the
/// X-axis.
public: Noise forceXNoise;

/// \brief Noise values related to the body-frame force on the
/// Y-axis.
public: Noise forceYNoise;

/// \brief Noise values related to the body-frame force on the
/// Z-axis.
public: Noise forceZNoise;

/// \brief Noise values related to the body-frame torque on the
/// X-axis.
public: Noise torqueXNoise;

/// \brief Noise values related to the body-frame torque on the
/// Y-axis.
public: Noise torqueYNoise;

/// \brief Noise values related to the body-frame torque on the
/// Z-axis.
public: Noise torqueZNoise;

/// \brief Name of the reference frame for the wrench values.
public: ForceTorqueFrame frame = ForceTorqueFrame::CHILD;

Expand Down Expand Up @@ -106,6 +130,29 @@ Errors ForceTorque::Load(ElementPtr _sdf)
}
}

auto loadAxisNoise = [&errors](sdf::ElementPtr _parent,
const std::string _groupLabel,
const std::string _axisLabel,
sdf::Noise& _noise)
{
if (_parent->HasElement(_groupLabel) &&
_parent->GetElement(_groupLabel)->HasElement(_axisLabel))
{
auto axis = _parent->GetElement(_groupLabel)->GetElement(_axisLabel);
sdf::Errors noiseErrors = _noise.Load(axis->GetElement("noise"));
errors.insert(errors.end(), noiseErrors.begin(), noiseErrors.end());
return true;
}
return false;
};

loadAxisNoise(_sdf, "force", "x", this->dataPtr->forceXNoise);
loadAxisNoise(_sdf, "force", "y", this->dataPtr->forceYNoise);
loadAxisNoise(_sdf, "force", "z", this->dataPtr->forceZNoise);
loadAxisNoise(_sdf, "torque", "x", this->dataPtr->torqueXNoise);
loadAxisNoise(_sdf, "torque", "y", this->dataPtr->torqueYNoise);
loadAxisNoise(_sdf, "torque", "z", this->dataPtr->torqueZNoise);

return errors;
}

Expand All @@ -125,7 +172,85 @@ bool ForceTorque::operator!=(const ForceTorque &_ft) const
bool ForceTorque::operator==(const ForceTorque &_ft) const
{
return this->dataPtr->frame == _ft.dataPtr->frame &&
this->dataPtr->measure_direction == _ft.dataPtr->measure_direction;
this->dataPtr->measure_direction == _ft.dataPtr->measure_direction &&
this->dataPtr->forceXNoise == _ft.dataPtr->forceXNoise &&
this->dataPtr->forceYNoise == _ft.dataPtr->forceYNoise &&
this->dataPtr->forceZNoise == _ft.dataPtr->forceZNoise &&
this->dataPtr->torqueXNoise == _ft.dataPtr->torqueXNoise &&
this->dataPtr->torqueYNoise == _ft.dataPtr->torqueYNoise &&
this->dataPtr->torqueZNoise == _ft.dataPtr->torqueZNoise;
}

//////////////////////////////////////////////////
const Noise &ForceTorque::ForceXNoise() const
{
return this->dataPtr->forceXNoise;
}

//////////////////////////////////////////////////
void ForceTorque::SetForceXNoise(const Noise &_noise)
{
this->dataPtr->forceXNoise = _noise;
}

//////////////////////////////////////////////////
const Noise &ForceTorque::ForceYNoise() const
{
return this->dataPtr->forceYNoise;
}

//////////////////////////////////////////////////
void ForceTorque::SetForceYNoise(const Noise &_noise)
{
this->dataPtr->forceYNoise = _noise;
}

//////////////////////////////////////////////////
const Noise &ForceTorque::ForceZNoise() const
{
return this->dataPtr->forceZNoise;
}

//////////////////////////////////////////////////
void ForceTorque::SetForceZNoise(const Noise &_noise)
{
this->dataPtr->forceZNoise = _noise;
}

//////////////////////////////////////////////////
const Noise &ForceTorque::TorqueXNoise() const
{
return this->dataPtr->torqueXNoise;
}

//////////////////////////////////////////////////
void ForceTorque::SetTorqueXNoise(const Noise &_noise)
{
this->dataPtr->torqueXNoise = _noise;
}

//////////////////////////////////////////////////
const Noise &ForceTorque::TorqueYNoise() const
{
return this->dataPtr->torqueYNoise;
}

//////////////////////////////////////////////////
void ForceTorque::SetTorqueYNoise(const Noise &_noise)
{
this->dataPtr->torqueYNoise = _noise;
}

//////////////////////////////////////////////////
const Noise &ForceTorque::TorqueZNoise() const
{
return this->dataPtr->torqueZNoise;
}

//////////////////////////////////////////////////
void ForceTorque::SetTorqueZNoise(const Noise &_noise)
{
this->dataPtr->torqueZNoise = _noise;
}

//////////////////////////////////////////////////
Expand Down
32 changes: 32 additions & 0 deletions src/ForceTorque_TEST.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -22,6 +22,38 @@
TEST(DOMForceTorque, Construction)
{
sdf::ForceTorque ft;
sdf::Noise defaultNoise, noise;

noise.SetType(sdf::NoiseType::GAUSSIAN);
noise.SetMean(1.2);
noise.SetStdDev(2.3);
noise.SetBiasMean(4.5);
noise.SetBiasStdDev(6.7);
noise.SetPrecision(8.9);

EXPECT_EQ(defaultNoise, ft.ForceXNoise());
ft.SetForceXNoise(noise);
EXPECT_EQ(noise, ft.ForceXNoise());

EXPECT_EQ(defaultNoise, ft.ForceYNoise());
ft.SetForceYNoise(noise);
EXPECT_EQ(noise, ft.ForceYNoise());

EXPECT_EQ(defaultNoise, ft.ForceZNoise());
ft.SetForceZNoise(noise);
EXPECT_EQ(noise, ft.ForceZNoise());

EXPECT_EQ(defaultNoise, ft.TorqueXNoise());
ft.SetTorqueXNoise(noise);
EXPECT_EQ(noise, ft.TorqueXNoise());

EXPECT_EQ(defaultNoise, ft.TorqueYNoise());
ft.SetTorqueYNoise(noise);
EXPECT_EQ(noise, ft.TorqueYNoise());

EXPECT_EQ(defaultNoise, ft.TorqueZNoise());
ft.SetTorqueZNoise(noise);
EXPECT_EQ(noise, ft.TorqueZNoise());

EXPECT_EQ(ft.Frame(), sdf::ForceTorqueFrame::CHILD);
ft.SetFrame(sdf::ForceTorqueFrame::PARENT);
Expand Down
23 changes: 19 additions & 4 deletions test/integration/joint_dom.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -890,9 +890,24 @@ TEST(DOMJoint, Sensors)
EXPECT_EQ(sdf::SensorType::FORCE_TORQUE, forceTorqueSensor->Type());
EXPECT_EQ(ignition::math::Pose3d(10, 11, 12, 0, 0, 0),
forceTorqueSensor->RawPose());
auto forceTorqueSensorConfig = forceTorqueSensor->ForceTorqueSensor();
ASSERT_NE(nullptr, forceTorqueSensorConfig);
EXPECT_EQ(sdf::ForceTorqueFrame::PARENT, forceTorqueSensorConfig->Frame());
auto forceTorqueSensorObj = forceTorqueSensor->ForceTorqueSensor();
ASSERT_NE(nullptr, forceTorqueSensorObj);
EXPECT_EQ(sdf::ForceTorqueFrame::PARENT, forceTorqueSensorObj->Frame());
EXPECT_EQ(sdf::ForceTorqueMeasureDirection::PARENT_TO_CHILD,
forceTorqueSensorConfig->MeasureDirection());
forceTorqueSensorObj->MeasureDirection());

EXPECT_DOUBLE_EQ(0.0, forceTorqueSensorObj->ForceXNoise().Mean());
EXPECT_DOUBLE_EQ(0.1, forceTorqueSensorObj->ForceXNoise().StdDev());
EXPECT_DOUBLE_EQ(1.0, forceTorqueSensorObj->ForceYNoise().Mean());
EXPECT_DOUBLE_EQ(1.1, forceTorqueSensorObj->ForceYNoise().StdDev());
EXPECT_DOUBLE_EQ(2.0, forceTorqueSensorObj->ForceZNoise().Mean());
EXPECT_DOUBLE_EQ(2.1, forceTorqueSensorObj->ForceZNoise().StdDev());

EXPECT_DOUBLE_EQ(3.0, forceTorqueSensorObj->TorqueXNoise().Mean());
EXPECT_DOUBLE_EQ(3.1, forceTorqueSensorObj->TorqueXNoise().StdDev());
EXPECT_DOUBLE_EQ(4.0, forceTorqueSensorObj->TorqueYNoise().Mean());
EXPECT_DOUBLE_EQ(4.1, forceTorqueSensorObj->TorqueYNoise().StdDev());
EXPECT_DOUBLE_EQ(5.0, forceTorqueSensorObj->TorqueZNoise().Mean());
EXPECT_DOUBLE_EQ(5.1, forceTorqueSensorObj->TorqueZNoise().StdDev());

}
40 changes: 40 additions & 0 deletions test/sdf/joint_sensors.sdf
View file
Original file line number Diff line number Diff line change
Expand Up @@ -11,6 +11,46 @@
<force_torque>
<frame>parent</frame>
<measure_direction>parent_to_child</measure_direction>
<force>
<x>
<noise type="gaussian">
<mean>0</mean>
<stddev>0.1</stddev>
</noise>
</x>
<y>
<noise type="gaussian">
<mean>1</mean>
<stddev>1.1</stddev>
</noise>
</y>
<z>
<noise type="gaussian">
<mean>2</mean>
<stddev>2.1</stddev>
</noise>
</z>
</force>
<torque>
<x>
<noise type="gaussian">
<mean>3</mean>
<stddev>3.1</stddev>
</noise>
</x>
<y>
<noise type="gaussian">
<mean>4</mean>
<stddev>4.1</stddev>
</noise>
</y>
<z>
<noise type="gaussian">
<mean>5</mean>
<stddev>5.1</stddev>
</noise>
</z>
</torque>
</force_torque>
</sensor>
</joint>
Expand Down

0 comments on commit 6bfff87

Please sign in to comment.