CollisionAvoidance.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    control::COMPONENT_NAME_SNAKE::skills
 * @author     Meixner ( andre dot meixner at kit dot edu )
 * @date       2025
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "CollisionAvoidance.h"
 
#include <Eigen/Geometry>
 
#include <SimoxUtility/algorithm/string/string_tools.h>
#include <VirtualRobot/ManipulationObject.h>
#include <VirtualRobot/VirtualRobot.h>
#include <VirtualRobot/XML/ObjectIO.h>
#include <VirtualRobot/XML/RobotIO.h>
 
#include <simox/control/environment/CollisionRobot.h>
#include <simox/control/environment/collision.h>
#include <simox/control/impl/simox/robot/Robot.h>
#include <simox/control/impl/simox/utils/conversion.h>
#include <simox/control/impl/simox/utils/primitive_conversion.h>
 
#include <ArmarXCore/core/exceptions/LocalException.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/time/Duration.h>
#include <ArmarXCore/util/CPPUtility/trace.h>
 
#include <RobotAPI/components/ArViz/Client/Elements.h>
#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointControllerRegistry.h>
 
#include <armarx/control/collision_avoidance/helpers/CollisionAvoidanceHelper.h>
#include <armarx/control/njoint_controller/controller_descriptions.h>
 
//#include <VisionX/libraries/armem_human/client/HumanPoseReader.h>
//#include <VisionX/libraries/human/pose/model/k4a_bt_body_32.h>
 
#include <armarx/control/skills/aron/ZeroTorque.aron.generated.h>
 
#include <armadillo_bits/def_atlas.hpp>
 
namespace armarx::control::collision_avoidance::skills
{
 
    ::armarx::skills::SkillDescription
    CollisionAvoidance::GetSkillDescription()
    {
        ParamType defaultParams;
        defaultParams.objectIDs = std::vector<std::string>{
            "Kitchen/mobile-fridge/0", "Kitchen/mobile-dishwasher/0", "Kitchen/mobile-kitchen-counter/0"};
 
        const std::string skillName = armarx::control::collision_avoidance::constants::SKILL_NAME;
        const armarx::skills::SkillID skillId = armarx::skills::SkillID{.skillName = skillName};
 
        return ::armarx::skills::SkillDescription{
            .skillId = skillId,
            .description = "TODO: Description of skill CollisionAvoidance.",
            .rootProfileDefaults = defaultParams.toAron(),
            .timeout = ::armarx::Duration::MilliSeconds(120000),
            .parametersType = ParamType::ToAronType()};
    }
 
    CollisionAvoidance::CollisionAvoidance(const Remote& r) :
        //Base(GetSkillDescription()), remote(r), properties(p), srv_(srv)
        Base(GetSkillDescription()),
        remote(r)
    {
    }
 
    struct CollisionWrapper
    {
        std::unique_ptr<simox::control::simox::robot::Robot> robot;
        std::unique_ptr<simox::control::environment::CollisionRobot<hpp::fcl::OBBRSS>>
            collisionRobot;
    };
 
    CollisionAvoidance::MainResult
    CollisionAvoidance::main(const SpecializedMainInput& in)
    {
        if (getParameters().flagDelete)
        {
            remote.collisionAvoidanceComponent->get()->deleteCollisionObjects();
            return MakeSucceededResult();
        }
 
        remote.collisionAvoidanceComponent->get()->sendCollisionObjectsToController(
            getParameters().objectIDs);
        if (true)
        {
            return MakeSucceededResult();
        }
        return MakeFailedResult();
    }
 
    CollisionAvoidance::ExitResult
    CollisionAvoidance::exit(const SpecializedExitInput& in)
    {
        return {::armarx::skills::TerminatedSkillStatus::Succeeded};
    }
 
 
} // namespace armarx::control::collision_avoidance::skills