Karlsruhe Institute of Technology (KIT) Logo

Master Motor Map

Whole-body human reference model and tools for unifying representations of whole-body human motion

MMM Sensor Plug-In-Architecture

The MMM framework consists of a plug-in-architecture to write and read different types of motion data.

See MMM Data Format to understand the xml data format of a motion.
See RapidXmlHelper for the API to write or read a motion represented as xml document.
See General Plug-In-Architecture for further informations about plugins.

Creating a Sensor Plug-In

This section describes how to implement a basic sensor plug-in containing the sensor object and methods for reading and writing the sensor from and to xml.
Here is an example xml representation:

<Sensor type="Example" name="ExampleSensor" description="This is an example sensor">
<Configuration>
<Joint name="BLNx_joint"/>
</Configuration>
<Data>
<Measurement timestep="0">
<Value>0.5</Value>
</Measurement>
<Measurement timestep="0.01">
<Value>0.1</Value>
</Measurement>
...
</Data>
</Sensor>

To implement this new sensor type in the mmm framework a subclass of MMM::SensorMeasurement and either a subclass of MMM::BasicSensor or MMM::InterpolatableSensor is needed (depending on interpolation).
The sensor measurement must include the interface SMCloneable and possibly the interface MMM::Interpolate. All available virtual methods to implement are described well in their documentation.

