The dynamically created joint and link don't show up in gzclient. The harness_link is shown in the list widget, but nothing happens if you click on it, and it's link frame isn't visible. Also, the joint doesn't show up in the list widget, its JointVisual is not visible, and it doesn't appear in the JointControlWidget.
I believe it is because the dynamically created link and joint are not calling Entity::Load(sdf::ElementPtr _sdf). In that function we set the parent name and id for the visual_msg. I'll try setting those in Link::FillMsg and Joint::FillMsg.
what's the inertia of the harness_link? is it unity mass and identity moi? This might cause issues later if we are trying to winch very large models, in which case we can skip attaching to a link and attach direct to the world, or allow user to adjust link inertia?
One issue might be critical is the "touch down", current plugin calls Joint::SetForce during HarnessPlugin::OnUpdate at every simulation step to maintain position. This might not be ideal as the robot touches down on the ground, the winch might very likely actually impart force driving the robot downwards. In a real harness, only support forces are applied at the robot torso.
To simulate this, we might have to (sorry @Nathan Koenig) create an intermediate link, so robot is attached to the harness_link and harness_link to the world. Where robot is attached to the harness_link is a prismatic joint with robot sitting at the lower joint limit, or a pseudo-contact joint, in either case the joint only applies support force in the upwards-z direction.
And the harness_link to world joint is PID force controlled like the current implementation.
actually, after talking it over with @Nathan Koenig we could probably just truncate the output force of the PID controller to be only positive and achieve similar effects.
I am working on a test branch.
One more issue, since this is a PID velocity control, the block in the example world drifts down ever so slowly. If I let simulation sit around long enough it will have moved completely downwards. Should we consider switching to position control?
Ok, in addition to a trivial whitespace fix (7dc8848a5ec7), I've made some additional commits that improve failure behavior:
e71aa0dc83a1: change parentName.substr(parentName.find("::")) so that it checks that the result of find is not std::string::npos, otherwise substr will thow an out_of_range exception. Gazebo catches it, but gives an unhelpful error message ([Err] [Model.cc:1076] Exception occured in the Load function of plugin with name[harness] and filename[libgazebo_ros_harness.so]. This plugin will not run.). With this check, it now gives more helpful error messages:
[Err] [Joint.cc:262] EXCEPTION: Couldn't Find Parent Link[fake][Err] [HarnessPlugin.cc:84] Unable to load joint[joint1]. Couldn't Find Parent Link[fake][Err] [HarnessPlugin.cc:100] No joints specified in the harness plugin.The harness plugin will not run.
0e926c2539f6: prevents the HarnessPlugin::Update function from being called if no joints were found; otherwise gazebo crashes