CollisionAvoidanceHelper.cpp

Go to the documentation of this file.

#include "CollisionAvoidanceHelper.h"
 
#include <SimoxUtility/algorithm/string/string_tools.h>
#include <VirtualRobot/ManipulationObject.h>
#include <VirtualRobot/XML/ObjectIO.h>
#include <VirtualRobot/XML/RobotIO.h>
 
#include <simox/control/environment/collision.h>
#include <simox/control/impl/simox/utils/conversion.h>
#include <simox/control/impl/simox/utils/primitive_conversion.h>
 
#include <ArmarXCore/core/ManagedIceObject.h>
#include <ArmarXCore/core/time/Clock.h>
#include <ArmarXCore/observers/variant/Variant.h>
 
#include <RobotAPI/interface/armem/server/ActionsInterface.h>
#include <RobotAPI/libraries/armem_objects/types.h>
 
namespace armarx::control::collision_avoidance::helpers
{
    const auto changeZtoYaxis = Eigen::AngleAxisd(-M_PI / 2., Eigen::Vector3d::UnitX());
 
    armarx::control::common::control_law::arondto::CollisionScene
    getCollisionSceneFromCollisionObjects(
        const std::map<std::string,
                       std::vector<simox::control::environment::TransformedCollisionObject>>&
            collisionObjects)
    {
        auto scene = armarx::control::common::control_law::arondto::CollisionScene();
        for (const auto& [fst, snd] : collisionObjects)
        {
            ARMARX_INFO << "adding " << snd.size() << " primitives for " << fst;
            for (const auto& [collisionObject, localTransformation] : snd)
            {
                auto globalTransform = collisionObject.getTransform();
                const auto& rot = globalTransform.getRotation();
                const auto& trans = globalTransform.getTranslation();
                Eigen::Matrix4d transformation = Eigen::Matrix4d::Identity();
                transformation.block<3, 3>(0, 0) = rot;
                transformation.block<3, 1>(0, 3) = trans;
 
                switch (collisionObject.getNodeType())
                {
                    case hpp::fcl::NODE_TYPE::GEOM_BOX:
                    {
                        auto box = armarx::control::common::control_law::arondto::Box();
                        // cast obj to Box
                        const hpp::fcl::CollisionGeometry* geomPtr =
                            collisionObject.collisionGeometry().get();
                        auto boxObj = dynamic_cast<const hpp::fcl::Box*>(geomPtr);
                        box.lengthX = boxObj->halfSide[0] * 2;
                        box.lengthZ = boxObj->halfSide[1] * 2;
                        box.lengthY = boxObj->halfSide[2] * 2;
 
                        box.transformation = transformation;
 
                        scene.boxes.push_back(box);
                        break;
                    }
 
                    case hpp::fcl::NODE_TYPE::GEOM_SPHERE:
                    {
                        auto sphere = armarx::control::common::control_law::arondto::Sphere();
                        // cast obj to Sphere
                        const hpp::fcl::CollisionGeometry* geomPtr =
                            collisionObject.collisionGeometry().get();
                        auto sphereObj = dynamic_cast<const hpp::fcl::Sphere*>(geomPtr);
                        sphere.radius = sphereObj->radius;
 
                        sphere.transformation = transformation;
 
                        scene.spheres.push_back(sphere);
                        break;
                    }
 
                    case hpp::fcl::NODE_TYPE::GEOM_CYLINDER:
                    {
                        auto cylinder = armarx::control::common::control_law::arondto::Cylinder();
                        // cast obj to Cylinder
                        const hpp::fcl::CollisionGeometry* geomPtr =
                            collisionObject.collisionGeometry().get();
                        auto cylinderObj = dynamic_cast<const hpp::fcl::Cylinder*>(geomPtr);
                        cylinder.radius = cylinderObj->radius;
                        cylinder.lengthY = cylinderObj->halfLength * 2;
 
                        transformation.block<3, 3>(0, 0) = rot * changeZtoYaxis;
                        cylinder.transformation = transformation;
 
                        scene.cylinders.push_back(cylinder);
                        break;
                    }
 
                    case hpp::fcl::NODE_TYPE::GEOM_CAPSULE:
                    {
                        auto capsule = armarx::control::common::control_law::arondto::Capsule();
                        // cast obj to Capsule
                        const hpp::fcl::CollisionGeometry* geomPtr =
                            collisionObject.collisionGeometry().get();
                        auto capsuleObj = dynamic_cast<const hpp::fcl::Capsule*>(geomPtr);
                        capsule.radius = capsuleObj->radius;
                        capsule.lengthY = capsuleObj->halfLength * 2;
 
                        transformation.block<3, 3>(0, 0) = rot * changeZtoYaxis;
                        capsule.transformation = transformation;
 
                        scene.capsules.push_back(capsule);
                        break;
                    }
 
                    case hpp::fcl::NODE_TYPE::GEOM_ELLIPSOID:
                    {
                        auto ellipsoid = armarx::control::common::control_law::arondto::Ellipsoid();
                        // cast obj to Ellipsoid
                        const hpp::fcl::CollisionGeometry* geomPtr =
                            collisionObject.collisionGeometry().get();
                        auto ellipsoidObj = dynamic_cast<const hpp::fcl::Ellipsoid*>(geomPtr);
                        ellipsoid.lengthX = static_cast<float>(ellipsoidObj->radii[0]) * 2.0F;
                        ellipsoid.lengthZ = static_cast<float>(ellipsoidObj->radii[1]) * 2.0F;
                        ellipsoid.lengthY = static_cast<float>(ellipsoidObj->radii[2]) * 2.0F;
 
                        ellipsoid.transformation = transformation;
 
                        scene.ellipsoids.push_back(ellipsoid);
                        break;
                    }
 
                    default:
                        // Handle unsupported shape or log warning
                        break;
                }
            }
        }
        return scene;
    }
 