... // includes and pointer intialization
class ExampleSensor : public MMM::InterpolatableSensor<ExampleSensorMeasurement> // use MMM::BasicSensor<ExampleSensorMeasurement> if the sensor should not be linear interpolatable
{
public:
ExampleSensor() {}
ExampleSensor(const std::string &jointName, const std::string &description = std::string()) : InterpolatableSensor(description), jointName(jointName) {}
bool checkModel(ModelPtr model) {
return model->getModelNode(jointName); // true, if the given model contains this joint
}
boost::shared_ptr<BasicSensor<ExampleSensorMeasurement> > cloneConfiguration() {
ExampleSensorPtr s(new ExampleSensor(jointName, description));
return s;
}
bool equals(SensorPtr other) {
ExampleSensorPtr ptr = boost::dynamic_pointer_cast<ExampleSensor>(other);
if (ptr) return jointName == ptr->jointName; // check if sensor configuration is equal
else return false;
}
std::string getType() {
return TYPE;
}
int getPriority() {
return 10; // position in the xml document (higher value equals earlier)
}
std::vector<std::string> getJointName() {
return jointName;
}
static constexpr const char* TYPE = "Example"; // needs to match the xml type
private:
void loadConfigurationXML(RapidXMLReaderNodePtr node) {
RapidXMLReaderNode jointNode = node->first_node("Joint");
jointName = jointNode->attribute_value(XML::ATTRIBUTE_NAME);
}
void loadMeasurementXML(RapidXMLReaderNodePtr node) {
ExampleSensorMeasurementPtr sm(new ExampleSensorMeasurement(XML::convertToFloat(node->attribute_value(XML::ATTRIBUTE_TIMESTEP).c_str()), XML::convertToFloat(node->first_node("Value")->value()));
addSensorMeasurement(sm);
}
void appendConfigurationXML(RapidXMLWriterNodePtr node) {
RapidXMLWriterNodePtr configurationNode = node->append_node("Configuration");
configurationNode->append_node("Joint")->append_attribute(XML::ATTRIBUTE_NAME, jointName);
}
std::string jointName;
};
... // includes and pointer intialization
class ExampleSensorMeasurement : public MMM::SensorMeasurement, MMM::Interpolate<ExampleSensorMeasurement>, MMM::SMCloneable<ExampleSensorMeasurement> // use Interpolate<SM> just when the sensor measurement pertains to an InterpolatableSensor<SM>
{
public:
ExampleSensorMeasurement(float timestep, float value, bool interpolated = false) : SensorMeasurement(timestep, interpolated), value(value) {}
SensorMeasurementPtr clone() {
return clone(timestep);
}
bool equals(SensorMeasurementPtr sensorMeasurement) {
ExampleSensorMeasurementPtr ptr = boost::dynamic_pointer_cast<ExampleSensorMeasurement>(sensorMeasurement);
if (ptr) return values == values;
else return false;
}
ExampleSensorMeasurementPtr clone(float newTimestep) {
ExampleSensorMeasurementPtr clonedSensorMeasurement(new ExampleSensorMeasurement(newTimestep, value, interpolated));
return clonedSensorMeasurement;
}
float getValue() {
return value;
}
ExampleSensorMeasurementPtr interpolate(KinematicSensorMeasurementPtr other, float timestep) {
ExampleSensorMeasurementPtr interpolatedSensorMeasurement(new ExampleSensorMeasurement(timestep, Math::linearInterpolation(value, this->timestep, other->value, other->timestep, timestep), true));
return interpolatedSensorMeasurement;
}
protected:
void appendMeasurementDataXML(RapidXMLWriterNodePtr measurementNode) {
measurementNode->append_node("Value")->append_data_node(XML::toString(value));
}
private:
float value;
};

To create your sensor plug-in a special factory as subclass of MMM::SensorFactory is needed.

... // includes
class ExampleSensorFactory : public MMM::SensorFactory
{
public:
ExampleSensorFactory() : SensorFactory() {}
virtual ~ExampleSensorFactory() {}
SensorPtr createSensor(RapidXMLReaderNodePtr sensorNode) {
ExampleSensorPtr sensor(new ExampleSensor());
sensor->loadSensor(sensorNode);
return sensor;
}
virtual std::string getName() {
return ExampleSensor::TYPE;
}
static SensorFactoryPtr createInstance(void*) {
return SensorFactoryPtr(new ExampleSensorFactory());
}
private:
static SubClassRegistry registry;
};
// register this factory
SensorFactory::SubClassRegistry ExampleSensorFactory::registry(ExampleSensor::TYPE, &ExampleSensorFactory::createInstance);
extern "C"
BOOST_EXTENSION_EXPORT_DECL SensorFactoryPtr getFactory() {
return SensorFactoryPtr(new ExampleSensorFactory());
}

As a whole these three classes need to be compiled to an independed library which can be loaded by the mmm framework (see Loading Plug-Ins).

Creating or accessing Information of a specific sensor

This section describes how a specific sensor type can be directly created without an xml file and how non public sensor data can be accessed. At first your project needs to be linked against the specific sensor plug-in library in CMake.
Creating a sensor can then be easily done by using the sensor's normal constructor.

// Creating an example sensor
ExampleSensorPtr createExampleSensor(const std::string &jointName, const std::string &description, std::vector<std::tuple<float, float> > timestepValues) {
ExampleSensorPtr exampleSensor(new ExampleSensor(jointName, description)); // Create sensor
for (auto timestepValue : timestepValues) {
ExampleSensorMeasurementPtr exampleSensorMeasurement(new ExampleSensorMeasurement(std::get<0>(timestepValue), std::get<1>(timestepValue)); // Create sensor measurement
exampleSensor->addSensorMeasurement(exampleSensorMeasurement); // Add sensor measurement to sensor
}
return exampleSensor;
}

However accessing not visible sensor data needs a dynamic pointer cast to the special subclass sensor.

// Accessing the information of an example sensor
void accessExampleSensorData(MMM::MotionPtr motion) {
MMM::SensorList sensors = motion->getSensorsByType("Example");
if (sensors.size() == 0) {
MMM_ERROR << "No example sensor in motion " + motion->getName() + " found." << std::endl;
return;
}
for (auto sensor : sensors) {
ExampleSensorPtr exampleSensor = boost::dynamic_pointer_cast<ExampleSensor>(sensor);
exampleSensor->getJointName(); // Access sensor configuration
std::vector<float> timesteps = exampleSensor->getTimesteps();
for (auto timestep : timesteps) {
ExampleSensorMeasurementPtr exampleSensorMeasurement = exampleSensor->getSensorMeasurement(timestep);
exampleSensorMeasurement->getValue(); // Access sensor data
}
}
}

Directly access information about model pose and kinematic joint angles

Most of the you want to directly access the kinematics of a motion. Therefore we provide a corresponding code snippet you just can copy&paste.

#include <MMM/Motion/MotionReaderXML.h>
#include <MMM/Motion/Plugin/KinematicPlugin/KinematicSensor.h>
#include <MMM/Motion/Plugin/ModelPosePlugin/ModelPoseSensor.h>
void doSomething(const std::string &motionFilePath) {
MMM::MotionList motions;
try {
MMM::MotionReaderXMLPtr motionReader(new MMM::MotionReaderXML());
// You could also load a spefic motion by name.
motions = motionReader->loadAllMotions(motionFilePath);
}
// TODO ERROR HANDLING
}
for (MMM::MotionPtr motion : motions) {
MMM::ModelPoseSensorPtr modelPoseSensor = motion->getSensorByType<MMM::ModelPoseSensor>(MMM::ModelPoseSensor::TYPE);
// This will return the first kinematic sensor of a motion file. If you need more, see method getSensorsByType().
MMM::KinematicSensorPtr kinematicSensor = motion->getSensorByType<MMM::KinematicSensor>(MMM::KinematicSensor::TYPE);
if (!modelPoseSensor || !kinematicSensor) continue;
std::vector<std::string> jointNames = kinematicSensor->getJointNames();
for (float timestep : modelPoseSensor->getTimesteps()) {
MMM::ModelPoseSensorMeasurementPtr modelPoseSensorMeasurement = modelPoseSensor->getDerivedMeasurement(timestep);
MMM::KinematicSensorMeasurementPtr kinematicSensorMeasurement = kinematicSensor->getDerivedMeasurement(timestep);
if (!kinematicSensorMeasurement) continue;
Eigen::VectorXf jointAngles = kinematicSensorMeasurement->getJointAngles();
Eigen::Matrix4f rootPose = modelPoseSensorMeasurement->getRootPose();
// TODO do what you want :)
// jointNames at index i is the name for the joint angle at index i
}
}
}

Maybe you want to transfer your motion on a virtual robot. Then you can do something like this:

#include <MMM/Motion/MotionReaderXML.h>
#include <MMM/Motion/Plugin/KinematicPlugin/KinematicSensor.h>
#include <MMM/Motion/Plugin/ModelPosePlugin/ModelPoseSensor.h>
#include <VirtualRobot/RobotConfig.h>
#include <VirtualRobot/RobotNodeSet.h>
#include <MMMSimoxTools/MMMSimoxTools.h>
void doSomething(const std::string &motionFilePath) {
MMM::MotionList motions;
try {
MMM::MotionReaderXMLPtr motionReader(new MMM::MotionReaderXML());
// You could also load a spefic motion by name.
motions = motionReader->loadAllMotions(motionFilePath);
}
// TODO ERROR HANDLING
}
for (MMM::MotionPtr motion : motions) {
MMM::ModelPoseSensorPtr modelPoseSensor = motion->getSensorByType<MMM::ModelPoseSensor>(MMM::ModelPoseSensor::TYPE);
// This will return the first kinematic sensor of a motion file. If you need more, see method getSensorsByType().
MMM::KinematicSensorPtr kinematicSensor = motion->getSensorByType<MMM::KinematicSensor>(MMM::KinematicSensor::TYPE);
if (!motion->getModel() || !modelPoseSensor || !kinematicSensor) continue;
VirtualRobot::RobotPtr robot = MMM::SimoxTools::buildModel(motion->getModel(), false);
VirtualRobot::RobotNodeSetPtr robotNodeSet = VirtualRobot::RobotNodeSet::createRobotNodeSet(robot, "ExampleRobotNodeSet", kinematicSensor->getJointNames(), "", "", true);
for (float timestep : modelPoseSensor->getTimesteps()) {
MMM::ModelPoseSensorMeasurementPtr modelPoseSensorMeasurement = modelPoseSensor->getDerivedMeasurement(timestep);
MMM::KinematicSensorMeasurementPtr kinematicSensorMeasurement = kinematicSensor->getDerivedMeasurement(timestep);
if (!kinematicSensorMeasurement) continue;
robot->setGlobalPose(modelPoseSensorMeasurement->getRootPose());
robotNodeSet->setJointValues(kinematicSensorMeasurement->getJointAngles());
// TODO do something with your configured robot
}
}
}
KoroiBot Logo WALK-MAN Logo Xperience Logo SecondHands Logo TimeStorm Logo I-SUPPORT Logo
PACO-PLUS Logo SFB-588 Logo SPP1527 Logo