Skip to content

Commit

Permalink
Rework the type support branch to simplify it
Browse files Browse the repository at this point in the history
  • Loading branch information
@mmurooka
mmurooka committed Aug 14, 2024
1 parent 8640f23 commit 723c67a
Show file tree
Hide file tree
Showing 2 changed files with 55 additions and 75 deletions.
23 changes: 9 additions & 14 deletions include/McRtcTactileSensorPlugin/TactileSensorPlugin.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -11,7 +11,6 @@
#if ENABLE_MUJOCO
# include <mujoco_tactile_sensor_plugin/TactileSensorData.h>
#endif
#include <variant>

namespace mc_plugin
{
Expand Down Expand Up @@ -46,6 +45,13 @@ struct TactileSensorPlugin : public mc_control::GlobalPlugin
std::string forceSensorName;
};

/** \brief Sensor data. */
struct SensorData
{
//! Wrench (represented in the frame of tactile sensor)
sva::ForceVecd wrench = sva::ForceVecd::Zero();
};

public:
/** \brief Returns the plugin configuration. */
mc_control::GlobalPlugin::GlobalPluginConfiguration configuration() override;
Expand Down Expand Up @@ -84,19 +90,8 @@ struct TactileSensorPlugin : public mc_control::GlobalPlugin
//! Sensor information list
std::vector<SensorInfo> sensorInfoList_;

//! Sensor message list
#if ENABLE_MUJOCO && ENABLE_ESKIN
std::vector<std::variant<std::nullptr_t,
std::shared_ptr<mujoco_tactile_sensor_plugin::TactileSensorData>,
std::shared_ptr<eskin_ros_utils::PatchData>>>
#elif ENABLE_MUJOCO && !ENABLE_ESKIN
std::vector<std::variant<std::nullptr_t, std::shared_ptr<mujoco_tactile_sensor_plugin::TactileSensorData>>>
#elif !ENABLE_MUJOCO && ENABLE_ESKIN
std::vector<std::variant<std::nullptr_t, std::shared_ptr<eskin_ros_utils::PatchData>>>
#else
std::vector<std::variant<std::nullptr_t>>
#endif
sensorMsgList_;
//! Sensor data list
std::vector<std::shared_ptr<SensorData>> sensorDataList_;

//! ROS variables
//! @{
Expand Down
107 changes: 46 additions & 61 deletions src/TactileSensorPlugin.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -122,8 +122,8 @@ void TactileSensorPlugin::init(mc_control::MCGlobalController & gc, const mc_rtc
void TactileSensorPlugin::reset(mc_control::MCGlobalController & // gc
)
{
sensorMsgList_.clear();
sensorMsgList_.resize(sensorSubList_.size(), nullptr);
sensorDataList_.clear();
sensorDataList_.resize(sensorSubList_.size(), nullptr);
}

void TactileSensorPlugin::before(mc_control::MCGlobalController & gc)
Expand All @@ -138,68 +138,21 @@ void TactileSensorPlugin::before(mc_control::MCGlobalController & gc)
for(size_t sensorIdx = 0; sensorIdx < sensorInfoList_.size(); sensorIdx++)
{
const auto & sensorInfo = sensorInfoList_[sensorIdx];
const auto & sensorData = sensorDataList_[sensorIdx];
auto & forceSensor = robot.data()->forceSensors.at(robot.data()->forceSensorsIndex.at(sensorInfo.forceSensorName));

sva::ForceVecd wrench = sva::ForceVecd::Zero();
if(std::holds_alternative<std::nullptr_t>(sensorMsgList_[sensorIdx]))
if(!sensorData)
{
// Do nothing
forceSensor.wrench(sva::ForceVecd::Zero());
continue;
}
#if ENABLE_MUJOCO
else if(std::holds_alternative<std::shared_ptr<mujoco_tactile_sensor_plugin::TactileSensorData>>(
sensorMsgList_[sensorIdx]))
{
const auto & sensorMsg =
std::get<std::shared_ptr<mujoco_tactile_sensor_plugin::TactileSensorData>>(sensorMsgList_[sensorIdx]);

// Calculate wrench by integrating tactile sensor data
for(size_t i = 0; i < sensorMsg->forces.size(); i++)
{
Eigen::Vector3d position;
Eigen::Vector3d normal;
tf::pointMsgToEigen(sensorMsg->positions[i], position);
tf::vectorMsgToEigen(sensorMsg->normals[i], normal);
Eigen::Vector3d force = sensorMsg->forces[i] * normal;
Eigen::Vector3d moment = position.cross(force);
wrench.force() += force;
wrench.moment() += moment;
}

// Transform from tactile sensor frame to force sensor frame
sva::PTransformd tactileSensorPose = robot.frame(sensorInfo.tactileSensorFrameName).position();
sva::PTransformd forceSensorPose = forceSensor.X_fsmodel_fsactual() * forceSensor.X_0_f(robot);
sva::PTransformd tactileToForceTrans = forceSensorPose * tactileSensorPose.inv();
wrench = tactileToForceTrans.dualMul(wrench);
}
#endif
#if ENABLE_ESKIN
else if(std::holds_alternative<std::shared_ptr<eskin_ros_utils::PatchData>>(sensorMsgList_[sensorIdx]))
{
const auto & sensorMsg = std::get<std::shared_ptr<eskin_ros_utils::PatchData>>(sensorMsgList_[sensorIdx]);

// Calculate wrench by integrating tactile sensor data
for(const auto & cellMsg : sensorMsg->cells)
{
Eigen::Vector3d position;
Eigen::Quaterniond quat;
tf::pointMsgToEigen(cellMsg.pose.position, position);
tf::quaternionMsgToEigen(cellMsg.pose.orientation, quat);
Eigen::Vector3d normal = -1.0 * quat.toRotationMatrix().col(2);
// Edit here to change the conversion calibration from e-Skin tactile measurements to force
Eigen::Vector3d force = 35.0 * (cellMsg.forces[0] + cellMsg.forces[1] + cellMsg.forces[2]) * normal;
Eigen::Vector3d moment = position.cross(force);
wrench.force() += force;
wrench.moment() += moment;
}

// Transform from tactile sensor frame to force sensor frame
sva::PTransformd tactileSensorPose = robot.frame(sensorInfo.tactileSensorFrameName).position();
sva::PTransformd forceSensorPose = forceSensor.X_fsmodel_fsactual() * forceSensor.X_0_f(robot);
sva::PTransformd tactileToForceTrans = forceSensorPose * tactileSensorPose.inv();
wrench = tactileToForceTrans.dualMul(wrench);
}
#endif
forceSensor.wrench(wrench);
// Transform wrench from tactile sensor frame to force sensor frame
sva::PTransformd tactileSensorPose = robot.frame(sensorInfo.tactileSensorFrameName).position();
sva::PTransformd forceSensorPose = forceSensor.X_fsmodel_fsactual() * forceSensor.X_0_f(robot);
sva::PTransformd tactileToForceTrans = forceSensorPose * tactileSensorPose.inv();
sva::ForceVecd wrenchInForceSensorFrame = tactileToForceTrans.dualMul(sensorData->wrench);
forceSensor.wrench(wrenchInForceSensorFrame);
}
}

