EnvironmentalPrimitive.cpp

Go to the documentation of this file.

/*
* This file is part of ArmarX.
*
* ArmarX is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License version 2 as
* published by the Free Software Foundation.
*
* ArmarX is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*
* @package    MemoryX::Core
* @author     Peter Kaiser <peter dot kaiser at kit dot edu>
* @date       2014
* @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
*             GNU General Public License
*/
 
#include "EnvironmentalPrimitive.h"
 
#include <SimoxUtility/algorithm/string/string_tools.h>
 
#include <ArmarXCore/core/util/StringHelpers.h>
#include <ArmarXCore/util/variants/eigen3/MatrixVariant.h>
 
#include <RobotAPI/libraries/core/FramedPose.h>
 
namespace memoryx
{
    EnvironmentalPrimitive::EnvironmentalPrimitive(const std::string& name,
                                                   const std::string& id,
                                                   const std::string& shape) :
        Entity()
    {
        setName(name);
        setId(id);
        setShape(shape);
    }
 
    std::string
    EnvironmentalPrimitive::getShape(const Ice::Current& c) const
    {
        return getAttributeValue("shape")->getString();
    }
 
    armarx::Vector3BasePtr
    memoryx::EnvironmentalPrimitive::getOBBDimensions(const Ice::Current&) const
    {
        return getAttributeValue("dimensionsOBB")->getClass<armarx::Vector3Base>();
    }
 
    void
    memoryx::EnvironmentalPrimitive::setOBBDimensions(const armarx::Vector3BasePtr& dimensions,
                                                      const Ice::Current&)
    {
        putAttribute("dimensionsOBB", dimensions);
    }
 
    armarx::FramedPoseBasePtr
    memoryx::EnvironmentalPrimitive::getPose(const Ice::Current&) const
    {
        return getAttributeValue("pose")->getClass<armarx::FramedPoseBase>();
    }
 
    void
    memoryx::EnvironmentalPrimitive::setPose(const armarx::FramedPoseBasePtr& pose,
                                             const Ice::Current&)
    {
        putAttribute("pose", pose);
    }
 
    PointList
    memoryx::EnvironmentalPrimitive::getGraspPoints(const Ice::Current&) const
    {
        auto entity = getAttribute("graspPoints");
        if (!entity || entity->size() <= 0)
        {
            return PointList();
        }
 
        Eigen::MatrixXf m = armarx::VariantPtr::dynamicCast(entity->getValueAt(0))
                                ->getClass<armarx::MatrixFloat>()
                                ->toEigen();
 
        PointList result;
        result.reserve(m.cols());
 
        for (unsigned int i = 0; i < m.cols(); i++)
        {
            result.push_back(new armarx::Vector3(Eigen::Vector3f(m.block<3, 1>(0, i))));
        }
 
        return result;
    }
 
    void
    EnvironmentalPrimitive::setGraspPoints(const PointList& graspPoints, const Ice::Current& c)
    {
        Eigen::MatrixXf m(3, graspPoints.size());
 
        for (unsigned int i = 0; i < graspPoints.size(); i++)
        {
            m.block<3, 1>(0, i) = armarx::Vector3Ptr::dynamicCast(graspPoints[i])->toEigen();
        }
 
        armarx::MatrixFloatBasePtr matrix(new armarx::MatrixFloat(m));
 
        armarx::VariantPtr variant(new armarx::Variant);
        variant->set<armarx::MatrixFloat>(matrix);
 
        EntityAttributeBasePtr entity = new EntityAttribute("graspPoints");
        entity->addValue(variant);
        putAttribute(entity);
    }
 
    PointList
    memoryx::EnvironmentalPrimitive::getInliers(const Ice::Current&) const
    {
        auto entity = getAttribute("inliers");
        if (!entity || entity->size() <= 0)
        {
            return PointList();
        }
 
        Eigen::MatrixXf m = armarx::VariantPtr::dynamicCast(entity->getValueAt(0))
                                ->getClass<armarx::MatrixFloat>()
                                ->toEigen();
 
        PointList result;
        result.reserve(m.cols());
 
        for (unsigned int i = 0; i < m.cols(); i++)
        {
            result.push_back(new armarx::Vector3(Eigen::Vector3f(m.block<3, 1>(0, i))));
        }
 
        return result;
    }
 
    void
    EnvironmentalPrimitive::setInliers(const PointList& inliers, const Ice::Current& c)
    {
        Eigen::MatrixXf m(3, inliers.size());
 
        for (unsigned int i = 0; i < inliers.size(); i++)
        {
            m.block<3, 1>(0, i) = armarx::Vector3Ptr::dynamicCast(inliers[i])->toEigen();
        }
 
        armarx::MatrixFloatBasePtr matrix(new armarx::MatrixFloat(m));
 
        armarx::VariantPtr variant(new armarx::Variant);
        variant->set<armarx::MatrixFloat>(matrix);
 
        EntityAttributeBasePtr entity = new EntityAttribute("inliers");
        entity->addValue(variant);
        putAttribute(entity);
    }
 
