From 67bd987018b7032d716c54a9d206a3d673774c05 Mon Sep 17 00:00:00 2001 From: Michael Maurus Date: Mon, 7 Sep 2020 17:17:56 +0200 Subject: [PATCH 1/5] added visualizer for RigidBodyStateSE3 and Wrench --- viz/CMakeLists.txt | 4 + viz/PluginLoader.cpp | 12 + viz/RigidBodyStateSE3Visualization.cpp | 425 +++++++++++++++++++++++++ viz/RigidBodyStateSE3Visualization.hpp | 146 +++++++++ viz/WrenchVisualization.cpp | 75 +++++ viz/WrenchVisualization.hpp | 40 +++ 6 files changed, 702 insertions(+) create mode 100644 viz/RigidBodyStateSE3Visualization.cpp create mode 100644 viz/RigidBodyStateSE3Visualization.hpp create mode 100644 viz/WrenchVisualization.cpp create mode 100644 viz/WrenchVisualization.hpp diff --git a/viz/CMakeLists.txt b/viz/CMakeLists.txt index 09e2f98f..b5f2a357 100644 --- a/viz/CMakeLists.txt +++ b/viz/CMakeLists.txt @@ -22,6 +22,8 @@ rock_vizkit_plugin(base-viz SonarVisualization.cpp PointcloudVisualization.cpp DepthMapVisualization.cpp + RigidBodyStateSE3Visualization.cpp + WrenchVisualization.cpp ${OPTIONAL_CPP} HEADERS Uncertainty.hpp @@ -37,6 +39,8 @@ rock_vizkit_plugin(base-viz SonarVisualization.hpp PointcloudVisualization.hpp DepthMapVisualization.hpp + RigidBodyStateSE3Visualization.hpp + WrenchVisualization.hpp ${OPTIONAL_HPP} DEPS base-types LIBS ${Boost_SYSTEM_LIBRARY} diff --git a/viz/PluginLoader.cpp b/viz/PluginLoader.cpp index 9f4688bd..31dbb3de 100644 --- a/viz/PluginLoader.cpp +++ b/viz/PluginLoader.cpp @@ -11,6 +11,8 @@ #include "SonarVisualization.hpp" #include "DepthMapVisualization.hpp" #include "DistanceImageVisualization.hpp" +#include "RigidBodyStateSE3Visualization.hpp" +#include "WrenchVisualization.hpp" namespace vizkit3d { class QtPluginVizkitBase : public vizkit3d::VizkitPluginFactory { @@ -39,6 +41,8 @@ namespace vizkit3d { pluginNames->push_back("SonarVisualization"); pluginNames->push_back("DepthMapVisualization"); pluginNames->push_back("DistanceImageVisualization"); + pluginNames->push_back("RigidBodyStateSE3Visualization"); + pluginNames->push_back("WrenchVisualization"); return pluginNames; } @@ -93,6 +97,14 @@ namespace vizkit3d { { plugin = new vizkit3d::DistanceImageVisualization(); } + else if (pluginName == "RigidBodyStateSE3Visualization") + { + plugin = new vizkit3d::RigidBodyStateSE3Visualization(); + } + else if (pluginName == "WrenchVisualization") + { + plugin = new vizkit3d::WrenchVisualization(); + } if (plugin) { diff --git a/viz/RigidBodyStateSE3Visualization.cpp b/viz/RigidBodyStateSE3Visualization.cpp new file mode 100644 index 00000000..80ee33b3 --- /dev/null +++ b/viz/RigidBodyStateSE3Visualization.cpp @@ -0,0 +1,425 @@ +#include "RigidBodyStateSE3Visualization.hpp" +#include +#include +#include +#include +#include +#include +#include + +using namespace osg; +namespace vizkit3d +{ + +RigidBodyStateSE3Visualization::RigidBodyStateSE3Visualization(QObject* parent) + : Vizkit3DPlugin(parent) + , color(1, 1, 1) + , total_size(1) + , main_size(0.1) + , text_size(0.0) + , translation(0, 0, 0) + , rotation(0, 0, 0, 1) + , body_type(BODY_NONE) + , texture_dirty(false) + , bump_mapping_dirty(false) + , forcePositionDisplay(false) + , forceOrientationDisplay(false) +{ + state.pose.position = base::Vector3d::Zero(); + state.pose.orientation = base::Quaterniond::Identity(); +} + +RigidBodyStateSE3Visualization::~RigidBodyStateSE3Visualization() +{ +} + +void RigidBodyStateSE3Visualization::setColor(const Vec4d& color, Geode* geode) +{ + Material *material = new Material(); + material->setDiffuse(Material::FRONT, Vec4(0.1, 0.1, 0.1, 1.0)); + material->setSpecular(Material::FRONT, Vec4(0.6, 0.6, 0.6, 1.0)); + material->setAmbient(Material::FRONT, Vec4(0.1, 0.1, 0.1, 1.0)); + material->setEmission(Material::FRONT, color); + material->setShininess(Material::FRONT, 10.0); + + geode->getOrCreateStateSet()->setAttribute(material); +} + +bool RigidBodyStateSE3Visualization::isPositionDisplayForced() const +{ return forcePositionDisplay; } + +void RigidBodyStateSE3Visualization::setPositionDisplayForceFlag(bool flag) +{ forcePositionDisplay = flag; emit propertyChanged("forcePositionDisplay"); } + +bool RigidBodyStateSE3Visualization::isOrientationDisplayForced() const +{ return forceOrientationDisplay; } + +void RigidBodyStateSE3Visualization::setOrientationDisplayForceFlag(bool flag) +{ forceOrientationDisplay = flag; emit propertyChanged("forceOrientationDisplay"); } + +void RigidBodyStateSE3Visualization::setTexture(QString const& path) +{ return setTexture(path.toStdString()); } + +void RigidBodyStateSE3Visualization::setTexture(std::string const& path) +{ + if (path.empty()) + return clearTexture(); + + image = osgDB::readImageFile(path); + texture_dirty = true; +} + +void RigidBodyStateSE3Visualization::clearTexture() +{ + image.release(); + texture_dirty = true; +} + +void RigidBodyStateSE3Visualization::addBumpMapping( + QString const& diffuse_color_map_path, + QString const& normal_map_path) +{ return addBumpMapping(diffuse_color_map_path.toStdString(), + normal_map_path.toStdString()); } + +void RigidBodyStateSE3Visualization::addBumpMapping( + std::string const& diffuse_color_map_path, + std::string const& normal_map_path) +{ + if (!body_model->asGeode()) + { + std::cerr << "model is not a geometry, cannot use bump mapping" << std::endl; + return; + } + + // Setup the textures + diffuse_image = osgDB::readImageFile(diffuse_color_map_path); + normal_image = osgDB::readImageFile(normal_map_path); + + // And add bump mapping + osgFX::BumpMapping* bump_mapping = new osgFX::BumpMapping(); + bump_mapping->setLightNumber(0); + bump_mapping->setNormalMapTextureUnit(1); + bump_mapping->setDiffuseTextureUnit(2); + this->bump_mapping = bump_mapping; + bump_mapping_dirty = true; +} + +void RigidBodyStateSE3Visualization::updateTexture() +{ + ref_ptr state = body_model->getOrCreateStateSet(); + if (!image) + { + state->setTextureMode(0, GL_TEXTURE_2D, StateAttribute::OFF); + return; + } + else + { + texture->setImage(image.get()); + state->setTextureAttributeAndModes(0, texture, StateAttribute::ON); + } +} + +void RigidBodyStateSE3Visualization::updateBumpMapping() +{ + ref_ptr state = body_model->getOrCreateStateSet(); + + if (!bump_mapping) + { + diffuse_image.release(); + normal_image.release(); + state->setTextureMode(1, GL_TEXTURE_2D, StateAttribute::OFF); + state->setTextureMode(2, GL_TEXTURE_2D, StateAttribute::OFF); + return; + } + else + { + + ref_ptr geometry = body_model->asGeode()->getDrawable(0)->asGeometry(); + ref_ptr tex_coord = geometry->getTexCoordArray(0); + geometry->setTexCoordArray(1, tex_coord); + geometry->setTexCoordArray(2, tex_coord); + + diffuse_texture->setImage(diffuse_image.get()); + normal_texture->setImage(normal_image.get()); + state->setTextureAttributeAndModes(1, normal_texture, StateAttribute::ON); + state->setTextureAttributeAndModes(2, diffuse_texture, StateAttribute::ON); + bump_mapping->prepareChildren(); + } +} + +void RigidBodyStateSE3Visualization::removeBumpMapping() +{ + bump_mapping.release(); + bump_mapping_dirty = true; +} + +ref_ptr RigidBodyStateSE3Visualization::createSimpleSphere(double size) +{ + ref_ptr group = new Group(); + + ref_ptr geode = new Geode(); + ref_ptr sp = new Sphere(Vec3f(0,0,0), main_size * size); + ref_ptr spd = new ShapeDrawable(sp); + spd->setColor(Vec4f(color.x(), color.y(), color.z(), 1.0)); + geode->addDrawable(spd); + group->addChild(geode); + + return group; +} + +ref_ptr RigidBodyStateSE3Visualization::createSimpleBody(double size) +{ + ref_ptr group = new Group(); + + ref_ptr geode = new Geode(); + ref_ptr sp = new Sphere(Vec3f(0,0,0), main_size * size); + ref_ptr spd = new ShapeDrawable(sp); + spd->setColor(Vec4f(color.x(), color.y(), color.z(), 1.0)); + geode->addDrawable(spd); + if(text_size>0.0) + { + double actual_size = text_size * size; + ref_ptr text= new osgText::Text; + text->setText(state.frame_id); + text->setCharacterSize(actual_size); + text->setPosition(osg::Vec3d(actual_size/2,actual_size/2,0)); + geode->addDrawable(text); + } + + group->addChild(geode); + + //up + ref_ptr c1g = new Geode(); + ref_ptr c1 = new Cylinder(Vec3f(0, 0, size / 2), size / 40, size); + ref_ptr c1d = new ShapeDrawable(c1); + c1g->addDrawable(c1d); + setColor(Vec4f(0, 0, 1.0, 1.0), c1g); + group->addChild(c1g); + + //north direction + ref_ptr c2g = new Geode(); + ref_ptr c2 = new Cylinder(Vec3f(0, size / 2, 0), size / 40, size); + c2->setRotation(Quat(M_PI/2.0, Vec3d(1,0,0))); + ref_ptr c2d = new ShapeDrawable(c2); + c2g->addDrawable(c2d); + setColor(Vec4f(0.0, 1.0, 0, 1.0), c2g); + group->addChild(c2g); + + //east + ref_ptr c3g = new Geode(); + ref_ptr c3 = new Cylinder(Vec3f(size / 2, 0, 0), size / 40, size); + c3->setRotation(Quat(M_PI/2.0, Vec3d(0,1,0))); + ref_ptr c3d = new ShapeDrawable(c3); + c3g->addDrawable(c3d); + setColor(Vec4f(1.0, 0.0, 0, 1.0), c3g); + group->addChild(c3g); + + return group; +} + +double RigidBodyStateSE3Visualization::getMainSphereSize() const +{ + return main_size; +} + +void RigidBodyStateSE3Visualization::setMainSphereSize(double size) +{ + main_size = size; + emit propertyChanged("sphereSize"); + // This triggers an update of the model if we don't have a custom model + setSize(total_size); +} + +double RigidBodyStateSE3Visualization::getTextSize() const +{ + return text_size; +} + +void RigidBodyStateSE3Visualization::setTextSize(double size) +{ + text_size = size; + emit propertyChanged("textSize"); + // This triggers an update of the model if we don't have a custom model + setSize(total_size); +} + +void RigidBodyStateSE3Visualization::setSize(double size) +{ + total_size = size; + emit propertyChanged("size"); + if (body_type == BODY_SIMPLE) + resetModel(size); + else if (body_type == BODY_SPHERE) + resetModelSphere(size); +} + +double RigidBodyStateSE3Visualization::getSize() const +{ + return total_size; +} + +void RigidBodyStateSE3Visualization::resetModel(double size) +{ + body_type = BODY_SIMPLE; + body_model = createSimpleBody(size); + setDirty(); +} + +void RigidBodyStateSE3Visualization::resetModelSphere(double size) +{ + body_type = BODY_SPHERE; + body_model = createSimpleSphere(size); + setDirty(); +} + +QString RigidBodyStateSE3Visualization::getModelPath() const +{ + if (body_type == BODY_SPHERE) + return "sphere"; + else if (body_type == BODY_SIMPLE) + return "simple"; + else + return model_path; +} + +void RigidBodyStateSE3Visualization::loadModel(QString const& path) +{ + return loadModel(path.toStdString()); +} + +void RigidBodyStateSE3Visualization::loadModel(std::string const& path) +{ + if (path == "sphere") + { + resetModelSphere(total_size); + return; + } + else if (path == "simple") + { + resetModel(total_size); + return; + } + + ref_ptr model = osgDB::readNodeFile(path); + body_type = BODY_CUSTOM_MODEL; + body_model = model; + if (!body_model->asGeode()) + std::cerr << "model is not a geode, using bump mapping will not be possible" << std::endl; + else if (!body_model->asGeode()->getDrawable(0)) + std::cerr << "model does not contain a mesh, using bump mapping will not be possible" << std::endl; + else if (!body_model->asGeode()->getDrawable(0)->asGeometry()) + std::cerr << "model does not contain a mesh, using bump mapping will not be possible" << std::endl; + + model_path = QString::fromStdString(path); + //set plugin name + if(vizkit3d_plugin_name.isEmpty()) + { + size_t found; + std::string str; + found = path.find_last_of("/\\"); + if(found == std::string::npos) + str = path; + else + str = path.substr(found+1); + found = str.find_last_of("."); + if(found != std::string::npos) + { + str = str.substr(0,found); + if(!str.empty()) + setPluginName(QString::fromStdString(str)); + } + } + + setDirty(); + emit propertyChanged("modelPath"); +} +QVector3D RigidBodyStateSE3Visualization::getTranslation() const +{ + return QVector3D(translation.x(), translation.y(), translation.z()); +} +void RigidBodyStateSE3Visualization::setTranslation(QVector3D const& v) +{ + translation = osg::Vec3(v.x(), v.y(), v.z()); + setDirty(); +} + +void RigidBodyStateSE3Visualization::setRotation(QQuaternion const& q) +{ + rotation = osg::Quat(q.x(), q.y(), q.z(), q.scalar()); + setDirty(); +} + +void RigidBodyStateSE3Visualization::setColor(base::Vector3d const& color) +{ this->color = color; } + +ref_ptr RigidBodyStateSE3Visualization::createMainNode() +{ + Group* group = new Group; + PositionAttitudeTransform* body_pose = + new PositionAttitudeTransform(); + if (!body_model) + resetModel(total_size); + body_pose->addChild(body_model); + group->addChild(body_pose); + + texture = new osg::Texture2D; + texture->setWrap(Texture::WRAP_S, Texture::REPEAT); + texture->setWrap(Texture::WRAP_T, Texture::REPEAT); + texture->setFilter(osg::Texture::MIN_FILTER, osg::Texture::LINEAR_MIPMAP_LINEAR); + texture->setFilter(osg::Texture::MAG_FILTER, osg::Texture::LINEAR); + diffuse_texture = new osg::Texture2D; + diffuse_texture->setWrap(Texture::WRAP_S, Texture::REPEAT); + diffuse_texture->setWrap(Texture::WRAP_T, Texture::REPEAT); + diffuse_texture->setFilter(osg::Texture::MIN_FILTER, osg::Texture::LINEAR_MIPMAP_LINEAR); + diffuse_texture->setFilter(osg::Texture::MAG_FILTER, osg::Texture::LINEAR); + normal_texture = new osg::Texture2D; + normal_texture->setWrap(Texture::WRAP_S, Texture::REPEAT); + normal_texture->setWrap(Texture::WRAP_T, Texture::REPEAT); + normal_texture->setFilter(osg::Texture::MIN_FILTER, osg::Texture::LINEAR_MIPMAP_LINEAR); + normal_texture->setFilter(osg::Texture::MAG_FILTER, osg::Texture::LINEAR); + + return group; +} + +void RigidBodyStateSE3Visualization::updateMainNode(Node* node) +{ + Group* group = node->asGroup(); + PositionAttitudeTransform* body_pose = + dynamic_cast(group->getChild(0)); + + // Reset the body model if needed + Node* body_node = body_pose->getChild(0); + if (bump_mapping && body_node != bump_mapping) + { + bump_mapping->addChild(body_model); + body_pose->setChild(0, bump_mapping); + } + else if (body_node != body_model) + body_pose->setChild(0, body_model); + + if (texture_dirty) + updateTexture(); + if (bump_mapping_dirty) + updateBumpMapping(); + + bool hasValidPose = state.hasValidPose(); + + if (forcePositionDisplay || hasValidPose) { + osg::Vec3d pos(state.pose.position.x(), state.pose.position.y(), state.pose.position.z()); + body_pose->setPosition(pos + translation); + } + if (forceOrientationDisplay || hasValidPose) { + osg::Quat orientation(state.pose.orientation.x(), + state.pose.orientation.y(), + state.pose.orientation.z(), + state.pose.orientation.w()); + body_pose->setAttitude(rotation * orientation); + } +} + +void RigidBodyStateSE3Visualization::updateDataIntern( const base::samples::RigidBodyStateSE3& state ) +{ + this->state = state; +} + +} \ No newline at end of file diff --git a/viz/RigidBodyStateSE3Visualization.hpp b/viz/RigidBodyStateSE3Visualization.hpp new file mode 100644 index 00000000..eab0ab26 --- /dev/null +++ b/viz/RigidBodyStateSE3Visualization.hpp @@ -0,0 +1,146 @@ +#ifndef __RIGID_BODY_STATE_SE3_VISUALIZATION_HPP__ +#define __RIGID_BODY_STATE_SE3_VISUALIZATION_HPP__ + +#include +#include +#include + +#include +#include + +namespace osgFX +{ + class BumpMapping; +} + +namespace vizkit3d +{ + +class RigidBodyStateSE3Visualization : public Vizkit3DPlugin +{ + Q_OBJECT + Q_PROPERTY(double size READ getSize WRITE setSize) + Q_PROPERTY(double sphereSize READ getMainSphereSize WRITE setMainSphereSize) + Q_PROPERTY(double textSize READ getTextSize WRITE setTextSize) + Q_PROPERTY(bool forcePositionDisplay READ isPositionDisplayForced WRITE setPositionDisplayForceFlag) + Q_PROPERTY(bool forceOrientationDisplay READ isOrientationDisplayForced WRITE setOrientationDisplayForceFlag) + Q_PROPERTY(QString modelPath READ getModelPath WRITE loadModel) + + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + RigidBodyStateSE3Visualization(QObject* parent = NULL); + virtual ~RigidBodyStateSE3Visualization(); + + Q_INVOKABLE void updateData( const base::samples::RigidBodyStateSE3& state ) + { return Vizkit3DPlugin::updateData(state); } + Q_INVOKABLE void updateRigidBodyStateSE3( const base::samples::RigidBodyStateSE3& state ) + { return updateData(state); } + + protected: + virtual osg::ref_ptr createMainNode(); + virtual void updateMainNode(osg::Node* node); + void updateDataIntern( const base::samples::RigidBodyStateSE3& state ); + base::samples::RigidBodyStateSE3 state; + + public slots: + bool isPositionDisplayForced() const; + void setPositionDisplayForceFlag(bool flag); + bool isOrientationDisplayForced() const; + void setOrientationDisplayForceFlag(bool flag); + + double getSize() const; + void setSize(double size); + + void resetModel(double size); + void resetModelSphere(double size); + + QString getModelPath() const; + void loadModel(std::string const& path); + void loadModel(QString const& path); + + /** When using the default body, sets the size of the main sphere, + * relative to the size of the complete object + * + * The default is 0.1 + */ + void setMainSphereSize(double size); + + /** When using one of the default bodies, returns the size of the main + * sphere, relative to the size of the complete object + * + * The default is 0.1 + */ + double getMainSphereSize() const; + + /** Sets the text size relative to the size of the complete object. + * If text size is positive, the name of the source frame is rendered in the visualization. + * The default is 0.0 + */ + void setTextSize(double size); + double getTextSize() const; + + /** Sets the color of the default body model in R, G, B + * + * Values must be between 0 and 1 + * + * If you call it after the plugin got attached, call resetModel to + * apply the new color + */ + void setColor(base::Vector3d const& color); + + void setColor(const osg::Vec4d& color, osg::Geode* geode); + + void setTexture(QString const& path); + void setTexture(std::string const& path); + void clearTexture(); + void addBumpMapping( + QString const& diffuse_color_map_path, + QString const& normal_map_path); + void addBumpMapping( + std::string const& diffuse_color_map_path, + std::string const& normal_map_path); + void removeBumpMapping(); + + QVector3D getTranslation() const; + void setTranslation(QVector3D const& v); + void setRotation(QQuaternion const& q); + + private: + base::Vector3d color; + double total_size; + double main_size; + double text_size; + + osg::Vec3 translation; + osg::Quat rotation; + + enum BODY_TYPES + { BODY_NONE, BODY_SIMPLE, BODY_SPHERE, BODY_CUSTOM_MODEL }; + + BODY_TYPES body_type; + osg::ref_ptr body_model; + osg::ref_ptr createSimpleBody(double size); + osg::ref_ptr createSimpleSphere(double size); + + osg::ref_ptr image; + osg::ref_ptr texture; + bool texture_dirty; + void updateTexture(); + + osg::ref_ptr diffuse_image; + osg::ref_ptr normal_image; + osg::ref_ptr diffuse_texture; + osg::ref_ptr normal_texture; + osg::ref_ptr bump_mapping; + bool bump_mapping_dirty; + void updateBumpMapping(); + + bool forcePositionDisplay; + bool forceOrientationDisplay; + + QString model_path; + +}; + +} +#endif // ROBOT_H diff --git a/viz/WrenchVisualization.cpp b/viz/WrenchVisualization.cpp new file mode 100644 index 00000000..2b444027 --- /dev/null +++ b/viz/WrenchVisualization.cpp @@ -0,0 +1,75 @@ +#include +#include "WrenchVisualization.hpp" +#include +#include +#include +#include + +using namespace vizkit3d; + +WrenchVisualization::WrenchVisualization() +{ +} + +WrenchVisualization::~WrenchVisualization() +{ +} + +osg::ref_ptr WrenchVisualization::createMainNode() +{ + // Geode is a common node used for vizkit3d plugins. It allows to display + // "arbitrary" geometries + + double size = 2.0; + + osg::Group* group = new osg::Group; + + //force + osg::MatrixTransform* force_dir = new osg::MatrixTransform; + osg::ref_ptr fg = new osg::Geode(); + osg::ref_ptr fc = new osg::Cylinder(osg::Vec3f(0, 0, 0.05), 0.01, 0.1); + osg::ref_ptr fcd = new osg::ShapeDrawable(fc); + fcd->setColor(osg::Vec4f(1, 0, 0, 1.0)); + + fg->addDrawable(fcd); + //osg::setColor(osg::Vec4f(1, 0, 0, 1.0), c1g); + force_dir->addChild(fg); + group->addChild(force_dir); + + //torque + osg::MatrixTransform* torque_dir = new osg::MatrixTransform; + osg::ref_ptr tg = new osg::Geode(); + osg::ref_ptr tc = new osg::Cylinder(osg::Vec3f(0, 0, 0.05), 0.01, 0.1); + osg::ref_ptr tcd = new osg::ShapeDrawable(tc); + tcd->setColor(osg::Vec4f(0, 1, 0, 1.0)); + tg->addDrawable(tcd); + //osg::setColor(osg::Vec4f(0.0, 0.0, 0, 1.0), c2g); + torque_dir->addChild(tg); + group->addChild(torque_dir); + + return group; +} + +void WrenchVisualization::updateMainNode ( osg::Node* node ) +{ + // Update the main node using the data in p->data + osg::Group* group = node->asGroup(); + + osg::MatrixTransform* force_transform = dynamic_cast(group->getChild(0)); + osg::Vec3d force(state.force.x(), state.force.y(), state.force.z()); + osg::Matrixd S = osg::Matrix::scale(1, 1, force.length()); + osg::Matrixd R; + R.makeRotate(osg::Vec3d(0,0,1), force); + force_transform->setMatrix(S * R); + + osg::MatrixTransform* torque_transform = dynamic_cast(group->getChild(1)); + osg::Vec3d torque(state.torque.x(), state.torque.y(), state.torque.z()); + S = osg::Matrix::scale(1, 1, torque.length()); + R.makeRotate(osg::Vec3d(0,0,1), torque); + torque_transform->setMatrix(S * R); +} + +void WrenchVisualization::updateDataIntern(base::Wrench const& value) +{ + state = value; +} \ No newline at end of file diff --git a/viz/WrenchVisualization.hpp b/viz/WrenchVisualization.hpp new file mode 100644 index 00000000..682f36cc --- /dev/null +++ b/viz/WrenchVisualization.hpp @@ -0,0 +1,40 @@ +#ifndef WrenchVisualization_H +#define WrenchVisualization_H + +#include +#include +#include +#include + +namespace vizkit3d +{ + class WrenchVisualization + : public vizkit3d::Vizkit3DPlugin + , boost::noncopyable + { + Q_OBJECT + public: + WrenchVisualization(); + ~WrenchVisualization(); + + Q_INVOKABLE void updateData(base::Wrench const &sample) + {vizkit3d::Vizkit3DPlugin::updateData(sample);} + + void setForce(const base::Vector3d& f) { + state.force = f; + } + + void setTorque(const base::Vector3d& t) { + state.torque = t; + } + + protected: + virtual osg::ref_ptr createMainNode(); + virtual void updateMainNode(osg::Node* node); + virtual void updateDataIntern(base::Wrench const& plan); + + private: + base::Wrench state; + }; +} +#endif From ae55026c1b554aab300581cc8b6eaebcf7a40a41 Mon Sep 17 00:00:00 2001 From: Michael Maurus Date: Tue, 19 Jan 2021 09:54:17 +0100 Subject: [PATCH 2/5] using osgviz PrimitivesFactory for displaying arrow and circular arrow. needs develop branch of osgviz --- viz/CMakeLists.txt | 4 +- viz/RigidBodyStateSE3Visualization.cpp | 183 ++++++++++++++++++++----- viz/RigidBodyStateSE3Visualization.hpp | 67 ++++++++- viz/WrenchModel.cpp | 147 ++++++++++++++++++++ viz/WrenchModel.hpp | 57 ++++++++ viz/WrenchVisualization.cpp | 85 +++++------- viz/WrenchVisualization.hpp | 94 +++++++++++-- viz/vizkit_plugin.rb | 6 + 8 files changed, 537 insertions(+), 106 deletions(-) create mode 100644 viz/WrenchModel.cpp create mode 100644 viz/WrenchModel.hpp diff --git a/viz/CMakeLists.txt b/viz/CMakeLists.txt index b5f2a357..04379fa7 100644 --- a/viz/CMakeLists.txt +++ b/viz/CMakeLists.txt @@ -23,6 +23,7 @@ rock_vizkit_plugin(base-viz PointcloudVisualization.cpp DepthMapVisualization.cpp RigidBodyStateSE3Visualization.cpp + WrenchModel.cpp WrenchVisualization.cpp ${OPTIONAL_CPP} HEADERS @@ -40,9 +41,10 @@ rock_vizkit_plugin(base-viz PointcloudVisualization.hpp DepthMapVisualization.hpp RigidBodyStateSE3Visualization.hpp + WrenchModel.hpp WrenchVisualization.hpp ${OPTIONAL_HPP} DEPS base-types LIBS ${Boost_SYSTEM_LIBRARY} - DEPS_PKGCONFIG base-logging + DEPS_PKGCONFIG base-logging PrimitivesFactory osgViz ) diff --git a/viz/RigidBodyStateSE3Visualization.cpp b/viz/RigidBodyStateSE3Visualization.cpp index 80ee33b3..cbc6bba2 100644 --- a/viz/RigidBodyStateSE3Visualization.cpp +++ b/viz/RigidBodyStateSE3Visualization.cpp @@ -16,7 +16,7 @@ RigidBodyStateSE3Visualization::RigidBodyStateSE3Visualization(QObject* parent) , color(1, 1, 1) , total_size(1) , main_size(0.1) - , text_size(0.0) + , text_size(0.1) , translation(0, 0, 0) , rotation(0, 0, 0, 1) , body_type(BODY_NONE) @@ -24,9 +24,16 @@ RigidBodyStateSE3Visualization::RigidBodyStateSE3Visualization(QObject* parent) , bump_mapping_dirty(false) , forcePositionDisplay(false) , forceOrientationDisplay(false) + , showPose(true) + , showVelocity(false) + , showAcceleration(false) + , showWrench(false) + , show_seperate_axes(false) + , resolution(0.1) { state.pose.position = base::Vector3d::Zero(); state.pose.orientation = base::Quaterniond::Identity(); + primitivesfactory = osgviz::OsgViz::getModuleInstance("PrimitivesFactory"); } RigidBodyStateSE3Visualization::~RigidBodyStateSE3Visualization() @@ -47,16 +54,50 @@ void RigidBodyStateSE3Visualization::setColor(const Vec4d& color, Geode* geode) bool RigidBodyStateSE3Visualization::isPositionDisplayForced() const { return forcePositionDisplay; } - void RigidBodyStateSE3Visualization::setPositionDisplayForceFlag(bool flag) { forcePositionDisplay = flag; emit propertyChanged("forcePositionDisplay"); } bool RigidBodyStateSE3Visualization::isOrientationDisplayForced() const { return forceOrientationDisplay; } - void RigidBodyStateSE3Visualization::setOrientationDisplayForceFlag(bool flag) { forceOrientationDisplay = flag; emit propertyChanged("forceOrientationDisplay"); } +bool RigidBodyStateSE3Visualization::isPoseDisplayed() const { + return showPose; +} +void RigidBodyStateSE3Visualization::setPoseDisplayed(bool flag) { + showPose = flag; + emit propertyChanged("showPose"); + updateModel(total_size); +} + +bool RigidBodyStateSE3Visualization::isVelocityDisplayed() const { + return showVelocity; +} +void RigidBodyStateSE3Visualization::setVelocityDisplayed(bool flag) { + showVelocity = flag; + emit propertyChanged("showVelocity"); + updateModel(total_size); +} + +bool RigidBodyStateSE3Visualization::isAccelerationDisplayed() const { + return showAcceleration; +} +void RigidBodyStateSE3Visualization::setAccelerationDisplayed(bool flag) { + showAcceleration = flag; + emit propertyChanged("showAcceleration"); + updateModel(total_size); +} + +bool RigidBodyStateSE3Visualization::isWrenchDisplayed() const { + return showWrench; +} +void RigidBodyStateSE3Visualization::setWrenchDisplayed(bool flag) { + showWrench = flag; + emit propertyChanged("showWrench"); + updateModel(total_size); +} + void RigidBodyStateSE3Visualization::setTexture(QString const& path) { return setTexture(path.toStdString()); } @@ -188,31 +229,64 @@ ref_ptr RigidBodyStateSE3Visualization::createSimpleBody(double size) group->addChild(geode); - //up - ref_ptr c1g = new Geode(); - ref_ptr c1 = new Cylinder(Vec3f(0, 0, size / 2), size / 40, size); - ref_ptr c1d = new ShapeDrawable(c1); - c1g->addDrawable(c1d); - setColor(Vec4f(0, 0, 1.0, 1.0), c1g); - group->addChild(c1g); + if (showPose) { + //up + ref_ptr c1g = new Geode(); + ref_ptr c1 = new Cylinder(Vec3f(0, 0, size / 2), size / 40, size); + ref_ptr c1d = new ShapeDrawable(c1); + c1g->addDrawable(c1d); + setColor(Vec4f(0, 0, 1.0, 1.0), c1g); + group->addChild(c1g); + + //north direction + ref_ptr c2g = new Geode(); + ref_ptr c2 = new Cylinder(Vec3f(0, size / 2, 0), size / 40, size); + c2->setRotation(Quat(M_PI/2.0, Vec3d(1,0,0))); + ref_ptr c2d = new ShapeDrawable(c2); + c2g->addDrawable(c2d); + setColor(Vec4f(0.0, 1.0, 0, 1.0), c2g); + group->addChild(c2g); + + //east + ref_ptr c3g = new Geode(); + ref_ptr c3 = new Cylinder(Vec3f(size / 2, 0, 0), size / 40, size); + c3->setRotation(Quat(M_PI/2.0, Vec3d(0,1,0))); + ref_ptr c3d = new ShapeDrawable(c3); + c3g->addDrawable(c3d); + setColor(Vec4f(1.0, 0.0, 0, 1.0), c3g); + group->addChild(c3g); + } - //north direction - ref_ptr c2g = new Geode(); - ref_ptr c2 = new Cylinder(Vec3f(0, size / 2, 0), size / 40, size); - c2->setRotation(Quat(M_PI/2.0, Vec3d(1,0,0))); - ref_ptr c2d = new ShapeDrawable(c2); - c2g->addDrawable(c2d); - setColor(Vec4f(0.0, 1.0, 0, 1.0), c2g); - group->addChild(c2g); - - //east - ref_ptr c3g = new Geode(); - ref_ptr c3 = new Cylinder(Vec3f(size / 2, 0, 0), size / 40, size); - c3->setRotation(Quat(M_PI/2.0, Vec3d(0,1,0))); - ref_ptr c3d = new ShapeDrawable(c3); - c3g->addDrawable(c3d); - setColor(Vec4f(1.0, 0.0, 0, 1.0), c3g); - group->addChild(c3g); + if (showVelocity) { + //linear velocity + //linear_vel_transform = new Arrow(osg::Vec4f(1,0,0,1)); + linear_vel_transform = primitivesfactory->createArrow(osg::Vec4f(1, 0, 0, 1.0), true); + group->addChild(linear_vel_transform); + + //angular velocity + //angular_vel_transform = new Arrow(osg::Vec4f(0,1,0,1)); + angular_vel_transform = primitivesfactory->createArrow(osg::Vec4f(0, 1, 0, 1.0), true); + group->addChild(angular_vel_transform); + } + + if (showAcceleration) { + //linear acceleration + //linear_acc_transform = new Arrow(osg::Vec4f(1,0,0,1)); + linear_acc_transform = primitivesfactory->createArrow(osg::Vec4f(1, 0, 0, 1.0), true); + group->addChild(linear_acc_transform); + + //angular acceleration + //angular_acc_transform = new Arrow(osg::Vec4f(0,1,0,1)); + angular_acc_transform = primitivesfactory->createArrow(osg::Vec4f(0, 1, 0, 1.0), true); + group->addChild(angular_acc_transform); + } + + if (showWrench) { + wrench_model = new WrenchModel(); + wrench_model->setResolution(resolution); + wrench_model->seperateAxes(show_seperate_axes); + group->addChild(wrench_model); + } return group; } @@ -247,10 +321,7 @@ void RigidBodyStateSE3Visualization::setSize(double size) { total_size = size; emit propertyChanged("size"); - if (body_type == BODY_SIMPLE) - resetModel(size); - else if (body_type == BODY_SPHERE) - resetModelSphere(size); + updateModel(total_size); } double RigidBodyStateSE3Visualization::getSize() const @@ -258,6 +329,13 @@ double RigidBodyStateSE3Visualization::getSize() const return total_size; } +void RigidBodyStateSE3Visualization::updateModel(double size) { + if (body_type == BODY_SIMPLE) + resetModel(size); + else if (body_type == BODY_SPHERE) + resetModelSphere(size); +} + void RigidBodyStateSE3Visualization::resetModel(double size) { body_type = BODY_SIMPLE; @@ -333,6 +411,7 @@ void RigidBodyStateSE3Visualization::loadModel(std::string const& path) setDirty(); emit propertyChanged("modelPath"); } + QVector3D RigidBodyStateSE3Visualization::getTranslation() const { return QVector3D(translation.x(), translation.y(), translation.z()); @@ -403,18 +482,51 @@ void RigidBodyStateSE3Visualization::updateMainNode(Node* node) updateBumpMapping(); bool hasValidPose = state.hasValidPose(); - if (forcePositionDisplay || hasValidPose) { osg::Vec3d pos(state.pose.position.x(), state.pose.position.y(), state.pose.position.z()); body_pose->setPosition(pos + translation); } if (forceOrientationDisplay || hasValidPose) { - osg::Quat orientation(state.pose.orientation.x(), + osg::Quat orientation(state.pose.orientation.x(), state.pose.orientation.y(), state.pose.orientation.z(), state.pose.orientation.w()); body_pose->setAttitude(rotation * orientation); } + + if (showVelocity && state.hasValidTwist()) { + osg::Quat Q; + const base::Twist& t = state.twist; + osg::Vec3d lin_vel(t.linear.x(), t.linear.y(), t.linear.z()); + Q.makeRotate(osg::Vec3d(0,0,1), lin_vel); + linear_vel_transform->setScale(osg::Vec3f(resolution, resolution, resolution*lin_vel.length())); + linear_vel_transform->setAttitude(Q); + + osg::Vec3d ang_vel(t.angular.x(), t.angular.y(), t.angular.z()); + Q.makeRotate(osg::Vec3d(0,0,1), ang_vel); + angular_vel_transform->setScale(osg::Vec3f(resolution, resolution, resolution*ang_vel.length())); + angular_vel_transform->setAttitude(Q); + } + + if (showAcceleration && state.hasValidAcceleration()) { + osg::Quat Q; + const base::Acceleration& acc = state.acceleration; + osg::Vec3d lin_acc(acc.linear.x(), acc.linear.y(), acc.linear.z()); + Q.makeRotate(osg::Vec3d(0,0,1), lin_acc); + linear_acc_transform->setScale(osg::Vec3f(resolution, resolution, resolution*lin_acc.length())); + linear_acc_transform->setAttitude(Q); + + osg::Vec3d ang_acc(acc.angular.x(), acc.angular.y(), acc.angular.z()); + Q.makeRotate(osg::Vec3d(0,0,1), ang_acc); + angular_acc_transform->setScale(osg::Vec3f(resolution, resolution, resolution*ang_acc.length())); + angular_acc_transform->setAttitude(Q); + } + + if (showWrench && state.hasValidWrench()) { + + wrench_model->setResolution(resolution); + wrench_model->update(state.wrench); + } } void RigidBodyStateSE3Visualization::updateDataIntern( const base::samples::RigidBodyStateSE3& state ) @@ -422,4 +534,7 @@ void RigidBodyStateSE3Visualization::updateDataIntern( const base::samples::Rigi this->state = state; } -} \ No newline at end of file +} + +//Macro that makes this plugin loadable in ruby, this is optional. +//VizkitQtPlugin(WrenchVisualization) \ No newline at end of file diff --git a/viz/RigidBodyStateSE3Visualization.hpp b/viz/RigidBodyStateSE3Visualization.hpp index eab0ab26..9f0f05ba 100644 --- a/viz/RigidBodyStateSE3Visualization.hpp +++ b/viz/RigidBodyStateSE3Visualization.hpp @@ -4,9 +4,13 @@ #include #include #include +#include +#include #include #include +#include +#include "WrenchModel.hpp" namespace osgFX { @@ -25,6 +29,12 @@ class RigidBodyStateSE3Visualization : public Vizkit3DPlugin createMainNode(); - virtual void updateMainNode(osg::Node* node); - void updateDataIntern( const base::samples::RigidBodyStateSE3& state ); + virtual void updateMainNode(osg::Node* node); + void updateDataIntern( const base::samples::RigidBodyStateSE3& state ); base::samples::RigidBodyStateSE3 state; public slots: @@ -48,11 +58,24 @@ class RigidBodyStateSE3Visualization : public Vizkit3DPlugin body_model; + osg::ref_ptr body_model; osg::ref_ptr createSimpleBody(double size); - osg::ref_ptr createSimpleSphere(double size); + osg::ref_ptr createSimpleSphere(double size); osg::ref_ptr image; osg::ref_ptr texture; @@ -135,11 +175,24 @@ class RigidBodyStateSE3Visualization : public Vizkit3DPlugin linear_vel_transform, angular_vel_transform; + osg::ref_ptr linear_acc_transform, angular_acc_transform; + osg::ref_ptr wrench_model; + bool forcePositionDisplay; bool forceOrientationDisplay; - + + bool showPose; + bool showVelocity; + bool showAcceleration; + bool showWrench; + bool show_seperate_axes; + double resolution; + QString model_path; + std::shared_ptr primitivesfactory; + }; } diff --git a/viz/WrenchModel.cpp b/viz/WrenchModel.cpp new file mode 100644 index 00000000..134a2b6c --- /dev/null +++ b/viz/WrenchModel.cpp @@ -0,0 +1,147 @@ +#include "WrenchModel.hpp" + +WrenchModel::WrenchModel(double resolution) + : show_seperate_axes_force(false) + , show_seperate_axes_torque(false) + , resolution(resolution) +{ + primitivesfactory = osgviz::OsgViz::getModuleInstance("PrimitivesFactory"); + if (!primitivesfactory) { + std::cerr << "Could not load PrimitivesFactory" << std::endl; + return; + } + buildGeometry(); +} + +osg::ref_ptr WrenchModel::createCircularArrow(const osg::Vec4f& color) { + // osg::ref_ptr arr = new CircularArrow(0.2, 0.02, 12, 32, 1.5*M_PI); + // arr->setColor(color); + // arr->rebuildGeometry(); + + osg::ref_ptr arr = primitivesfactory->createCircularArrow(0.1, 0.02, 12, 32, 1.5*M_PI, 2.0, color); + + return arr; +} + + +void WrenchModel::buildGeometry() { + + + + // force + force_node = primitivesfactory->createArrow(osg::Vec4f(1, 0, 0, 1.0), true); + addChild(force_node, true); // child 0 + + // torque + torque_group = new osg::Group; + + torque_group->addChild(primitivesfactory->createArrow(osg::Vec4f(0, 1, 0, 1.0), true)); + torque_group->addChild(createCircularArrow(osg::Vec4f(0, 1, 0, 1.0))); + addChild(torque_group, true); // child 1 + + // seperated along axes + force_axes_group = new osg::Switch; + torque_axes_group = new osg::Switch; + + for (unsigned int i=0; i<3; ++i) { + //add force axis + force_axes_group->addChild(primitivesfactory->createArrow(osg::Vec4f(1, 0, 0, 1.0), true), true); + + //add torque axis + osg::ref_ptr torque_axis_group = new osg::Group; + torque_axis_group->addChild(primitivesfactory->createArrow(osg::Vec4f(0, 1, 0, 1.0), true)); + torque_axis_group->addChild(createCircularArrow(osg::Vec4f(0, 1, 0, 1.0))); + + torque_axes_group->addChild(torque_axis_group, true); + } + + addChild(force_axes_group, false); // child 2 + addChild(torque_axes_group, false); // child 3 +} + +void WrenchModel::update(const base::Wrench& wrench) { + osg::Matrixd R, S; + osg::Quat Q; + + if (show_seperate_axes_force) { + // for all axes + for (unsigned int i=0; i<3; ++i) { + if (wrench.force[i] == 0) { + // if component is zero, disable + force_axes_group->setValue(i, false); + } + else { + //update transform + osg::PositionAttitudeTransform* force_transform = dynamic_cast(force_axes_group->getChild(i)); + osg::Vec3d force(0,0,0); + force[i] = wrench.force[i]; + Q.makeRotate(osg::Vec3d(0,0,1), force); + force_transform->setScale(osg::Vec3f(resolution, resolution, resolution*force.length())); + force_transform->setAttitude(Q); + force_axes_group->setChildValue(force_transform, true); + } + } + } + else { + if (wrench.force.isZero()) { + // if force is zero vector, disable + setChildValue(force_node, false); + } + else { + // update transform + osg::Vec3d force(wrench.force.x(), wrench.force.y(), wrench.force.z()); + Q.makeRotate(osg::Vec3d(0,0,1), force); + //force_node->setScale(osg::Vec3f(resolution, resolution, resolution*force.length())); + + osgviz::ArrowNode* f_node = dynamic_cast(force_node.get()); + f_node->setLength(resolution*force.length()); + force_node->setAttitude(Q); + + setChildValue(force_node, true); + } + } + + if (show_seperate_axes_torque) { + for (unsigned int i=0; i<3; ++i) { + if (wrench.torque[i] == 0) { + torque_axes_group->setValue(i, false); + } + else { + osg::Group* group = dynamic_cast(torque_axes_group->getChild(i)); + osg::PositionAttitudeTransform* torque_vec_transform = dynamic_cast(group->getChild(0)); + osg::Vec3d torque(0,0,0); + torque[i] = wrench.torque[i]; + Q.makeRotate(osg::Vec3d(0,0,1), torque); + torque_vec_transform->setScale(osg::Vec3f(resolution, resolution, resolution * torque.length())); + torque_vec_transform->setAttitude(Q); + + osg::PositionAttitudeTransform* torque_transform = dynamic_cast(group->getChild(1)); + torque_transform->setAttitude(Q); + torque_transform->setPosition(torque * 0.4 * resolution); + //torque_transform->setScale(osg::Vec3f(resolution,resolution,resolution)); + torque_axes_group->setValue(i, true); + } + } + } + else { + if (wrench.torque.isZero()) { + // if torque is zero vector, disable + setChildValue(torque_group, false); + } + else { + // update transform + osgviz::ArrowNode* torque_vec_transform = dynamic_cast(torque_group->getChild(0)); + osg::Vec3d torque(wrench.torque.x(), wrench.torque.y(), wrench.torque.z()); + Q.makeRotate(osg::Vec3d(0,0,1), torque); + //torque_vec_transform->setScale(osg::Vec3d(resolution, resolution, resolution*torque.length())); + torque_vec_transform->setLength(resolution*torque.length()); + torque_vec_transform->setAttitude(Q); + + osg::PositionAttitudeTransform* torque_transform = dynamic_cast(torque_group->getChild(1)); + torque_transform->setAttitude(Q); + torque_transform->setPosition(torque * 0.4 * resolution); + //torque_transform->setScale(osg::Vec3f(resolution,resolution,resolution)); + setChildValue(torque_group, true); + } + } +} \ No newline at end of file diff --git a/viz/WrenchModel.hpp b/viz/WrenchModel.hpp new file mode 100644 index 00000000..7ff72192 --- /dev/null +++ b/viz/WrenchModel.hpp @@ -0,0 +1,57 @@ +#ifndef WRENCHMODEL_HPP +#define WRENCHMODEL_HPP + +#include +#include + +#include +#include +#include + +class WrenchModel : public osg::Switch { + +public: + WrenchModel(double resolution = 1.0); + + void update(const base::Wrench& wrench); + + void seperateAxes(bool val = true) { + seperateAxesForce(val); + seperateAxesTorque(val); + } + void seperateAxesForce(bool val=true) { + show_seperate_axes_force = val; + setChildValue(force_node, !val); + setChildValue(force_axes_group, val); + } + void seperateAxesTorque(bool val=true) { + show_seperate_axes_torque = val; + setChildValue(torque_group, !val); + setChildValue(torque_axes_group, val); + } + void setResolution(double res) { + resolution = res; + } + +protected: + //osg::ref_ptr model; + + osg::ref_ptr createCircularArrow(const osg::Vec4f& color); + void buildGeometry(); + + bool show_seperate_axes_force; + bool show_seperate_axes_torque; + double resolution; + + osg::ref_ptr force_node; + osg::ref_ptr torque_group; + osg::ref_ptr force_axes_group, torque_axes_group; + + std::shared_ptr primitivesfactory; + +private: + + +}; + +#endif \ No newline at end of file diff --git a/viz/WrenchVisualization.cpp b/viz/WrenchVisualization.cpp index 2b444027..01143ecd 100644 --- a/viz/WrenchVisualization.cpp +++ b/viz/WrenchVisualization.cpp @@ -1,13 +1,13 @@ #include #include "WrenchVisualization.hpp" -#include -#include -#include -#include using namespace vizkit3d; WrenchVisualization::WrenchVisualization() + : text_size(0.1) + , show_seperate_axes_force(false) + , show_seperate_axes_torque(false) + , resolution(0.1) { } @@ -17,59 +17,46 @@ WrenchVisualization::~WrenchVisualization() osg::ref_ptr WrenchVisualization::createMainNode() { - // Geode is a common node used for vizkit3d plugins. It allows to display - // "arbitrary" geometries - - double size = 2.0; - - osg::Group* group = new osg::Group; - - //force - osg::MatrixTransform* force_dir = new osg::MatrixTransform; - osg::ref_ptr fg = new osg::Geode(); - osg::ref_ptr fc = new osg::Cylinder(osg::Vec3f(0, 0, 0.05), 0.01, 0.1); - osg::ref_ptr fcd = new osg::ShapeDrawable(fc); - fcd->setColor(osg::Vec4f(1, 0, 0, 1.0)); + wrench_model = new WrenchModel(resolution); + return wrench_model; +} - fg->addDrawable(fcd); - //osg::setColor(osg::Vec4f(1, 0, 0, 1.0), c1g); - force_dir->addChild(fg); - group->addChild(force_dir); - - //torque - osg::MatrixTransform* torque_dir = new osg::MatrixTransform; - osg::ref_ptr tg = new osg::Geode(); - osg::ref_ptr tc = new osg::Cylinder(osg::Vec3f(0, 0, 0.05), 0.01, 0.1); - osg::ref_ptr tcd = new osg::ShapeDrawable(tc); - tcd->setColor(osg::Vec4f(0, 1, 0, 1.0)); - tg->addDrawable(tcd); - //osg::setColor(osg::Vec4f(0.0, 0.0, 0, 1.0), c2g); - torque_dir->addChild(tg); - group->addChild(torque_dir); +double WrenchVisualization::getTextSize() const +{ + return text_size; +} - return group; +void WrenchVisualization::setTextSize(double size) +{ + text_size = size; + text->setCharacterSize(text_size); + emit propertyChanged("textSize"); + setDirty(); } void WrenchVisualization::updateMainNode ( osg::Node* node ) { - // Update the main node using the data in p->data - osg::Group* group = node->asGroup(); - - osg::MatrixTransform* force_transform = dynamic_cast(group->getChild(0)); - osg::Vec3d force(state.force.x(), state.force.y(), state.force.z()); - osg::Matrixd S = osg::Matrix::scale(1, 1, force.length()); - osg::Matrixd R; - R.makeRotate(osg::Vec3d(0,0,1), force); - force_transform->setMatrix(S * R); + // WrenchModel* wrench_model = dynamic_cast(node); + // if (wrench_model) { + // wrench_model->seperateAxes(show_seperate_axes); + // wrench_model->setResolution(resolution); + // wrench_model->update(state); + // } + if (state.isValid()) + wrench_model->update(state); +} - osg::MatrixTransform* torque_transform = dynamic_cast(group->getChild(1)); - osg::Vec3d torque(state.torque.x(), state.torque.y(), state.torque.z()); - S = osg::Matrix::scale(1, 1, torque.length()); - R.makeRotate(osg::Vec3d(0,0,1), torque); - torque_transform->setMatrix(S * R); +void WrenchVisualization::updateDataIntern(const base::Wrench& value) +{ + state.force = value.force; + state.torque = value.torque; } -void WrenchVisualization::updateDataIntern(base::Wrench const& value) +void WrenchVisualization::updateDataIntern(const base::samples::Wrench& value) { state = value; -} \ No newline at end of file +} + +//Macro that makes this plugin loadable in ruby, this is optional. +//VizkitQtPlugin(WrenchVisualization) + diff --git a/viz/WrenchVisualization.hpp b/viz/WrenchVisualization.hpp index 682f36cc..46f31927 100644 --- a/viz/WrenchVisualization.hpp +++ b/viz/WrenchVisualization.hpp @@ -1,40 +1,104 @@ -#ifndef WrenchVisualization_H -#define WrenchVisualization_H +#ifndef teleop_mapping_WrenchVisualization_H +#define teleop_mapping_WrenchVisualization_H #include #include #include -#include +#include +#include +#include +#include +#include "WrenchModel.hpp" namespace vizkit3d { class WrenchVisualization - : public vizkit3d::Vizkit3DPlugin - , boost::noncopyable + : public vizkit3d::Vizkit3DPlugin + , public vizkit3d::VizPluginAddType { Q_OBJECT + Q_PROPERTY(double textSize READ getTextSize WRITE setTextSize) + Q_PROPERTY(bool showForceSeperateAxes READ isSeperateAxesForce WRITE setSeperateAxesForce) + Q_PROPERTY(bool showTorqueSeperateAxes READ isSeperateAxesTorque WRITE setSeperateAxesTorque) + Q_PROPERTY(double resolution READ getResolution WRITE setResolution) public: WrenchVisualization(); ~WrenchVisualization(); - Q_INVOKABLE void updateData(base::Wrench const &sample) - {vizkit3d::Vizkit3DPlugin::updateData(sample);} + Q_INVOKABLE void updateData(base::samples::Wrench const &sample) + {vizkit3d::Vizkit3DPlugin::updateData(sample);} + + Q_INVOKABLE void updateData(base::Wrench const &sample) + {vizkit3d::Vizkit3DPlugin::updateData(sample);} - void setForce(const base::Vector3d& f) { - state.force = f; - } + void setForce(const base::Vector3d& f) { + state.force = f; + } - void setTorque(const base::Vector3d& t) { - state.torque = t; - } + void setTorque(const base::Vector3d& t) { + state.torque = t; + } + + public slots: + /** Sets the text size relative to the size of the complete object. + * If text size is positive, the name of the source frame is rendered in the visualization. + * The default is 0.0 + */ + void setTextSize(double size); + double getTextSize() const; + + void setSeperateAxesForce(bool val = true) { + show_seperate_axes_force = val; + wrench_model->seperateAxesForce(val); + emit propertyChanged("showForceSeperateAxes"); + setDirty(); + } + bool isSeperateAxesForce() const { + return show_seperate_axes_force; + } + void setSeperateAxesTorque(bool val = true) { + show_seperate_axes_torque = val; + wrench_model->seperateAxesTorque(val); + emit propertyChanged("showTorqueSeperateAxes"); + setDirty(); + } + bool isSeperateAxesTorque() const { + return show_seperate_axes_torque; + } + void setResolution(double res) { + resolution = res; + emit propertyChanged("resolution"); + wrench_model->setResolution(res); + setDirty(); + } + double getResolution() const { + return resolution; + } protected: virtual osg::ref_ptr createMainNode(); virtual void updateMainNode(osg::Node* node); - virtual void updateDataIntern(base::Wrench const& plan); + virtual void updateDataIntern( const base::Wrench& wrench ); + virtual void updateDataIntern( const base::samples::Wrench& wrench ); private: - base::Wrench state; + base::samples::Wrench state; + osg::ref_ptr wrench_model; + osg::ref_ptr text; + double text_size; + bool show_seperate_axes_force; + bool show_seperate_axes_torque; + double resolution; + + // public slots: + // void setForce(const base::Vector3d& f) { + // state.force = f; + // } + + // void setTorque(const base::Vector3d& t) { + // state.torque = t; + // } + }; } #endif diff --git a/viz/vizkit_plugin.rb b/viz/vizkit_plugin.rb index dde27110..1ec8543d 100644 --- a/viz/vizkit_plugin.rb +++ b/viz/vizkit_plugin.rb @@ -29,3 +29,9 @@ Vizkit::UiLoader.register_3d_plugin_for('DepthMapVisualization', "/base/samples/DepthMap", :updateDepthMap) Vizkit::UiLoader.register_3d_plugin('DistanceImageVisualization', "base", 'DistanceImageVisualization') Vizkit::UiLoader.register_3d_plugin_for('DistanceImageVisualization', "/base/samples/DistanceImage", :updateDistanceImage) +Vizkit::UiLoader.register_3d_plugin('WrenchVisualization', 'base', 'WrenchVisualization') +Vizkit::UiLoader.register_3d_plugin_for('WrenchVisualization', "/base/Wrench", :updateData ) +Vizkit::UiLoader.register_3d_plugin_for('WrenchVisualization', "/base/samples/Wrench", :updateData ) +Vizkit::UiLoader.register_3d_plugin('RigidBodyStateSE3Visualization',"base", 'RigidBodyStateSE3Visualization') +Vizkit::UiLoader.register_3d_plugin_for('RigidBodyStateSE3Visualization', "/base/samples/RigidBodyStateSE3", :updateRigidBodyStateSE3) +Vizkit::UiLoader.register_3d_plugin_for('RigidBodyStateSE3Visualization', "/base/samples/RigidBodyStateSE3", :updateData) \ No newline at end of file From e6b0c1884371b24e52ea27ade25765ccfbafa774 Mon Sep 17 00:00:00 2001 From: Michael Maurus Date: Tue, 2 Feb 2021 12:51:18 +0100 Subject: [PATCH 3/5] added test script vor wrench visualization --- test/viz/test_wrench_visualization.rb | 112 ++++++++++++++++++++++++++ 1 file changed, 112 insertions(+) create mode 100644 test/viz/test_wrench_visualization.rb diff --git a/test/viz/test_wrench_visualization.rb b/test/viz/test_wrench_visualization.rb new file mode 100644 index 00000000..bda7a301 --- /dev/null +++ b/test/viz/test_wrench_visualization.rb @@ -0,0 +1,112 @@ +require_relative 'helpers' +require 'orocos' +Orocos.load_typekit 'base' + +module Vizkit + describe 'WrenchVisualization' do + include TestHelpers + + before do + register_widget(@vizkit3d_widget = + Vizkit.default_loader.create_plugin("vizkit3d::Vizkit3DWidget")) + @viz = @vizkit3d_widget.createPlugin('base', 'WrenchVisualization') + + @wrench = Types.base.samples.Wrench.new + @wrench.frame_id = "test_frame" + @wrench.force = Types.base.Vector3d.new(1,1,1) + @wrench.torque = Types.base.Vector3d.new(-1,-1,1) + + @vizkit3d_widget.setCameraEye(5, 0, 20) + @vizkit3d_widget.setCameraLookAt(5, 0, 0) + @vizkit3d_widget.show + end + + describe "updateData(Wrench)" do + wrench = Types.base.samples.Wrench.new + wrench.frame_id = "test_frame" + wrench.force = Types.base.Vector3d.new(1,1,1) + wrench.torque = Types.base.Vector3d.new(-1,-1,1) + + it "updates wrench" do + @viz.updateData(wrench) + confirm 'Force should be displayed as a red arrow from (0,0,0) to (1,1,1) and torque as a green arrow from (0,0,0) to (-1,-1,1) with a circular arrow rotating counter-clockwise around axis' + end + + it "updates wrench" do + wrench.force = Types.base.Vector3d.new(1,0,0) + wrench.torque = Types.base.Vector3d.new(1,0,0) + @viz.updateData(wrench) + confirm 'Force should be displayed as a red arrow from (0,0,0) to (1,0,0) and torque as a green arrow from (0,0,0) to (1,0,0) with a circular arrow rotating counter-clockwise around axis' + end + + it "updates wrench" do + wrench.force = Types.base.Vector3d.new(0,1,0) + wrench.torque = Types.base.Vector3d.new(0,1,0) + @viz.updateData(wrench) + confirm 'Force should be displayed as a red arrow from (0,0,0) to (0,1,0) and torque as a green arrow from (0,0,0) to (0,1,0) with a circular arrow rotating counter-clockwise around axis' + end + + it "updates wrench" do + wrench.force = Types.base.Vector3d.new(0,0,1) + wrench.torque = Types.base.Vector3d.new(0,0,1) + @viz.updateData(wrench) + confirm 'Force should be displayed as a red arrow from (0,0,0) to (0,0,1) and torque as a green arrow from (0,0,0) to (0,0,1) with a circular arrow rotating counter-clockwise around axis' + end + + it "updates wrench" do + wrench.force = Types.base.Vector3d.new(0,0,0) + wrench.torque = Types.base.Vector3d.new(0,0,0) + @viz.updateData(wrench) + confirm 'Nothing should be displayed' + end + + it "updates wrench" do + wrench.force = Types.base.Vector3d.new(0.1,0,0) + wrench.torque = Types.base.Vector3d.new(0.3,0,0) + @viz.updateData(wrench) + confirm 'Force should be displayed as a red arrow from (0,0,0) to (0,0,0.1) and torque as a green arrow from (0,0,0) to (0.3,0,0) with a circular arrow rotating counter-clockwise around axis. Both arrows should be displayed without the arrow head' + end + end + + describe "setSeperateAxesTorque" do + it "displays torque axis component" do + torque = Type.base.Vector3d.new(1,1,1) + @viz.setTorque(torque) + @viz.setSeperateAxesTorque(true) + confirm 'Torque should be displayed as three arrows along axes each with length 1 and a circular arrow rotating counter-clockwise' + @viz.setSeperateAxesTorque(false) + confirm 'Torque should be displayed as one arrows starting from (0,0,0) to (1,1,1) with circular arrow rotating counter-clockwise' + end + end + + describe "setSeperateAxesForce" do + it "displays force axis component" do + force = Type.base.Vector3d.new(1,1,1) + @viz.setForce(force) + @viz.setSeperateAxesForce(true) + confirm 'Force should be displayed as three arrows along axes each with length 1' + @viz.setSeperateAxesForce(false) + confirm 'Force should be displayed as one arrows starting from (0,0,0) to (1,1,1)' + end + end + + describe "setResolution" do + wrench = Types.base.samples.Wrench.new + wrench.frame_id = "test_frame" + wrench.force = Types.base.Vector3d.new(1,0,0) + wrench.torque = Types.base.Vector3d.new(0,1,0) + + it "sets resolution to 1" do + @viz.updateData(wrench) + @viz.setResolution(1.0) + confirm "Force and torque will be displayed as arrows from (0,0,0) to (1,0,0) / (0,1,0)" + end + it "it sets resolution to 0.1" do + @viz.updateData(wrench) + @viz.setResolution(0.1) + confirm "Force and torque will be displayed as arrows from (0,0,0) to (0.1,0,0) / (0,0.1,0) without arrow head" + end + end + end +end + From 3374a6105aca9c5fbb9fa598fd729a8d567c7d73 Mon Sep 17 00:00:00 2001 From: Michael Maurus Date: Tue, 2 Feb 2021 12:51:32 +0100 Subject: [PATCH 4/5] fixed intendation and empty lines --- viz/PluginLoader.cpp | 182 +++++++++++++++++++++---------------------- viz/WrenchModel.cpp | 11 --- 2 files changed, 91 insertions(+), 102 deletions(-) diff --git a/viz/PluginLoader.cpp b/viz/PluginLoader.cpp index 31dbb3de..7cbe27f6 100644 --- a/viz/PluginLoader.cpp +++ b/viz/PluginLoader.cpp @@ -18,99 +18,99 @@ namespace vizkit3d { class QtPluginVizkitBase : public vizkit3d::VizkitPluginFactory { private: public: - - QtPluginVizkitBase() { - } - - /** - * Returns a list of all available visualization plugins. - * @return list of plugin names - */ - virtual QStringList* getAvailablePlugins() const - { - QStringList *pluginNames = new QStringList(); - pluginNames->push_back("WaypointVisualization"); - pluginNames->push_back("TrajectoryVisualization"); - pluginNames->push_back("MotionCommandVisualization"); - pluginNames->push_back("RigidBodyStateVisualization"); - pluginNames->push_back("BodyStateVisualization"); - pluginNames->push_back("LaserScanVisualization"); - pluginNames->push_back("SonarGroundDistanceVisualization"); - pluginNames->push_back("PointcloudVisualization"); - pluginNames->push_back("SonarBeamVisualization"); - pluginNames->push_back("SonarVisualization"); - pluginNames->push_back("DepthMapVisualization"); - pluginNames->push_back("DistanceImageVisualization"); - pluginNames->push_back("RigidBodyStateSE3Visualization"); - pluginNames->push_back("WrenchVisualization"); - return pluginNames; - } - - virtual QObject* createPlugin(QString const& pluginName) + + QtPluginVizkitBase() { + } + + /** + * Returns a list of all available visualization plugins. + * @return list of plugin names + */ + virtual QStringList* getAvailablePlugins() const + { + QStringList *pluginNames = new QStringList(); + pluginNames->push_back("WaypointVisualization"); + pluginNames->push_back("TrajectoryVisualization"); + pluginNames->push_back("MotionCommandVisualization"); + pluginNames->push_back("RigidBodyStateVisualization"); + pluginNames->push_back("BodyStateVisualization"); + pluginNames->push_back("LaserScanVisualization"); + pluginNames->push_back("SonarGroundDistanceVisualization"); + pluginNames->push_back("PointcloudVisualization"); + pluginNames->push_back("SonarBeamVisualization"); + pluginNames->push_back("SonarVisualization"); + pluginNames->push_back("DepthMapVisualization"); + pluginNames->push_back("DistanceImageVisualization"); + pluginNames->push_back("RigidBodyStateSE3Visualization"); + pluginNames->push_back("WrenchVisualization"); + return pluginNames; + } + + virtual QObject* createPlugin(QString const& pluginName) + { + vizkit3d::VizPluginBase* plugin = 0; + if (pluginName == "WaypointVisualization") + { + plugin = new vizkit3d::WaypointVisualization(); + } + else if (pluginName == "MotionCommandVisualization") + { + plugin = new vizkit3d::MotionCommandVisualization(); + } + else if (pluginName == "TrajectoryVisualization") + { + plugin = new vizkit3d::TrajectoryVisualization(); + } + else if (pluginName == "RigidBodyStateVisualization") + { + plugin = new vizkit3d::RigidBodyStateVisualization(); + } + else if (pluginName == "BodyStateVisualization") + { + plugin = new vizkit3d::BodyStateVisualization(); + } + else if (pluginName == "LaserScanVisualization") + { + plugin = new vizkit3d::LaserScanVisualization(); + } + else if (pluginName == "SonarGroundDistanceVisualization") { - vizkit3d::VizPluginBase* plugin = 0; - if (pluginName == "WaypointVisualization") - { - plugin = new vizkit3d::WaypointVisualization(); - } - else if (pluginName == "MotionCommandVisualization") - { - plugin = new vizkit3d::MotionCommandVisualization(); - } - else if (pluginName == "TrajectoryVisualization") - { - plugin = new vizkit3d::TrajectoryVisualization(); - } - else if (pluginName == "RigidBodyStateVisualization") - { - plugin = new vizkit3d::RigidBodyStateVisualization(); - } - else if (pluginName == "BodyStateVisualization") - { - plugin = new vizkit3d::BodyStateVisualization(); - } - else if (pluginName == "LaserScanVisualization") - { - plugin = new vizkit3d::LaserScanVisualization(); - } - else if (pluginName == "SonarGroundDistanceVisualization") - { - plugin = new vizkit3d::SonarGroundDistanceVisualization(); - } - else if (pluginName == "PointcloudVisualization") - { - plugin = new vizkit3d::PointcloudVisualization(); - } - else if (pluginName == "SonarBeamVisualization") - { - plugin = new vizkit3d::SonarBeamVisualization(); - } - else if (pluginName == "SonarVisualization") - { - plugin = new vizkit3d::SonarVisualization(); - } - else if (pluginName == "DepthMapVisualization") - { - plugin = new vizkit3d::DepthMapVisualization(); - } - else if (pluginName == "DistanceImageVisualization") - { - plugin = new vizkit3d::DistanceImageVisualization(); - } - else if (pluginName == "RigidBodyStateSE3Visualization") - { - plugin = new vizkit3d::RigidBodyStateSE3Visualization(); - } - else if (pluginName == "WrenchVisualization") - { - plugin = new vizkit3d::WrenchVisualization(); - } + plugin = new vizkit3d::SonarGroundDistanceVisualization(); + } + else if (pluginName == "PointcloudVisualization") + { + plugin = new vizkit3d::PointcloudVisualization(); + } + else if (pluginName == "SonarBeamVisualization") + { + plugin = new vizkit3d::SonarBeamVisualization(); + } + else if (pluginName == "SonarVisualization") + { + plugin = new vizkit3d::SonarVisualization(); + } + else if (pluginName == "DepthMapVisualization") + { + plugin = new vizkit3d::DepthMapVisualization(); + } + else if (pluginName == "DistanceImageVisualization") + { + plugin = new vizkit3d::DistanceImageVisualization(); + } + else if (pluginName == "RigidBodyStateSE3Visualization") + { + plugin = new vizkit3d::RigidBodyStateSE3Visualization(); + } + else if (pluginName == "WrenchVisualization") + { + plugin = new vizkit3d::WrenchVisualization(); + } - if (plugin) - { - return plugin; - } - return NULL; + if (plugin) + { + return plugin; + } + return NULL; }; }; Q_EXPORT_PLUGIN2(QtPluginVizkitBase, QtPluginVizkitBase) diff --git a/viz/WrenchModel.cpp b/viz/WrenchModel.cpp index 134a2b6c..4822185a 100644 --- a/viz/WrenchModel.cpp +++ b/viz/WrenchModel.cpp @@ -14,20 +14,12 @@ WrenchModel::WrenchModel(double resolution) } osg::ref_ptr WrenchModel::createCircularArrow(const osg::Vec4f& color) { - // osg::ref_ptr arr = new CircularArrow(0.2, 0.02, 12, 32, 1.5*M_PI); - // arr->setColor(color); - // arr->rebuildGeometry(); - osg::ref_ptr arr = primitivesfactory->createCircularArrow(0.1, 0.02, 12, 32, 1.5*M_PI, 2.0, color); - return arr; } void WrenchModel::buildGeometry() { - - - // force force_node = primitivesfactory->createArrow(osg::Vec4f(1, 0, 0, 1.0), true); addChild(force_node, true); // child 0 @@ -51,7 +43,6 @@ void WrenchModel::buildGeometry() { osg::ref_ptr torque_axis_group = new osg::Group; torque_axis_group->addChild(primitivesfactory->createArrow(osg::Vec4f(0, 1, 0, 1.0), true)); torque_axis_group->addChild(createCircularArrow(osg::Vec4f(0, 1, 0, 1.0))); - torque_axes_group->addChild(torque_axis_group, true); } @@ -133,14 +124,12 @@ void WrenchModel::update(const base::Wrench& wrench) { osgviz::ArrowNode* torque_vec_transform = dynamic_cast(torque_group->getChild(0)); osg::Vec3d torque(wrench.torque.x(), wrench.torque.y(), wrench.torque.z()); Q.makeRotate(osg::Vec3d(0,0,1), torque); - //torque_vec_transform->setScale(osg::Vec3d(resolution, resolution, resolution*torque.length())); torque_vec_transform->setLength(resolution*torque.length()); torque_vec_transform->setAttitude(Q); osg::PositionAttitudeTransform* torque_transform = dynamic_cast(torque_group->getChild(1)); torque_transform->setAttitude(Q); torque_transform->setPosition(torque * 0.4 * resolution); - //torque_transform->setScale(osg::Vec3f(resolution,resolution,resolution)); setChildValue(torque_group, true); } } From 3b0eaea472ba5625de78fb522e944c637a01518e Mon Sep 17 00:00:00 2001 From: Michael Maurus Date: Thu, 4 Feb 2021 12:52:18 +0100 Subject: [PATCH 5/5] addded test for WrenchVisualization --- test/viz/test_wrench_visualization.rb | 148 ++++++++++++++------------ viz/WrenchModel.hpp | 1 + viz/WrenchVisualization.cpp | 4 +- viz/WrenchVisualization.hpp | 8 +- 4 files changed, 88 insertions(+), 73 deletions(-) diff --git a/test/viz/test_wrench_visualization.rb b/test/viz/test_wrench_visualization.rb index bda7a301..4ae40fef 100644 --- a/test/viz/test_wrench_visualization.rb +++ b/test/viz/test_wrench_visualization.rb @@ -15,65 +15,74 @@ module Vizkit @wrench.frame_id = "test_frame" @wrench.force = Types.base.Vector3d.new(1,1,1) @wrench.torque = Types.base.Vector3d.new(-1,-1,1) + @viz.updateData(@wrench) - @vizkit3d_widget.setCameraEye(5, 0, 20) - @vizkit3d_widget.setCameraLookAt(5, 0, 0) + @viz.setResolution(1.0) + + @vizkit3d_widget.setCameraEye(5, 4, 5) + @vizkit3d_widget.setCameraLookAt(0, 0, 0) @vizkit3d_widget.show end - describe "updateData(Wrench)" do - wrench = Types.base.samples.Wrench.new - wrench.frame_id = "test_frame" - wrench.force = Types.base.Vector3d.new(1,1,1) - wrench.torque = Types.base.Vector3d.new(-1,-1,1) - - it "updates wrench" do - @viz.updateData(wrench) - confirm 'Force should be displayed as a red arrow from (0,0,0) to (1,1,1) and torque as a green arrow from (0,0,0) to (-1,-1,1) with a circular arrow rotating counter-clockwise around axis' - end - - it "updates wrench" do - wrench.force = Types.base.Vector3d.new(1,0,0) - wrench.torque = Types.base.Vector3d.new(1,0,0) - @viz.updateData(wrench) - confirm 'Force should be displayed as a red arrow from (0,0,0) to (1,0,0) and torque as a green arrow from (0,0,0) to (1,0,0) with a circular arrow rotating counter-clockwise around axis' - end - - it "updates wrench" do - wrench.force = Types.base.Vector3d.new(0,1,0) - wrench.torque = Types.base.Vector3d.new(0,1,0) - @viz.updateData(wrench) - confirm 'Force should be displayed as a red arrow from (0,0,0) to (0,1,0) and torque as a green arrow from (0,0,0) to (0,1,0) with a circular arrow rotating counter-clockwise around axis' - end - - it "updates wrench" do - wrench.force = Types.base.Vector3d.new(0,0,1) - wrench.torque = Types.base.Vector3d.new(0,0,1) - @viz.updateData(wrench) - confirm 'Force should be displayed as a red arrow from (0,0,0) to (0,0,1) and torque as a green arrow from (0,0,0) to (0,0,1) with a circular arrow rotating counter-clockwise around axis' - end - - it "updates wrench" do - wrench.force = Types.base.Vector3d.new(0,0,0) - wrench.torque = Types.base.Vector3d.new(0,0,0) - @viz.updateData(wrench) - confirm 'Nothing should be displayed' - end - - it "updates wrench" do - wrench.force = Types.base.Vector3d.new(0.1,0,0) - wrench.torque = Types.base.Vector3d.new(0.3,0,0) - @viz.updateData(wrench) - confirm 'Force should be displayed as a red arrow from (0,0,0) to (0,0,0.1) and torque as a green arrow from (0,0,0) to (0.3,0,0) with a circular arrow rotating counter-clockwise around axis. Both arrows should be displayed without the arrow head' - end - end + # describe "updateData(Wrench)" do + # @wrench = Types.base.samples.Wrench.new + # @wrench.frame_id = "test_frame" + # @wrench.force = Types.base.Vector3d.new(1,1,1) + # @wrench.torque = Types.base.Vector3d.new(-1,-1,1) + + # it "updates wrench" do + # @viz.updateData(@wrench) + # confirm 'Force should be displayed as a red arrow from (0,0,0) to (1,1,1) and torque as a green arrow from (0,0,0) to (-1,-1,1) with a circular arrow rotating counter-clockwise around axis' + # end + + # it "updates wrench" do + # @wrench.force = Types.base.Vector3d.new(1,0,0) + # @wrench.torque = Types.base.Vector3d.new(0,1,0) + # @viz.updateData(@wrench) + # confirm 'Force should be displayed as a red arrow from (0,0,0) to (1,0,0) and torque as a green arrow from (0,0,0) to (0,1,0) with a circular arrow rotating counter-clockwise around axis' + # end + + # it "updates wrench" do + # @wrench.force = Types.base.Vector3d.new(0,1,0) + # @wrench.torque = Types.base.Vector3d.new(0,0,1) + # @viz.updateData(@wrench) + # confirm 'Force should be displayed as a red arrow from (0,0,0) to (0,1,0) and torque as a green arrow from (0,0,0) to (0,0,1) with a circular arrow rotating counter-clockwise around axis' + # end + + # it "updates wrench" do + # @wrench.force = Types.base.Vector3d.new(0,0,1) + # @wrench.torque = Types.base.Vector3d.new(1,0,0) + # @viz.updateData(@wrench) + # confirm 'Force should be displayed as a red arrow from (0,0,0) to (0,0,1) and torque as a green arrow from (0,0,0) to (1,0,0) with a circular arrow rotating counter-clockwise around axis' + # end + + # it "updates wrench" do + # @wrench.force = Types.base.Vector3d.new(0,0,0) + # @wrench.torque = Types.base.Vector3d.new(0,0,0) + # @viz.updateData(@wrench) + # confirm 'Nothing should be displayed' + # end + + # it "updates wrench" do + # @wrench.force = Types.base.Vector3d.new(0.1,0,0) + # @wrench.torque = Types.base.Vector3d.new(0.3,0,0) + # @viz.updateData(@wrench) + # confirm 'Force should be displayed as a red arrow from (0,0,0) to (0,0,0.1) and torque as a green arrow from (0,0,0) to (0.3,0,0) with a circular arrow rotating counter-clockwise around axis. Both arrows should be displayed without the arrow head' + # end + # end describe "setSeperateAxesTorque" do it "displays torque axis component" do - torque = Type.base.Vector3d.new(1,1,1) + torque = Types.base.Vector3d.new(1,1,1) @viz.setTorque(torque) @viz.setSeperateAxesTorque(true) confirm 'Torque should be displayed as three arrows along axes each with length 1 and a circular arrow rotating counter-clockwise' + + end + + it "displays torque" do + torque = Types.base.Vector3d.new(1,1,1) + @viz.setTorque(torque) @viz.setSeperateAxesTorque(false) confirm 'Torque should be displayed as one arrows starting from (0,0,0) to (1,1,1) with circular arrow rotating counter-clockwise' end @@ -81,32 +90,37 @@ module Vizkit describe "setSeperateAxesForce" do it "displays force axis component" do - force = Type.base.Vector3d.new(1,1,1) + force = Types.base.Vector3d.new(1,1,1) @viz.setForce(force) @viz.setSeperateAxesForce(true) confirm 'Force should be displayed as three arrows along axes each with length 1' + end + + it "displays force" do + force = Types.base.Vector3d.new(1,1,1) + @viz.setForce(force) @viz.setSeperateAxesForce(false) confirm 'Force should be displayed as one arrows starting from (0,0,0) to (1,1,1)' end end - describe "setResolution" do - wrench = Types.base.samples.Wrench.new - wrench.frame_id = "test_frame" - wrench.force = Types.base.Vector3d.new(1,0,0) - wrench.torque = Types.base.Vector3d.new(0,1,0) - - it "sets resolution to 1" do - @viz.updateData(wrench) - @viz.setResolution(1.0) - confirm "Force and torque will be displayed as arrows from (0,0,0) to (1,0,0) / (0,1,0)" - end - it "it sets resolution to 0.1" do - @viz.updateData(wrench) - @viz.setResolution(0.1) - confirm "Force and torque will be displayed as arrows from (0,0,0) to (0.1,0,0) / (0,0.1,0) without arrow head" - end - end + # describe "setResolution" do + # wrench = Types.base.samples.Wrench.new + # wrench.frame_id = "test_frame" + # wrench.force = Types.base.Vector3d.new(1,0,0) + # wrench.torque = Types.base.Vector3d.new(0,1,0) + + # it "sets resolution to 1" do + # @viz.updateData(wrench) + # @viz.setResolution(1.0) + # confirm "Force and torque will be displayed as arrows from (0,0,0) to (1,0,0) / (0,1,0)" + # end + # it "it sets resolution to 0.1" do + # @viz.updateData(wrench) + # @viz.setResolution(0.1) + # confirm "Force and torque will be displayed as arrows from (0,0,0) to (0.1,0,0) / (0,0.1,0) without arrow head" + # end + # end end end diff --git a/viz/WrenchModel.hpp b/viz/WrenchModel.hpp index 7ff72192..2f74a400 100644 --- a/viz/WrenchModel.hpp +++ b/viz/WrenchModel.hpp @@ -12,6 +12,7 @@ class WrenchModel : public osg::Switch { public: WrenchModel(double resolution = 1.0); + ~WrenchModel() {} void update(const base::Wrench& wrench); diff --git a/viz/WrenchVisualization.cpp b/viz/WrenchVisualization.cpp index 01143ecd..bcd8cdfd 100644 --- a/viz/WrenchVisualization.cpp +++ b/viz/WrenchVisualization.cpp @@ -7,8 +7,9 @@ WrenchVisualization::WrenchVisualization() : text_size(0.1) , show_seperate_axes_force(false) , show_seperate_axes_torque(false) - , resolution(0.1) + , resolution(1.0) { + wrench_model = new WrenchModel(resolution); } WrenchVisualization::~WrenchVisualization() @@ -17,7 +18,6 @@ WrenchVisualization::~WrenchVisualization() osg::ref_ptr WrenchVisualization::createMainNode() { - wrench_model = new WrenchModel(resolution); return wrench_model; } diff --git a/viz/WrenchVisualization.hpp b/viz/WrenchVisualization.hpp index 46f31927..fa84559f 100644 --- a/viz/WrenchVisualization.hpp +++ b/viz/WrenchVisualization.hpp @@ -31,11 +31,11 @@ namespace vizkit3d Q_INVOKABLE void updateData(base::Wrench const &sample) {vizkit3d::Vizkit3DPlugin::updateData(sample);} - void setForce(const base::Vector3d& f) { + Q_INVOKABLE void setForce(const base::Vector3d& f) { state.force = f; } - void setTorque(const base::Vector3d& t) { + Q_INVOKABLE void setTorque(const base::Vector3d& t) { state.torque = t; } @@ -49,8 +49,8 @@ namespace vizkit3d void setSeperateAxesForce(bool val = true) { show_seperate_axes_force = val; - wrench_model->seperateAxesForce(val); emit propertyChanged("showForceSeperateAxes"); + wrench_model->seperateAxesForce(val); setDirty(); } bool isSeperateAxesForce() const { @@ -58,8 +58,8 @@ namespace vizkit3d } void setSeperateAxesTorque(bool val = true) { show_seperate_axes_torque = val; - wrench_model->seperateAxesTorque(val); emit propertyChanged("showTorqueSeperateAxes"); + wrench_model->seperateAxesTorque(val); setDirty(); } bool isSeperateAxesTorque() const {