Skip to content

Commit

Permalink
Flip mass shifter (#137)
Browse files Browse the repository at this point in the history 
Signed-off-by: Louise Poubel <louise@openrobotics.org>

Co-authored-by: Mabel Zhang <mabel@openrobotics.org>
  • Loading branch information
@chapulina @mabelzhang
chapulina and mabelzhang committed Nov 2, 2021
1 parent 737fe08 commit 2b07660
Show file tree
Hide file tree
Showing 7 changed files with 101 additions and 63 deletions.
3 changes: 2 additions & 1 deletion lrauv_description/models/tethys/model.sdf
View file
Original file line number Diff line number Diff line change
Expand Up @@ -348,8 +348,9 @@
</axis>
</joint>

<!-- positive joint values move mass forward -->
<joint name="battery_joint" type="prismatic">
<pose>0 0 0 0 0 0</pose>
<pose degrees="true">0 0 0 0 0 180</pose>
<parent>base_link</parent>
<child>battery</child>
<axis>
Expand Down
34 changes: 20 additions & 14 deletions lrauv_ignition_plugins/example/example_mass_shifter.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -24,8 +24,11 @@
* Tests the mass shifter actuator by sending commands to shift the battery
* forward and backward.
*
* Positive mass shifter commands move the battery towards the front, causing the vehicle
* to pitch downwards (more negative pitch). Negative commands do the opposite.
*
* Usage:
* $ LRAUV_example_mass_shifter <vehicle_name>
* $ LRAUV_example_mass_shifter <vehicle_name> <mass_pos_meters>
*/

#include <chrono>
Expand All @@ -43,27 +46,30 @@ int main(int _argc, char **_argv)
vehicleName = _argv[1];
}

double pos{0.001};
if (_argc > 2)
{
pos = atof(_argv[2]);
}

ignition::transport::Node node;
auto commandTopic = "/" + vehicleName + "/command_topic";
auto commandPub =
node.Advertise<lrauv_ignition_plugins::msgs::LRAUVCommand>(commandTopic);

// Negative moves toward nose of vehicle; positive moves toward tail
double dist = -0.001;
while (!commandPub.HasConnections())
{
std::cout << "Command publisher waiting for connections..." << std::endl;
std::this_thread::sleep_for(std::chrono::milliseconds(1000));
}

std::this_thread::sleep_for(std::chrono::milliseconds(1000));
lrauv_ignition_plugins::msgs::LRAUVCommand batteryMsg;
batteryMsg.set_masspositionaction_(dist);
batteryMsg.set_masspositionaction_(pos);

// Keep it stable
batteryMsg.set_buoyancyaction_(0.0005);
batteryMsg.set_dropweightstate_(1);
commandPub.Publish(batteryMsg);
std::cout << "Commanding mass shifter to " << dist << std::endl;

std::this_thread::sleep_for(std::chrono::milliseconds(1000));
lrauv_ignition_plugins::msgs::LRAUVCommand batteryReverseMsg;
batteryReverseMsg.set_masspositionaction_(-dist);
batteryReverseMsg.set_dropweightstate_(1);
batteryReverseMsg.set_buoyancyaction_(0.0005);
commandPub.Publish(batteryReverseMsg);
std::cout << "Commanding mass shifter to " << -dist << std::endl;
commandPub.Publish(batteryMsg);
std::cout << "Commanding mass shifter to [" << pos << "] m" << std::endl;
}
10 changes: 8 additions & 2 deletions lrauv_ignition_plugins/proto/lrauv_command.proto
View file
Original file line number Diff line number Diff line change
Expand Up @@ -44,7 +44,10 @@ message LRAUVCommand
// rotated more clockwise when looking
// from the top (i.e. to starboard)
float elevatorAngle_ = 4;
float massPosition_ = 5;
float massPosition_ = 5; // Position where the controller believes
// the mass shifter's joint is in. Unit:
// meters. Positive values have the battery
// forward, tilting the nose downward.
float buoyancyPosition_ = 6;
bool dropWeightState_ = 7; // Indicator "dropweight OK"
// 1 = in place, 0 = dropped
Expand All @@ -55,7 +58,10 @@ message LRAUVCommand
// vertical fins clockwise when looking
// from the top (i.e. to starboard)
float elevatorAngleAction_ = 10;
float massPositionAction_ = 11;
float massPositionAction_ = 11; // Target position for the battery's joint.
// Unit: meters. Positive values move the
// battery forward, tilting the nose
// downward.
float buoyancyAction_ = 12; // Volume in cubic meters

