Absolem update

[peci1]

Followup of #528 and #520. Thanks for pushing this forward.

I fixed the flipper control (plugin names were wrong, and I fixed the problem with positional commands only reacting after zero velocity command).

Realsense sensor was moved back and the image rendering issue was fixed by setting min clipping plane to 10 cm. It seems more proper to me than moving the sensor origin. I've also noticed a weird bug - if I set the clipping plane to 5 cm, both RGB and D images get blank when the robot is rotated too much from any world axis (e.g. at 45°, the RGB image is black and Depth image is white).

I improved the flipper control plugin so that it also allows relative positional commands (this is useful for teleoperation).

I also incorporated all of the SDF-only changes back to the Xacro file, as that one is the "source" for the robot model. The empty tags in SDF are automatically created by ign sdf and they have no effect, so I'd like to keep them (otherwise there'd need to be extra logic to remove them).

[peci1]

I also started looking into the issue of reading <limit><velocity> in the code. I've come with this:

sdf::Model model;
model.Load(_sdf->GetParent());
const auto flipperJoint = model.JointByName(this->jointName);
if (flipperJoint == nullptr)
{
  ignerr << "FlipperControlPlugin did not find the joint specified in <joint_name> parameter." << std::endl;
  return;
}

this->maxAngularVelocity = _sdf->Get<double>("max_velocity", flipperJoint->Axis(0)->MaxVelocity()).first;
this->maxAngularVelocity = (std::min)(this->maxAngularVelocity, flipperJoint->Axis(0)->MaxVelocity());

Could you comment on the approach? It might be ineffective during load time as it parses the whole SDF as model... But I haven't found a nicer solution.

[angelacmaio]

Thank you for your responsiveness and for following through with model improvements. This PR will not be incorporated for Cave Circuit to avoid delaying the model's release. The issues you cited (flipper position control, flipper velocity limits, Realsense blockage) have been resolved in the cave_feature_release6 branch and a workaround for flipper relative positions can already be achieved by reading the joint_state topic before publishing absolute positions to the flippers_cmd_pos topics. Please pull the latest updates.

[peci1]

@angelacmaio Thanks for the updates. Do you think that also the reported bug with flipper control was resolved? (Not being able to send position command without first sending a zero velocity command). From the changes in code I see, it doesn't seem so (I can't verify it right now). It should be okay if you'd just copy over flipper_control_plugin.cpp from this PR. It is a bit larger change, but I tested it and it works.

[acschang]

@angelacmaio Thanks for the updates. Do you think that also the reported bug with flipper control was resolved? (Not being able to send position command without first sending a zero velocity command). From the changes in code I see, it doesn't seem so (I can't verify it right now). It should be okay if you'd just copy over flipper_control_plugin.cpp from this PR. It is a bit larger change, but I tested it and it works.

The bug fixed on cave_feature_release6

[peci1]

Closing this PR in favor of #542, #543 and other future PRs.

[peci1]

I found a better way to query the joint axis limits:

gazebo::Model model(_entity);
const auto jointEntity = model.JointByName(_ecm, this->jointName);
const auto jointAxisComponent = _ecm.Component<components::JointAxis>(jointEntity);
const auto jointAxis = jointAxisComponent->Data();
const auto maxVelocity = jointAxis.MaxVelocity();