    float
    memoryx::EnvironmentalPrimitive::getProbability(const Ice::Current&) const
    {
        return getAttributeValue("probability")->getFloat();
    }
 
    void
    memoryx::EnvironmentalPrimitive::setProbability(float probability, const Ice::Current&)
    {
        putAttribute("probability", probability);
    }
 
    float
    memoryx::EnvironmentalPrimitive::getCircularityProbability(const Ice::Current&) const
    {
        return getAttributeValue("circularityprobability")->getFloat();
    }
 
    void
    memoryx::EnvironmentalPrimitive::setCircularityProbability(float probability,
                                                               const Ice::Current&)
    {
        putAttribute("circularityprobability", probability);
    }
 
    float
    memoryx::EnvironmentalPrimitive::getLength(const Ice::Current&) const
    {
        return getAttributeValue("length")->getFloat();
    }
 
    void
    memoryx::EnvironmentalPrimitive::setLength(float length, const Ice::Current&)
    {
        putAttribute("length", length);
    }
 
    int
    EnvironmentalPrimitive::getLabel(const Ice::Current& c) const
    {
        return getAttributeValue("label")->getInt();
    }
 
    void
    EnvironmentalPrimitive::setLabel(int label, const Ice::Current& c)
    {
        putAttribute("label", label);
    }
 
    armarx::TimestampBasePtr
    memoryx::EnvironmentalPrimitive::getTime(const Ice::Current&) const
    {
        return getAttributeValue("time")->getClass<armarx::TimestampBase>();
    }
 
    void
    memoryx::EnvironmentalPrimitive::setTime(const armarx::TimestampBasePtr& time,
                                             const Ice::Current&)
    {
        putAttribute("time", time);
    }
 
    armarx::MatrixFloatBasePtr
    EnvironmentalPrimitive::getSampling(const Ice::Current& c) const
    {
        auto entity = getAttribute("sampling");
        if (!entity || entity->size() <= 0)
        {
            return armarx::MatrixFloatBasePtr(new armarx::MatrixFloat(4, 0));
        }
 
        return armarx::VariantPtr::dynamicCast(entity->getValueAt(0))
            ->getClass<armarx::MatrixFloat>();
    }
 
    void
    EnvironmentalPrimitive::setSampling(const armarx::MatrixFloatBasePtr& sampling,
                                        const Ice::Current& c)
    {
        armarx::VariantPtr variant(new armarx::Variant);
        variant->set<armarx::MatrixFloat>(sampling);
 
        EntityAttributeBasePtr entity = new EntityAttribute("sampling");
        entity->addValue(variant);
        putAttribute(entity);
    }
 
    Ice::ObjectPtr
    EnvironmentalPrimitive::ice_clone() const
    {
        return this->clone();
    }
 
    EnvironmentalPrimitivePtr
    EnvironmentalPrimitive::clone(const Ice::Current& c) const
    {
        EnvironmentalPrimitivePtr ret = new EnvironmentalPrimitive(*this);
        return ret;
    }
 
    void
    EnvironmentalPrimitive::setShape(const std::string& shape, const Ice::Current& c)
    {
        putAttribute("shape", shape);
    }
 
    memoryx::PlanePrimitive::PlanePrimitive(const std::string& name, const std::string& id) :
        EnvironmentalPrimitive(name, id, "plane")
    {
    }
 
    memoryx::PlanePrimitive::PlanePrimitive(std::vector<float>& coeffs) :
        EnvironmentalPrimitive("", "", "plane")
    {
        armarx::Vector3BasePtr planeNormal = new armarx::Vector3(coeffs[0], coeffs[1], coeffs[2]);
        setPlaneNormal(planeNormal);
        setPlaneDistance(coeffs[3]);
    }
 
    armarx::Vector3BasePtr
    memoryx::PlanePrimitive::getPlaneNormal(const Ice::Current&) const
    {
        return getAttributeValue("normal")->getClass<armarx::Vector3Base>();
    }
 
    void
    memoryx::PlanePrimitive::setPlaneNormal(const armarx::Vector3BasePtr& normal,
                                            const Ice::Current&)
    {
        putAttribute("normal", normal);
    }
 
    float
    memoryx::PlanePrimitive::getPlaneDistance(const Ice::Current&) const
    {
        return getAttributeValue("distance")->getFloat();
    }
 
