diff --git a/cmake/vrep_interface_tests/free_flying_robot/CMakeLists.txt b/cmake/vrep_interface_tests/free_flying_robot/CMakeLists.txt new file mode 100644 index 0000000..e8f6194 --- /dev/null +++ b/cmake/vrep_interface_tests/free_flying_robot/CMakeLists.txt @@ -0,0 +1,16 @@ +cmake_minimum_required(VERSION 3.5) + +include(${CMAKE_CURRENT_SOURCE_DIR}/../../dqrobotics_dependencies.cmake) +find_package (Threads REQUIRED) + +project(free_flying_robot LANGUAGES CXX) + + +add_executable(free_flying_robot free_flying_robot.cpp) + +TARGET_LINK_LIBRARIES(free_flying_robot + pthread + qpOASES + dqrobotics + dqrobotics-interface-vrep +) diff --git a/cmake/vrep_interface_tests/free_flying_robot/README.md b/cmake/vrep_interface_tests/free_flying_robot/README.md new file mode 100644 index 0000000..367c3cd --- /dev/null +++ b/cmake/vrep_interface_tests/free_flying_robot/README.md @@ -0,0 +1 @@ +drawing diff --git a/cmake/vrep_interface_tests/free_flying_robot/free_flying_robot.cpp b/cmake/vrep_interface_tests/free_flying_robot/free_flying_robot.cpp new file mode 100644 index 0000000..9f095e0 --- /dev/null +++ b/cmake/vrep_interface_tests/free_flying_robot/free_flying_robot.cpp @@ -0,0 +1,155 @@ +/** +(C) Copyright 2023 DQ Robotics Developers +This file is part of DQ Robotics. + DQ Robotics is free software: you can redistribute it and/or modify + it under the terms of the GNU Lesser General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + DQ Robotics 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 Lesser General Public License for more details. + You should have received a copy of the GNU Lesser General Public License + along with DQ Robotics. If not, see . + +Contributors: +- Juan Jose Quiroz Omana (juanjqo@g.ecc.u-tokyo.ac.jp) + + +Prerequisites: +- dqrobotics +- dqrobotics-interface-vrep +- dqrobotics-interface-qpoases + +Instructions: +1) Open the CoppeliaSim scene free_flying_robot.ttt +2) Compile, run and enjoy! +*/ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace DQ_robotics; +using namespace Eigen; + + +/** + * @brief This function computes the VFI constraints required to prevent collisions between the robot and + * the environment. + * @param vi The client object. + * @param robot The free-flying robot object + * @param obstacle_names The name of the spheric obstacles in the scene + * @param safe_distances The vector containing the safe distances. + * @param robot_pose The unit dual quaternion that represents + * the free-flying robot configuration.. + * @param robot_jacobian The Jacobian of the free-flying robot. + * @return The desired constraints. + */ +std::tuple compute_constraints(const std::shared_ptr& vi, + const DQ_FreeFlyingRobot& robot, + const std::vector& obstacle_names, + const std::vector& safe_distances, + const DQ& robot_pose, const MatrixXd& robot_jacobian); + +int main() +{ + auto vi = std::make_shared(); + try + { + vi->connect(19997,100,10); + vi->set_synchronous(true); + std::cout << "Starting the CoppeliaSim simulation..." << std::endl; + vi->start_simulation(); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + + auto robot = DQ_FreeFlyingRobot(); + double T = 0.01; + double lambda = 1; + DQ xd = vi->get_object_pose("xdesired"); + DQ x = vi->get_object_pose("coffee_drone"); + DQ xerror = x.conj()*xd; + + MatrixXd IM = MatrixXd::Zero(8,6); + for (auto i=0;i<3;i++){ + IM(i+1,i) = 1.0; + IM(i+5,i+3) = 1.0; + } + + auto solver = std::make_shared(); + + std::vector obstacle_names = {"obstacle1", "obstacle2", "obstacle3", + "obstacle4", "obstacle5", "obstacle6", + "obstacle7", "obstacle8", "obstacle9", + "obstacle10"}; + std::vector safe_distances(obstacle_names.size(), 0.25); + + MatrixXd A; + VectorXd b; + MatrixXd Aeq; + VectorXd beq; + + while (log(xerror).vec6().norm() > 0.0001) + { + + MatrixXd J = robot.pose_jacobian(x)*IM; + MatrixXd M = pinv(Q8(xerror))*haminus8(xd)*C8()*J; + MatrixXd H = M.transpose()*M; + VectorXd yerror = vec6(log(xerror)); + VectorXd f = 2*lambda*yerror.transpose()*M; + + std::tie(A, b) = compute_constraints(vi, robot, obstacle_names, safe_distances,x, J); + auto uvec = solver->solve_quadratic_program(H, f, A, b, Aeq, beq); + + x = exp(T/2*DQ(uvec))*x; + xerror = x.conj()*xd; + std::cout<<"error: "<set_object_pose("coffee_drone", x); + vi->trigger_next_simulation_step(); + } + + } + catch(std::exception& e) + { + std::cout << e.what() << std::endl; + vi->stop_simulation(); + vi->disconnect(); + return 0; + } + vi->wait_for_simulation_step_to_end(); + vi->stop_simulation(); + vi->disconnect(); + return 0; +} + + +std::tuple compute_constraints(const std::shared_ptr& vi, + const DQ_FreeFlyingRobot& robot, + const std::vector& obstacle_names, + const std::vector& safe_distances, + const DQ& robot_pose, const MatrixXd& robot_jacobian) +{ + DQ t = robot_pose.translation(); + MatrixXd J_t = robot.translation_jacobian(robot_jacobian, robot_pose); + int n = obstacle_names.size(); + MatrixXd A = MatrixXd::Zero(n+1, 6); + VectorXd b = VectorXd::Zero(n+1); + + + for (auto i=0;iget_object_pose(obstacle_names[i]).translation(); + A.row(i) = -1*robot.point_to_point_distance_jacobian(J_t, t, obstacle_translation); + b(i) = DQ_Geometry::point_to_point_squared_distance(t, obstacle_translation) + - std::pow(safe_distances[i],2); + } + A.row(n) = -1*robot.point_to_plane_distance_jacobian(J_t, t, k_); + b(n) = DQ_Geometry::point_to_plane_distance(t,k_); + return {A, b}; +} diff --git a/cmake/vrep_interface_tests/free_flying_robot/free_flying_robot.ttt b/cmake/vrep_interface_tests/free_flying_robot/free_flying_robot.ttt new file mode 100644 index 0000000..99107ea Binary files /dev/null and b/cmake/vrep_interface_tests/free_flying_robot/free_flying_robot.ttt differ