Expand All @@ -208,14 +161,46 @@ void TactileSensorPlugin::mujocoSensorCallback(
const mujoco_tactile_sensor_plugin::TactileSensorData::ConstPtr & sensorMsg,
size_t sensorIdx)
{
sensorMsgList_[sensorIdx] = std::make_shared<mujoco_tactile_sensor_plugin::TactileSensorData>(*sensorMsg);
auto sensorData = std::make_shared<SensorData>();

// Calculate wrench by integrating tactile sensor measurements
for(size_t i = 0; i < sensorMsg->forces.size(); i++)
{
Eigen::Vector3d position;
Eigen::Vector3d normal;
tf::pointMsgToEigen(sensorMsg->positions[i], position);
tf::vectorMsgToEigen(sensorMsg->normals[i], normal);
Eigen::Vector3d force = sensorMsg->forces[i] * normal;
Eigen::Vector3d moment = position.cross(force);
sensorData->wrench.force() += force;
sensorData->wrench.moment() += moment;
}

sensorDataList_[sensorIdx] = sensorData;
}
#endif

#if ENABLE_ESKIN
void TactileSensorPlugin::eskinSensorCallback(const eskin_ros_utils::PatchData::ConstPtr & sensorMsg, size_t sensorIdx)
{
sensorMsgList_[sensorIdx] = std::make_shared<eskin_ros_utils::PatchData>(*sensorMsg);
auto sensorData = std::make_shared<SensorData>();

// Calculate wrench by integrating tactile sensor measurements
for(const auto & cellMsg : sensorMsg->cells)
{
Eigen::Vector3d position;
Eigen::Quaterniond quat;
tf::pointMsgToEigen(cellMsg.pose.position, position);
tf::quaternionMsgToEigen(cellMsg.pose.orientation, quat);
Eigen::Vector3d normal = -1.0 * quat.toRotationMatrix().col(2);
// Edit here to change the conversion calibration from e-Skin tactile measurements to force
Eigen::Vector3d force = 35.0 * (cellMsg.forces[0] + cellMsg.forces[1] + cellMsg.forces[2]) * normal;
Eigen::Vector3d moment = position.cross(force);
sensorData->wrench.force() += force;
sensorData->wrench.moment() += moment;
}

sensorDataList_[sensorIdx] = sensorData;
}
#endif
} // namespace mc_plugin
Expand Down

0 comments on commit 723c67a

Please sign in to comment.