    void
    memoryx::PlanePrimitive::setPlaneDistance(float distance, const Ice::Current&)
    {
        putAttribute("distance", distance);
    }
 
    Ice::ObjectPtr
    memoryx::PlanePrimitive::ice_clone() const
    {
        return this->clone();
    }
 
    PlanePrimitivePtr
    memoryx::PlanePrimitive::clone(const Ice::Current&) const
    {
        PlanePrimitivePtr ret = new PlanePrimitive(*this);
        return ret;
    }
 
    memoryx::SpherePrimitive::SpherePrimitive(const std::string& name, const std::string& id) :
        EnvironmentalPrimitive(name, id, "sphere")
    {
    }
 
    memoryx::SpherePrimitive::SpherePrimitive(std::vector<float>& coeffs) :
        EnvironmentalPrimitive("", "", "sphere")
    {
        setSphereCenter(new armarx::Vector3(coeffs[0], coeffs[1], coeffs[2]));
        setSphereRadius(coeffs[3]);
    }
 
    armarx::Vector3BasePtr
    SpherePrimitive::getSphereCenter(const Ice::Current& c) const
    {
        return getAttributeValue("center")->getClass<armarx::Vector3Base>();
    }
 
    void
    SpherePrimitive::setSphereCenter(const armarx::Vector3BasePtr& center, const Ice::Current& c)
    {
        putAttribute("center", center);
    }
 
    float
    SpherePrimitive::getSphereRadius(const Ice::Current& c) const
    {
        return getAttributeValue("radius")->getFloat();
    }
 
    void
    SpherePrimitive::setSphereRadius(float radius, const Ice::Current& c)
    {
        putAttribute("radius", radius);
    }
 
    Ice::ObjectPtr
    memoryx::SpherePrimitive::ice_clone() const
    {
        return this->clone();
    }
 
    SpherePrimitivePtr
    memoryx::SpherePrimitive::clone(const Ice::Current&) const
    {
        SpherePrimitivePtr ret = new SpherePrimitive(*this);
        return ret;
    }
 
    memoryx::CylinderPrimitive::CylinderPrimitive(const std::string& name, const std::string& id) :
        EnvironmentalPrimitive(name, id, "cylinder")
    {
    }
 
    memoryx::CylinderPrimitive::CylinderPrimitive(std::vector<float>& coeffs) :
        EnvironmentalPrimitive("", "", "cylinder")
    {
        Eigen::Vector3f base;
        base << coeffs[0], coeffs[1], coeffs[2];
 
        Eigen::Vector3f direction;
        direction << coeffs[3], coeffs[4], coeffs[5];
 
        setCylinderPoint(new armarx::Vector3(base));
        setCylinderAxisDirection(new armarx::Vector3(direction.normalized()));
        setCylinderRadius(coeffs[6]);
        setLength(direction.norm());
    }
 
    armarx::Vector3BasePtr
    CylinderPrimitive::getCylinderPoint(const Ice::Current& c) const
    {
        return getAttributeValue("point")->getClass<armarx::Vector3Base>();
    }
 
    void
    CylinderPrimitive::setCylinderPoint(const armarx::Vector3BasePtr& point, const Ice::Current& c)
    {
        putAttribute("point", point);
    }
 
    armarx::Vector3BasePtr
    CylinderPrimitive::getCylinderAxisDirection(const Ice::Current& c) const
    {
        return getAttributeValue("axis")->getClass<armarx::Vector3Base>();
    }
 
    void
    CylinderPrimitive::setCylinderAxisDirection(const armarx::Vector3BasePtr& axis,
                                                const Ice::Current& c)
    {
        putAttribute("axis", axis);
    }
 
    float
    CylinderPrimitive::getCylinderRadius(const Ice::Current& c) const
    {
        return getAttributeValue("radius")->getFloat();
    }
 
    void
    CylinderPrimitive::setCylinderRadius(float radius, const Ice::Current& c)
    {
        putAttribute("radius", radius);
    }
 
    Ice::ObjectPtr
    memoryx::CylinderPrimitive::ice_clone() const
    {
        return this->clone();
    }
 
    CylinderPrimitivePtr
    memoryx::CylinderPrimitive::clone(const Ice::Current&) const
    {
        CylinderPrimitivePtr ret = new CylinderPrimitive(*this);
        return ret;
    }
 
    ConePrimitive::ConePrimitive(const std::string& name, const std::string& id) :
        EnvironmentalPrimitive(name, id, "cone")
    {
    }
 
    ConePrimitive::ConePrimitive(std::vector<float>& coeffs) :
        EnvironmentalPrimitive("", "", "cone")
    {
        setConeApex(new armarx::Vector3(coeffs[0], coeffs[1], coeffs[2]));
        setConeAxisDirection(new armarx::Vector3(coeffs[3], coeffs[4], coeffs[5]));
        setConeOpeningAngle(coeffs[6]);
    }
 
