search

jrl-umi3218 / mc_rtc

mc_rtc is an interface for simulated and real robotic systems suitable for real-time control
BSD 2-Clause "Simplified" License
122 stars 37 forks source link

Create JointSensor device #266

Closed rohanpsingh closed 2 years ago

rohanpsingh commented 2 years ago

This PR will create a "JointSensor" device that holds:

associated with a joint on the robot. It can be added to any joint and in practice, would be added to those joints of the robot from where we want sensors readings.

In the robot module, a joint sensor would be added as such:

_jointSensors.push_back(mc_rbdyn::JointSensor(jsJOINT_NAME, JOINT_NAME));

It is not thoroughly tested and the implementation is not yet complete. But I am creating this draft PR to receive your views/comments.

rohanpsingh commented 2 years ago

Thanks a lot for the review! I will make the appropriate changes and reply back to you soon.

rohanpsingh commented 2 years ago

Hi @gergondet Sorry for the delay.

I did most of the changes that you asked for. Most importantly, JointSensor does not derive from Device anymore. Could you please see if it looks alright? I think there might be a need to improve the constructors.

I'm now thinking on how to best integrate this within mc_global_controller. We want the interface to be able to set the joint sensor values name-wise and/or with a vector of values. But there might be cases where we want to have temperature/current readings only for some joints. Maybe it should be similar to "setWrenches"?

Thanks!

gergondet commented 2 years ago

I'm now thinking on how to best integrate this within mc_global_controller. We want the interface to be able to set the joint sensor values name-wise and/or with a vector of values. But there might be cases where we want to have temperature/current readings only for some joints. Maybe it should be similar to "setWrenches"?

Maybe:

// Rince and repeat for other entries in a joint sensor

// Single joint sensor value setting
void setJointSensorTemperature(const std::string & robot, const std::string & joint, double temperature);
// Many joint sensors value setting
void setJointSensorTemperatures(const std::string & robot, const std::map<std::string, double> & temperatures);
rohanpsingh commented 2 years ago

Okay, this works if the interface sets the sensor values as follows:

controller.setJointMotorCurrent(controller.robot().name(), "R_HIP_P", 11);

for example.

There are a few remaining problems that I need to fix:

gergondet commented 2 years ago

mc_log_ui crashes if it sees a nan value (default) <--- not sure what to do about this

I will look this and then we can merge (merging as-is would break all logs for everyone unfortunately)

rohanpsingh commented 2 years ago

Thanks @gergondet ! Do you think logging the joint sensor values by name is a good thing to do? https://github.com/jrl-umi3218/mc_rtc/pull/266/files#diff-67d79af4f4b21496b9f648bdbea64d8d31cc8f6e6ad0097b384a25b6ec5891a8R416-R421

I don't think other items are being logged by name. And currently, it kind of breaks when the robot's joint names contain underscores (like R_HIP_P for JVRC1).

gergondet commented 2 years ago

Hello @rohanpsingh

I couldn't get it to crash by having NaN in the logs but I identified an issue when there was only NaN for a given entry (not crashing though). I have fixed it in 10238d8e5

Please let me know if that works for you.

Do you think logging the joint sensor values by name is a good thing to do?

Yes. We don't do it for encoders and other similar joint-entries because we get the data as a std::vector<double> corresponding to the reference joint order. Since this sensor is joint-based, it makes sense.

I don't think other items are being logged by name. And currently, it kind of breaks when the robot's joint names contain underscores (like R_HIP_P for JVRC1).

That's because the UI creates new group if multiples entries share a common suffix split by _. We can add special logic in the UI to treat joints' names differently but I don't think this should be a high priority.

rohanpsingh commented 2 years ago

In my case, it was crashing before the fix if it only sees nan values:

  File "/home/rohan/openrtp/lib/python2.7/site-packages/mc_log_ui/mc_log_plotcanvas.py", line 351, in getRange
    first = check[0][0]
IndexError: index 0 is out of bounds for axis 0 with size 0
Aborted (core dumped)

After the fix it seems to work fine.

We can add special logic in the UI to treat joints' names differently but I don't think this should be a high priority.

OK

gergondet commented 2 years ago

In my case, it was crashing before the fix if it only sees nan values:

Ok. I saw the same error message but it wasn't crashing the GUI for me (but I'm working under WSL these days so that may be why)

In any case, with this out of the way we can merge this! Thanks @rohanpsingh 🥳