float density_ = 13;
Expand Down
7 changes: 5 additions & 2 deletions lrauv_ignition_plugins/proto/lrauv_state.proto
View file
Original file line number Diff line number Diff line change
Expand Up @@ -52,11 +52,14 @@ message LRAUVState
// rotated more clockwise when looking
// from the top (i.e. to starboard)
float elevatorAngle_ = 9;
float massPosition_ = 10;
float massPosition_ = 10; // Position of the battery's joint.
// Unit: meters. Positive values have
// the battery forward, tilting the
// nose downward.
float buoyancyPosition_ = 11; // Volume in cubic meters
float depth_ = 12;

ignition.msgs.Vector3d rph_ = 13; // roll_, pitch_, heading_ in SimResultStruct
ignition.msgs.Vector3d rph_ = 13; // roll_, pitch_, heading_ in SimResultStruct (rad)

float speed_ = 14;
double latitudeDeg_ = 15;
Expand Down
35 changes: 24 additions & 11 deletions lrauv_ignition_plugins/test/helper/LrauvTestFixture.hh
View file
Original file line number Diff line number Diff line change
Expand Up @@ -107,39 +107,52 @@ class LrauvTestFixture : public ::testing::Test
/// \brief Check that a pose is within a given range.
/// \param[in] _index Pose index in tethysPoses
/// \param[in] _refPose Reference pose
/// \param[in] _tols Tolerances for position (X) and rotation (Y)
/// \param[in] _posTols Tolerances for X, Y, Z
/// \param[in] _rotTols Tolerances for Roll, Pitch, Yaw
public: void CheckRange(int _index, const ignition::math::Pose3d &_refPose,
const ignition::math::Vector2d &_tols = {0.25, 0.1})
const ignition::math::Vector3d &_posTols,
const ignition::math::Vector3d &_rotTols)
{
EXPECT_LE(_refPose.Pos().X() - _tols[0], this->tethysPoses[_index].X()) <<
EXPECT_LE(_refPose.Pos().X() - _posTols[0], this->tethysPoses[_index].X()) <<
"Index: " << _index << ", X, reference: " << _refPose.Pos().X();
EXPECT_GE(_refPose.Pos().X() + _tols[0], this->tethysPoses[_index].X()) <<
EXPECT_GE(_refPose.Pos().X() + _posTols[0], this->tethysPoses[_index].X()) <<
"Index: " << _index << ", X, reference: " << _refPose.Pos().X();

EXPECT_LE(_refPose.Pos().Y() - _tols[0], this->tethysPoses[_index].Y()) <<
EXPECT_LE(_refPose.Pos().Y() - _posTols[1], this->tethysPoses[_index].Y()) <<
"Index: " << _index << ", Y, reference: " << _refPose.Pos().Y();
EXPECT_GE(_refPose.Pos().Y() + _tols[0], this->tethysPoses[_index].Y()) <<
EXPECT_GE(_refPose.Pos().Y() + _posTols[1], this->tethysPoses[_index].Y()) <<
"Index: " << _index << ", Y, reference: " << _refPose.Pos().Y();

EXPECT_LE(_refPose.Pos().Z() - _tols[0], this->tethysPoses[_index].Z()) <<
EXPECT_LE(_refPose.Pos().Z() - _posTols[2], this->tethysPoses[_index].Z()) <<
"Index: " << _index << ", Z, reference: " << _refPose.Pos().Z();
EXPECT_GE(_refPose.Pos().Z() + _tols[0], this->tethysPoses[_index].Z()) <<
EXPECT_GE(_refPose.Pos().Z() + _posTols[2], this->tethysPoses[_index].Z()) <<
"Index: " << _index << ", Z, reference: " << _refPose.Pos().Z();

auto diffRoll = abs(angleDiff(_refPose.Rot().Roll(),
this->tethysPoses[_index].Rot().Roll()));
EXPECT_LE(diffRoll, _tols[1]) <<
EXPECT_LE(diffRoll, _rotTols[0]) <<
"Index: " << _index << ", Roll, difference: " << diffRoll;

auto diffPitch = abs(angleDiff(_refPose.Rot().Pitch(),
this->tethysPoses[_index].Rot().Pitch()));
EXPECT_LE(diffPitch, _tols[1]) <<
EXPECT_LE(diffPitch, _rotTols[1]) <<
"Index: " << _index << ", Pitch, difference: " << diffPitch;

auto diffYaw = abs(angleDiff(_refPose.Rot().Yaw(),
this->tethysPoses[_index].Rot().Yaw()));
EXPECT_LE(diffYaw, _tols[1]) <<
EXPECT_LE(diffYaw, _rotTols[2]) <<
"Index: " << _index << ", Yaw, difference: " << diffYaw;
}

