From 12fe3b7fe8485d7f90444969230cb2819929c4a7 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Bal=C3=A1zs=20A=2E=20B=C3=A1lint?= Date: Tue, 19 Sep 2023 14:08:07 +0200 Subject: [PATCH] feat(sensors): Implement ROS wrench publishing In the ROS ecosystem, force-torque data is often available from one source as a 6-element `geometry_msgs/Wrench` or its stamped variant. Having such a single composite data source eliminates the need for the client or the user side to implement some synchronization between the separate force and torque topics, bringing the simulation closer to how a real-world system would function. --- .../mujoco_sensor_handler_plugin.h | 38 +++ .../src/mujoco_sensor_handler_plugin.cpp | 242 ++++++++++++++++++ 2 files changed, 280 insertions(+) diff --git a/mujoco_ros_sensors/include/mujoco_ros_sensors/mujoco_sensor_handler_plugin.h b/mujoco_ros_sensors/include/mujoco_ros_sensors/mujoco_sensor_handler_plugin.h index f7ce0f91..19b794d2 100644 --- a/mujoco_ros_sensors/include/mujoco_ros_sensors/mujoco_sensor_handler_plugin.h +++ b/mujoco_ros_sensors/include/mujoco_ros_sensors/mujoco_sensor_handler_plugin.h @@ -76,6 +76,41 @@ struct SensorConfig typedef std::shared_ptr SensorConfigPtr; +struct WrenchSensorConfig +{ +public: + WrenchSensorConfig() : force_config(), torque_config(){}; + WrenchSensorConfig(const int force_sensor_id, const SensorConfigPtr force_config, const int torque_sensor_id, + const SensorConfigPtr torque_config) + : force_sensor_id(force_sensor_id) + , force_config(force_config) + , torque_sensor_id(torque_sensor_id) + , torque_config(torque_config){}; + + void setFrameId(const std::string frame_id) + { + this->force_config->setFrameId(frame_id); + this->torque_config->setFrameId(frame_id); + }; + + void registerPub(ros::Publisher pub) { value_pub = pub; }; + + void registerGTPub(ros::Publisher pub) { gt_pub = pub; }; + + ros::Publisher gt_pub; + ros::Publisher value_pub; + + SensorConfigPtr force_config; + SensorConfigPtr torque_config; + + int force_sensor_id; + int torque_sensor_id; + + uint8_t is_set = 0; // 0 for unset, otherwise binary code for combination of dims +}; + +typedef std::shared_ptr WrenchSensorConfigPtr; + class MujocoRosSensorsPlugin : public mujoco_ros::MujocoPlugin { public: @@ -96,6 +131,9 @@ class MujocoRosSensorsPlugin : public mujoco_ros::MujocoPlugin std::map sensor_map_; + std::vector wrench_sensor_names_; + std::map wrench_sensor_map_; + ros::ServiceServer register_noise_model_server_; bool registerNoiseModelsCB(mujoco_ros_msgs::RegisterSensorNoiseModels::Request &req, diff --git a/mujoco_ros_sensors/src/mujoco_sensor_handler_plugin.cpp b/mujoco_ros_sensors/src/mujoco_sensor_handler_plugin.cpp index cb7642d8..bd4c06e6 100644 --- a/mujoco_ros_sensors/src/mujoco_sensor_handler_plugin.cpp +++ b/mujoco_ros_sensors/src/mujoco_sensor_handler_plugin.cpp @@ -399,6 +399,106 @@ void MujocoRosSensorsPlugin::lastStageCallback(mujoco_ros::mjModelPtr model, muj break; } } + + for (std::string wrench_sensor_name : wrench_sensor_names_) { + auto wrench_sensor_config = wrench_sensor_map_[wrench_sensor_name]; + auto force_sensor_config = wrench_sensor_config->force_config; + auto torque_sensor_config = wrench_sensor_config->torque_config; + + geometry_msgs::WrenchStamped msg; + msg.header.frame_id = force_sensor_config->frame_id; + msg.header.stamp = ros::Time::now(); + + int id = wrench_sensor_config->force_sensor_id; + adr = model->sensor_adr[id]; + type = model->sensor_type[id]; + cutoff = (model->sensor_cutoff[id] > 0 ? model->sensor_cutoff[id] : 1); + noise_idx = 0; + + // No noise configured + if (force_sensor_config->is_set == 0) { + msg.wrench.force.x = (float)(data->sensordata[adr] / cutoff); + msg.wrench.force.y = (float)(data->sensordata[adr + 1] / cutoff); + msg.wrench.force.z = (float)(data->sensordata[adr + 2] / cutoff); + } else { // Noise at least in one dim + if (force_sensor_config->is_set & 0x01) { + // shift and scale standard normal to desired distribution + noise = noise_dist(rand_generator) * force_sensor_config->sigma[noise_idx] + + force_sensor_config->mean[noise_idx]; + noise_idx += 1; + } else { + noise = 0; + } + msg.wrench.force.x = (float)(data->sensordata[adr] + noise / cutoff); + + if (force_sensor_config->is_set & 0x02) { + // shift and scale standard normal to desired distribution + noise = noise_dist(rand_generator) * force_sensor_config->sigma[noise_idx] + + force_sensor_config->mean[noise_idx]; + noise_idx += 1; + } else { + noise = 0; + } + msg.wrench.force.y = (float)(data->sensordata[adr + 1] + noise / cutoff); + + if (force_sensor_config->is_set & 0x04) { + // shift and scale standard normal to desired distribution + noise = noise_dist(rand_generator) * force_sensor_config->sigma[noise_idx] + + force_sensor_config->mean[noise_idx]; + } else { + noise = 0; + } + msg.wrench.force.z = (float)(data->sensordata[adr + 2] + noise / cutoff); + } + + id = wrench_sensor_config->torque_sensor_id; + adr = model->sensor_adr[id]; + type = model->sensor_type[id]; + cutoff = (model->sensor_cutoff[id] > 0 ? model->sensor_cutoff[id] : 1); + noise_idx = 0; + + // No noise configured + if (torque_sensor_config->is_set == 0) { + msg.wrench.torque.x = (float)(data->sensordata[adr] / cutoff); + msg.wrench.torque.y = (float)(data->sensordata[adr + 1] / cutoff); + msg.wrench.torque.z = (float)(data->sensordata[adr + 2] / cutoff); + } else { // Noise at least in one dim + if (torque_sensor_config->is_set & 0x01) { + // shift and scale standard normal to desired distribution + noise = noise_dist(rand_generator) * torque_sensor_config->sigma[noise_idx] + + torque_sensor_config->mean[noise_idx]; + noise_idx += 1; + } else { + noise = 0; + } + msg.wrench.torque.x = (float)(data->sensordata[adr] + noise / cutoff); + + if (torque_sensor_config->is_set & 0x02) { + // shift and scale standard normal to desired distribution + noise = noise_dist(rand_generator) * torque_sensor_config->sigma[noise_idx] + + torque_sensor_config->mean[noise_idx]; + noise_idx += 1; + } else { + noise = 0; + } + msg.wrench.torque.y = (float)(data->sensordata[adr + 1] + noise / cutoff); + + if (torque_sensor_config->is_set & 0x04) { + // shift and scale standard normal to desired distribution + noise = noise_dist(rand_generator) * torque_sensor_config->sigma[noise_idx] + + torque_sensor_config->mean[noise_idx]; + } else { + noise = 0; + } + msg.wrench.torque.z = (float)(data->sensordata[adr + 2] + noise / cutoff); + } + + wrench_sensor_config->value_pub.publish(msg); + + if (!env_ptr_->settings_.eval_mode) { + wrench_sensor_config->gt_pub.publish(msg); + } + } } void MujocoRosSensorsPlugin::initSensors(mujoco_ros::mjModelPtr model, mujoco_ros::mjDataPtr data) @@ -561,6 +661,148 @@ void MujocoRosSensorsPlugin::initSensors(mujoco_ros::mjModelPtr model, mujoco_ro break; } } + + for (int n = 0; n < model->ntuple; ++n) { + std::string tuple_name; + if (model->names[model->name_tupleadr[n]]) + tuple_name = mj_id2name(model.get(), mjOBJ_TUPLE, n); + else { + ROS_WARN_STREAM_NAMED("sensors", "Tuple name resolution error. Skipping"); + continue; + } + + if (tuple_name != "wrench") + continue; + + int adr = model->tuple_adr[n]; + int size = model->tuple_size[n]; + WrenchSensorConfigPtr config; + + for (int i = 0; i < size; ++i) { + std::string wrench_sensor_name = + mj_id2name(model.get(), model->tuple_objtype[adr + i], model->tuple_objid[adr + i]); + + if (mjOBJ_TUPLE != model->tuple_objtype[adr + i]) { + ROS_WARN_STREAM_NAMED("sensors", "Wrench sensor '" << wrench_sensor_name << "' is not a tuple. Skipping"); + continue; + } + + int id = mj_name2id(model.get(), mjOBJ_TUPLE, wrench_sensor_name.c_str()); + int size = model->tuple_size[id]; + + if (size != 2) { + ROS_WARN_STREAM_NAMED("sensors", "Wrench sensor '" << wrench_sensor_name << "' is not a 2-tuple. Skipping"); + continue; + } + + int adr = model->tuple_adr[id]; + + if (model->tuple_objtype[adr] != mjOBJ_SENSOR) { + ROS_WARN_STREAM_NAMED("sensors", "The first element of wrench sensor '" << wrench_sensor_name + << "' is not a sensor. Skipping"); + continue; + } + if (model->tuple_objtype[adr + 1] != mjOBJ_SENSOR) { + ROS_WARN_STREAM_NAMED("sensors", "The second element of wrench sensor '" << wrench_sensor_name + << "' is not a sensor. Skipping"); + continue; + } + + int force_sensor_id = model->tuple_objid[adr]; + int torque_sensor_id = model->tuple_objid[adr + 1]; + + if (model->sensor_type[force_sensor_id] != mjSENS_FORCE) { + ROS_WARN_STREAM_NAMED("sensors", "The first element of wrench sensor '" + << wrench_sensor_name << "' is not a force sensor. Skipping"); + continue; + } + if (model->sensor_type[torque_sensor_id] != mjSENS_TORQUE) { + ROS_WARN_STREAM_NAMED("sensors", "The second element of wrench sensor '" + << wrench_sensor_name << "' is not a force sensor. Skipping"); + continue; + } + + std::string force_sensor_name = mj_id2name(model.get(), mjOBJ_SENSOR, force_sensor_id); + std::string torque_sensor_name = mj_id2name(model.get(), mjOBJ_SENSOR, torque_sensor_id); + + // ROS_INFO_STREAM_NAMED("sensors", "Wrench sensor '" << name << "' from '" << force_sensor_name << "' and + // '" + // << torque_sensor_name << "'"); + + config.reset(new WrenchSensorConfig(force_sensor_id, sensor_map_[force_sensor_name], torque_sensor_id, + sensor_map_[torque_sensor_name])); + + if (config->force_config->frame_id != config->torque_config->frame_id) { + ROS_ERROR_STREAM_NAMED("sensors", "The force sensor " << force_sensor_name << " and the torque sensor " + << torque_sensor_name + << " does not measure in the same frame"); + continue; + } + + config->registerPub(sensors_nh_->advertise(wrench_sensor_name, 1, true)); + if (!env_ptr_->settings_.eval_mode) { + config->registerGTPub( + sensors_nh_->advertise(wrench_sensor_name + "_GT", 1, true)); + } + + wrench_sensor_names_.push_back(wrench_sensor_name); + wrench_sensor_map_[wrench_sensor_name] = config; + } + } + + // for (std::string name : wrench_sensor_names_) { + // WrenchSensorConfigPtr config; + + // int id = mj_name2id(model.get(), mjOBJ_TUPLE, name.c_str()); + // int size = model->tuple_size[id]; + + // if (size != 2) { + // ROS_WARN_STREAM_NAMED("sensors", "Wrench sensor '" << name << "' is not a 2-tuple. Skipping"); + // continue; + // } + + // int adr = model->tuple_adr[id]; + + // if (model->tuple_objtype[adr] != mjOBJ_SENSOR) { + // ROS_WARN_STREAM_NAMED("sensors", + // "The first element of wrench sensor '" << name << "' is not a sensor. Skipping"); + // continue; + // } + // if (model->tuple_objtype[adr + 1] != mjOBJ_SENSOR) { + // ROS_WARN_STREAM_NAMED("sensors", + // "The second element of wrench sensor '" << name << "' is not a sensor. Skipping"); + // continue; + // } + + // int force_sensor_id = model->tuple_objid[adr]; + // int torque_sensor_id = model->tuple_objid[adr + 1]; + + // if (model->sensor_type[force_sensor_id] != mjSENS_FORCE) { + // ROS_WARN_STREAM_NAMED("sensors", + // "The first element of wrench sensor '" << name << "' is not a force sensor. + // Skipping"); continue; + // } + // if (model->sensor_type[torque_sensor_id] != mjSENS_TORQUE) { + // ROS_WARN_STREAM_NAMED("sensors", + // "The second element of wrench sensor '" << name << "' is not a force sensor. + // Skipping"); continue; + // } + + // std::string force_sensor_name = mj_id2name(model.get(), mjOBJ_SENSOR, force_sensor_id); + // std::string torque_sensor_name = mj_id2name(model.get(), mjOBJ_SENSOR, torque_sensor_id); + + // // ROS_INFO_STREAM_NAMED("sensors", "Wrench sensor '" << name << "' from '" << force_sensor_name << "' and + // '" + // // << torque_sensor_name << "'"); + + // config.reset(new WrenchSensorConfig(sensor_map_[force_sensor_name], sensor_map_[torque_sensor_name])); + // config->registerPub(sensors_nh_->advertise(name, 1, true)); + // if (!env_ptr_->settings_.eval_mode) { + // config->registerGTPub(sensors_nh_->advertise(name + "_GT", 1, true)); + // } + + // wrench_sensor_map_[name] = config; + // } } // Nothing to do on reset