    armarx::control::common::control_law::arondto::CollisionScene
    getCollisionSceneFromMemory(
        const std::vector<armarx::ObjectID>& objectIDs,
        armarx::armem::obj::clazz::ClassReader& objectClassReader,
        armarx::armem::obj::instance::Reader& objectInstanceReader,
        armarx::armem::articulated_object::ArticulatedObjectReader& articulatedObjectReader)
    {
        auto classes = objectClassReader.getObjectClasses(objectIDs);
        std::map<std::string, std::vector<simox::control::environment::TransformedCollisionObject>>
            collisionObjects;
        for (const auto& id : objectIDs)
        {
            auto classIt = classes.find(id.getClassID());
            if (classIt != classes.end())
            {
                const auto& classObj = classIt->second;
                if (classObj.simoxXmlPath.serialize().package.empty())
                {
                    ARMARX_INFO_S << "Skipping " << id << " due to empty simoxXmlPath";
                    continue;
                }
                std::filesystem::path simoxXmlPath = classObj.simoxXmlPath.toSystemPath();
 
                // TODO remove line but path in ObjectMemory is currently wrong
                simoxXmlPath = simox::alg::replace_all(
                    simoxXmlPath, "PriorKnowledgeData/PriorKnowledgeData", "PriorKnowledgeData");
                auto manipulationObject =
                    VirtualRobot::ObjectIO::loadManipulationObject(simoxXmlPath);
                ARMARX_CHECK_NOT_NULL(manipulationObject);
 
                simox::control::utils::primitive::Model primitiveModel;
 
                if (!id.instanceName().empty())
                { // can only read ArticulatedObjectMemory for object instances
 
                    auto articulatedObject =
                        articulatedObjectReader.queryState(id.str(), Clock::Now(), std::nullopt);
 
                    if (articulatedObject.has_value() and
                        not classObj.articulatedSimoxXmlPath.serialize().package.empty())
                    {
 
                        auto robot = VirtualRobot::RobotIO::loadRobot(
                            classObj.articulatedSimoxXmlPath.toSystemPath().string());
 
                        robot->setJointValues(articulatedObject.value().jointMap);
                        // iterate over all children, get their primitives and transform and combine in the primitiveModel
                        std::function<void(VirtualRobot::SceneObjectPtr)> processAll =
                            [&](const VirtualRobot::SceneObjectPtr& sceneObject)
                        {
                            if (!sceneObject)
                            {
                                return;
                            }
                            for (const auto& primitive :
                                 sceneObject->getPrimitiveApproximation().getModels(
                                     std::vector<std::string>(), true))
                            {
                                // combine global transform of parent with local transform of primitive
                                primitive->transform =
                                    sceneObject->getGlobalPose() * primitive->transform;
                                simox::control::simox::utils::convert(*primitive, primitiveModel);
                            }
                            for (const auto& child : sceneObject->getChildren())
                            {
                                processAll(child);
                            }
                        };
                        for (const auto& child : robot->getChildren())
                        {
                            processAll(child);
                        }
                    }
                }
 
                // get not articulated primitives from the manipulation object
                for (const auto& primitive :
                     manipulationObject->getPrimitiveApproximation().getModels(
                         std::vector<std::string>(), true))
                {
                    simox::control::simox::utils::convert(*primitive, primitiveModel);
                }
                if (id.instanceName() == "")
                {
                    objectInstanceReader.queryLatestObjectInstances(id);
 
                    for (const auto& [instance, object] :
                         objectInstanceReader.queryLatestObjectInstances(id))
                    {
                        Eigen::Isometry3d globalPose =
                            simox::control::simox::utils::from_simox(object.objectPoseGlobal);
                        collisionObjects[instance] =
                            simox::control::environment::createCollisionObjects(primitiveModel,
                                                                                globalPose);
                    }
                }
                else
                {
                    auto pose = objectInstanceReader.queryLatestObjectInstance(id);
 
                    if (pose.has_value())
                    {
                        Eigen::Isometry3d globalPose =
                            simox::control::simox::utils::from_simox(pose.value().objectPoseGlobal);
                        collisionObjects[id.str()] =
                            simox::control::environment::createCollisionObjects(primitiveModel,
                                                                                globalPose);
                    }
                }
            }
        }
        return getCollisionSceneFromCollisionObjects(collisionObjects);
    }
 
    armarx::control::common::control_law::arondto::CollisionScene
    getCollisionSceneFromMemory(
        const std::vector<std::string>& objectIDs,
        armarx::armem::obj::clazz::ClassReader& objectClassReader,
        armarx::armem::obj::instance::Reader& objectInstanceReader,
        armarx::armem::articulated_object::ArticulatedObjectReader& articulatedObjectReader)
    {
        std::vector<armarx::ObjectID> convertedObjectIDs;
        for (const auto& id : objectIDs)
        {
            convertedObjectIDs.emplace_back(armarx::ObjectID(id));
        }
        return getCollisionSceneFromMemory(
            convertedObjectIDs, objectClassReader, objectInstanceReader, articulatedObjectReader);
    }
 
} // namespace armarx::control::collision_avoidance::helpers