/// \brief Check that a pose is within a given range.
/// \param[in] _index Pose index in tethysPoses
/// \param[in] _refPose Reference pose
/// \param[in] _tols Tolerances for position (X) and rotation (Y)
public: void CheckRange(int _index, const ignition::math::Pose3d &_refPose,
const ignition::math::Vector2d &_tols = {0.25, 0.1})
{
this->CheckRange(_index, _refPose, {_tols.X(), _tols.X(), _tols.X()},
{_tols.Y(), _tols.Y(), _tols.Y()});
};

/// \brief Get a process' PID based on its name
Expand Down
8 changes: 4 additions & 4 deletions lrauv_ignition_plugins/test/test_mass_shifter.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -31,19 +31,19 @@
//////////////////////////////////////////////////
TEST_F(LrauvTestFixture, MassShifterTilt)
{
// Get initial X
// Check initial pitch
this->fixture->Server()->Run(true, 100, false);
EXPECT_EQ(100, this->iterations);
EXPECT_EQ(100, this->tethysPoses.size());
EXPECT_NEAR(0.0, this->tethysPoses.back().Rot().Pitch(), 0.01);

// Tell the vehicle to tilt forward
// Tell the vehicle to tilt downward by moving the mass forward (positive command)
lrauv_ignition_plugins::msgs::LRAUVCommand cmdMsg;
cmdMsg.set_masspositionaction_(-0.01);
cmdMsg.set_masspositionaction_(0.01);
cmdMsg.set_buoyancyaction_(0.0005);

// Run server until the command is processed and the model tilts to a
// certain pitch
// certain pitch. Negative pitch means vehicle's nose is pointing down.
double targetPitch{-0.15};
this->PublishCommandWhile(cmdMsg, [&]()
{
Expand Down
67 changes: 38 additions & 29 deletions lrauv_ignition_plugins/test/test_mission_pitch_mass.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -45,6 +45,11 @@ TEST_F(LrauvTestFixture, PitchMass)
this->fixture->Server()->Run(false, 0, false);

// Launch mission
// SetSpeed.speed = 0 m/s^2
// Point.rudderAngle = 0 deg
// Buoyancy.position = neutral
// Pitch.pitch = 20 deg
// Pitch.elevatorAngle = 0
std::atomic<bool> lrauvRunning{true};
std::thread lrauvThread([&]()
{
Expand All @@ -70,34 +75,38 @@ TEST_F(LrauvTestFixture, PitchMass)
int maxIterations{28000};
ASSERT_LT(maxIterations, this->tethysPoses.size());

// Uncomment to get new expectations
// for (int i = 2000; i <= maxIterations; i += 2000)
// {
// auto pose = this->tethysPoses[i];
// std::cout << "this->CheckRange(" << i << ", {"
// << std::setprecision(2) << std::fixed
// << pose.Pos().X() << ", "
// << pose.Pos().Y() << ", "
// << pose.Pos().Z() << ", "
// << pose.Rot().Roll() << ", "
// << pose.Rot().Pitch() << ", "
// << pose.Rot().Yaw() << "}, {0.5, 0.5});"
// << std::endl;
// }

this->CheckRange(2000, {0.00, 0.00, -0.01, -0.00, -0.59, -0.00}, {0.5, 0.5});
this->CheckRange(4000, {-0.01, -0.00, -0.01, -0.00, -0.68, -0.00}, {0.5, 0.5});
this->CheckRange(6000, {-0.02, -0.00, -0.01, 0.00, -0.69, -0.00}, {0.5, 0.5});
this->CheckRange(8000, {-0.02, -0.00, -0.01, 0.00, -0.71, -0.00}, {0.5, 0.5});
this->CheckRange(10000, {-0.03, -0.00, -0.01, 0.00, -0.72, -0.00}, {0.5, 0.5});
this->CheckRange(12000, {-0.03, -0.00, -0.02, -0.00, -0.69, -0.00}, {0.5, 0.5});
this->CheckRange(14000, {-0.04, -0.00, -0.02, -0.00, -0.69, -0.00}, {0.5, 0.5});
this->CheckRange(16000, {-0.04, -0.00, -0.02, 0.00, -0.70, -0.00}, {0.5, 0.5});
this->CheckRange(18000, {-0.04, -0.00, -0.02, 0.00, -0.71, -0.00}, {0.5, 0.5});
this->CheckRange(20000, {-0.04, -0.00, -0.02, -0.00, -0.70, -0.00}, {0.5, 0.5});
this->CheckRange(22000, {-0.04, -0.00, -0.02, -0.00, -0.69, -0.00}, {0.5, 0.5});
this->CheckRange(24000, {-0.05, -0.00, -0.02, 0.00, -0.70, -0.00}, {0.5, 0.5});
this->CheckRange(26000, {-0.05, -0.00, -0.02, -0.00, -0.71, -0.00}, {0.5, 0.5});
this->CheckRange(28000, {-0.05, -0.00, -0.02, -0.00, -0.71, -0.00}, {0.5, 0.5});
// * Y, roll and yaw are kept pretty stable close to zero with low tolerances
// * The pitch target is 20 deg, but there's a lot of oscillation, so we need
// the high tolerance
// * X and Y are meant to stay close to zero, but the vehicle goes forward
// (-X, +Z) slowly. That could be caused by the pitch oscillation?
this->CheckRange(2000, {-0.01, 0.00, -0.06, 0.00, IGN_DTOR(20), 0.00},
{0.1, 0.01, 0.1}, {IGN_DTOR(2), IGN_DTOR(18), IGN_DTOR(2)});
this->CheckRange(4000, {-0.17, 0.00, -0.01, 0.00, IGN_DTOR(20), 0.00},
{0.1, 0.01, 0.1}, {IGN_DTOR(2), IGN_DTOR(18), IGN_DTOR(2)});
this->CheckRange(6000, {-0.51, 0.00, 0.12, 0.00, IGN_DTOR(20), 0.00},
{0.1, 0.01, 0.1}, {IGN_DTOR(2), IGN_DTOR(18), IGN_DTOR(2)});
this->CheckRange(8000, {-0.87, 0.00, 0.22, 0.00, IGN_DTOR(20), 0.00},
{0.1, 0.01, 0.1}, {IGN_DTOR(2), IGN_DTOR(18), IGN_DTOR(2)});
this->CheckRange(10000, {-1.21, 0.00, 0.34, 0.00, IGN_DTOR(20), 0.00},
{0.1, 0.01, 0.1}, {IGN_DTOR(2), IGN_DTOR(18), IGN_DTOR(2)});
this->CheckRange(12000, {-1.55, 0.00, 0.50, 0.00, IGN_DTOR(20), 0.00},
{0.1, 0.01, 0.1}, {IGN_DTOR(2), IGN_DTOR(18), IGN_DTOR(2)});
this->CheckRange(14000, {-1.90, 0.00, 0.63, 0.00, IGN_DTOR(20), 0.00},
{0.1, 0.01, 0.1}, {IGN_DTOR(2), IGN_DTOR(18), IGN_DTOR(2)});
this->CheckRange(16000, {-2.25, 0.00, 0.73, 0.00, IGN_DTOR(20), 0.00},
{0.1, 0.01, 0.1}, {IGN_DTOR(2), IGN_DTOR(18), IGN_DTOR(2)});
this->CheckRange(18000, {-2.60, 0.00, 0.86, 0.00, IGN_DTOR(20), 0.00},
{0.1, 0.01, 0.1}, {IGN_DTOR(2), IGN_DTOR(18), IGN_DTOR(2)});
this->CheckRange(20000, {-2.93, 0.00, 1.02, 0.00, IGN_DTOR(20), 0.00},
{0.1, 0.01, 0.1}, {IGN_DTOR(2), IGN_DTOR(18), IGN_DTOR(2)});
this->CheckRange(22000, {-3.29, 0.00, 1.13, 0.00, IGN_DTOR(20), 0.00},
{0.1, 0.01, 0.1}, {IGN_DTOR(2), IGN_DTOR(18), IGN_DTOR(2)});
this->CheckRange(24000, {-3.64, 0.00, 1.24, 0.00, IGN_DTOR(20), 0.00},
{0.1, 0.01, 0.1}, {IGN_DTOR(2), IGN_DTOR(18), IGN_DTOR(2)});
this->CheckRange(26000, {-3.97, 0.00, 1.39, 0.00, IGN_DTOR(20), 0.00},
{0.1, 0.01, 0.1}, {IGN_DTOR(2), IGN_DTOR(18), IGN_DTOR(2)});
this->CheckRange(28000, {-4.32, 0.00, 1.53, 0.00, IGN_DTOR(20), 0.00},
{0.1, 0.01, 0.1}, {IGN_DTOR(2), IGN_DTOR(18), IGN_DTOR(2)});
}

0 comments on commit 2b07660

Please sign in to comment.