    armarx::Vector3BasePtr
    ConePrimitive::getConeApex(const Ice::Current& c) const
    {
        return getAttributeValue("apex")->getClass<armarx::Vector3Base>();
    }
 
    void
    ConePrimitive::setConeApex(const armarx::Vector3BasePtr& apex, const Ice::Current& c)
    {
        putAttribute("apex", apex);
    }
 
    armarx::Vector3BasePtr
    ConePrimitive::getConeAxisDirection(const Ice::Current& c) const
    {
        return getAttributeValue("axis")->getClass<armarx::Vector3Base>();
    }
 
    void
    ConePrimitive::setConeAxisDirection(const armarx::Vector3BasePtr& axis, const Ice::Current& c)
    {
        putAttribute("axis", axis);
    }
 
    float
    ConePrimitive::getConeOpeningAngle(const Ice::Current&) const
    {
        return getAttributeValue("angle")->getFloat();
    }
 
    void
    ConePrimitive::setConeOpeningAngle(float angle, const Ice::Current& c)
    {
        putAttribute("angle", angle);
    }
 
    Ice::ObjectPtr
    ConePrimitive::ice_clone() const
    {
        return this->clone();
    }
 
    ConePrimitivePtr
    ConePrimitive::clone(const Ice::Current& c) const
    {
        ConePrimitivePtr ret = new ConePrimitive(*this);
        return ret;
    }
 
    BoxPrimitive::BoxPrimitive(const std::string& name, const std::string& id) :
        EnvironmentalPrimitive(name, id, "box")
    {
    }
 
    memoryx::EntityRefList
    BoxPrimitive::getBoxSides(const Ice::Current& c) const
    {
        memoryx::EntityRefList result;
        EntityAttributeBasePtr attribute = getAttribute("sides");
        for (int i = 0; i < attribute->size(); i++)
        {
            armarx::VariantPtr value = armarx::VariantPtr::dynamicCast(attribute->getValueAt(i));
            result.push_back(value->get<EntityRefBase>());
        }
        return result;
    }
 
    void
    BoxPrimitive::setBoxSides(const memoryx::EntityRefList& sides, const Ice::Current& c)
    {
        EntityAttributePtr attribute = new EntityAttribute("sides");
        for (memoryx::EntityRefBasePtr ref : sides)
        {
            attribute->addValue(new armarx::Variant(ref));
        }
        putAttribute(attribute);
    }
 
    Ice::ObjectPtr
    BoxPrimitive::ice_clone() const
    {
        return this->clone();
    }
 
    BoxPrimitivePtr
    BoxPrimitive::clone(const Ice::Current& c) const
    {
        BoxPrimitivePtr ret = new BoxPrimitive(*this);
        return ret;
    }
 
    HigherSemanticStructure::HigherSemanticStructure(const std::string& name,
                                                     const std::string& id) :
        EnvironmentalPrimitive(name, id, "structure")
    {
    }
 
    memoryx::EntityRefList
    HigherSemanticStructure::getPrimitives(const Ice::Current& c) const
    {
        memoryx::EntityRefList result;
        EntityAttributeBasePtr attribute = getAttribute("primitives");
        for (int i = 0; i < attribute->size(); i++)
        {
            armarx::VariantPtr value = armarx::VariantPtr::dynamicCast(attribute->getValueAt(i));
            result.push_back(value->get<EntityRefBase>());
        }
        return result;
    }
 
    void
    HigherSemanticStructure::setPrimitives(const memoryx::EntityRefList& primitives,
                                           const Ice::Current& c)
    {
        EntityAttributePtr attribute = new EntityAttribute("primitives");
        for (memoryx::EntityRefBasePtr ref : primitives)
        {
            attribute->addValue(new armarx::Variant(ref));
        }
        putAttribute(attribute);
    }
 
    void
    HigherSemanticStructure::setLabels(const Ice::StringSeq& labels, const Ice::Current& c)
    {
        return putAttribute("labels", simox::alg::join(labels, ","));
    }
 
    Ice::StringSeq
    HigherSemanticStructure::getLabels(const Ice::Current&) const
    {
        std::string attrValue = getAttributeValue("labels")->getString();
        std::vector<std::string> labels = armarx::Split(attrValue, ",");
        return labels;
    }
 
    Ice::ObjectPtr
    HigherSemanticStructure::ice_clone() const
    {
        return this->clone();
    }
 
    HigherSemanticStructurePtr
    HigherSemanticStructure::clone(const Ice::Current& c) const
    {
        HigherSemanticStructurePtr ret = new HigherSemanticStructure(*this);
        return ret;
    }
} // namespace memoryx