diff --git a/src/btree/trees/actorTreeColab.xml b/src/btree/trees/actorTreeColab.xml index 684f350..687354e 100644 --- a/src/btree/trees/actorTreeColab.xml +++ b/src/btree/trees/actorTreeColab.xml @@ -19,6 +19,10 @@ + + + + diff --git a/src/btree/trees/actorTreeCoop.xml b/src/btree/trees/actorTreeCoop.xml index e7ff2b2..4e2390a 100644 --- a/src/btree/trees/actorTreeCoop.xml +++ b/src/btree/trees/actorTreeCoop.xml @@ -1,26 +1,51 @@ - - - - - + + - - - - - - + + - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/btree/trees/robotTreeColab.xml b/src/btree/trees/robotTreeColab.xml index ef0b86c..7a766f2 100644 --- a/src/btree/trees/robotTreeColab.xml +++ b/src/btree/trees/robotTreeColab.xml @@ -11,16 +11,16 @@ - - - - - - - - - - + + + + + + + + + + diff --git a/src/controller_test/src/test.cpp b/src/controller_test/src/test.cpp index e169cea..d40ebbd 100644 --- a/src/controller_test/src/test.cpp +++ b/src/controller_test/src/test.cpp @@ -12,192 +12,178 @@ std::shared_ptr node; -void common_goal_response( - const rclcpp_action::ClientGoalHandle - ::SharedPtr& future) -{ - RCLCPP_DEBUG( - node->get_logger(), "common_goal_response time: %f", - rclcpp::Clock().now().seconds()); - auto goal_handle = future.get(); - if (!goal_handle) { - printf("Goal rejected\n"); - } else { - printf("Goal accepted\n"); - } +void common_goal_response(const rclcpp_action::ClientGoalHandle::SharedPtr& future){ + RCLCPP_DEBUG( + node->get_logger(), "common_goal_response time: %f", + rclcpp::Clock().now().seconds()); + auto goal_handle = future.get(); + if (!goal_handle) { + printf("Goal rejected\n"); + } else { + printf("Goal accepted\n"); + } } -void common_result_response( - const rclcpp_action::ClientGoalHandle - ::WrappedResult & result) -{ - printf("common_result_response time: %f\n", rclcpp::Clock().now().seconds()); - switch (result.code) { - case rclcpp_action::ResultCode::SUCCEEDED: - printf("SUCCEEDED result code\n"); - break; - case rclcpp_action::ResultCode::ABORTED: - printf("Goal was aborted\n"); - return; - case rclcpp_action::ResultCode::CANCELED: - printf("Goal was canceled\n"); - return; - default: - printf("Unknown result code\n"); - return; - } +void common_result_response(const rclcpp_action::ClientGoalHandle::WrappedResult & result){ + printf("common_result_response time: %f\n", rclcpp::Clock().now().seconds()); + switch (result.code) { + case rclcpp_action::ResultCode::SUCCEEDED: + printf("SUCCEEDED result code\n"); + break; + case rclcpp_action::ResultCode::ABORTED: + printf("Goal was aborted\n"); + return; + case rclcpp_action::ResultCode::CANCELED: + printf("Goal was canceled\n"); + return; + default: + printf("Unknown result code\n"); + return; + } } -void common_feedback( - const rclcpp_action::ClientGoalHandle::SharedPtr&, - const std::shared_ptr& feedback) -{ - std::cout << "feedback->desired.positions :"; - for (auto & x : feedback->desired.positions) { - std::cout << x << "\t"; - } - std::cout << std::endl; - std::cout << "feedback->desired.velocities :"; - for (auto & x : feedback->desired.velocities) { - std::cout << x << "\t"; - } - std::cout << std::endl; +void common_feedback(const rclcpp_action::ClientGoalHandle::SharedPtr&, + const std::shared_ptr& feedback){ + std::cout << "feedback->desired.positions :"; + for (auto & x : feedback->desired.positions) { + std::cout << x << "\t"; + } + std::cout << std::endl; + std::cout << "feedback->desired.velocities :"; + for (auto & x : feedback->desired.velocities) { + std::cout << x << "\t"; + } + std::cout << std::endl; } -int main(int argc, char * argv[]) -{ - rclcpp::init(argc, argv); +int main(int argc, char * argv[]){ + rclcpp::init(argc, argv); - node = std::make_shared("trajectory_test_node"); + node = std::make_shared("trajectory_test_node"); - std::cout << "node created" << std::endl; + std::cout << "node created" << std::endl; - rclcpp_action::Client::SharedPtr action_client; - action_client = rclcpp_action::create_client( - node->get_node_base_interface(), - node->get_node_graph_interface(), - node->get_node_logging_interface(), - node->get_node_waitables_interface(), - "/arm_controller/follow_joint_trajectory"); + rclcpp_action::Client::SharedPtr action_client; + action_client = rclcpp_action::create_client( + node->get_node_base_interface(), + node->get_node_graph_interface(), + node->get_node_logging_interface(), + node->get_node_waitables_interface(), + "/arm_controller/follow_joint_trajectory" + ); - bool response = - action_client->wait_for_action_server(std::chrono::seconds(10)); - if (!response) { - throw std::runtime_error("could not get action server"); - } - std::cout << "Created action server" << std::endl; + bool response = action_client->wait_for_action_server(std::chrono::seconds(10)); + if (!response) { + throw std::runtime_error("could not get action server"); + } + std::cout << "Created action server" << std::endl; - std::vector joint_names = { - "base_rot", - "base_link1_joint", - "link1_link2_joint", - "link2_rot", - "link2_link3_joint", - "link3_rot" + std::vector joint_names = { + "base_rot", + "base_link1_joint", + "link1_link2_joint", + "link2_rot", + "link2_link3_joint", + "link3_rot" }; - std::vector points; - trajectory_msgs::msg::JointTrajectoryPoint point; - point.time_from_start = rclcpp::Duration::from_seconds(0.0); // start asap - point.positions.resize(joint_names.size()); + std::vector points; + trajectory_msgs::msg::JointTrajectoryPoint point; + point.time_from_start = rclcpp::Duration::from_seconds(0.0); // start asap + point.positions.resize(joint_names.size()); - point.positions[0] = 0.0; - point.positions[1] = 0.0; - point.positions[2] = 0.0; - point.positions[3] = 0.0; - point.positions[4] = 0.0; - point.positions[5] = 0.0; + point.positions[0] = 0.0; + point.positions[1] = 0.0; + point.positions[2] = 0.0; + point.positions[3] = 0.0; + point.positions[4] = 0.0; + point.positions[5] = 0.0; - trajectory_msgs::msg::JointTrajectoryPoint point2; - point2.time_from_start = rclcpp::Duration::from_seconds(1.0); - point2.positions.resize(joint_names.size()); + trajectory_msgs::msg::JointTrajectoryPoint point2; + point2.time_from_start = rclcpp::Duration::from_seconds(1.0); + point2.positions.resize(joint_names.size()); - point2.positions[0] = -1.0; - point2.positions[1] = 0.0; - point2.positions[2] = 0.0; - point2.positions[3] = 0.0; - point2.positions[4] = 0.0; - point2.positions[5] = 0.0; + point2.positions[0] = -1.0; + point2.positions[1] = 0.0; + point2.positions[2] = 0.0; + point2.positions[3] = 0.0; + point2.positions[4] = 0.0; + point2.positions[5] = 0.0; - trajectory_msgs::msg::JointTrajectoryPoint point3; - point3.time_from_start = rclcpp::Duration::from_seconds(2.0); - point3.positions.resize(joint_names.size()); + trajectory_msgs::msg::JointTrajectoryPoint point3; + point3.time_from_start = rclcpp::Duration::from_seconds(2.0); + point3.positions.resize(joint_names.size()); - point3.positions[0] = 1.0; - point3.positions[1] = 0.0; - point3.positions[2] = 0.0; - point3.positions[3] = 0.0; - point3.positions[4] = 0.0; - point3.positions[5] = 0.0; + point3.positions[0] = 1.0; + point3.positions[1] = 0.0; + point3.positions[2] = 0.0; + point3.positions[3] = 0.0; + point3.positions[4] = 0.0; + point3.positions[5] = 0.0; - trajectory_msgs::msg::JointTrajectoryPoint point4; - point4.time_from_start = rclcpp::Duration::from_seconds(3.0); - point4.positions.resize(joint_names.size()); + trajectory_msgs::msg::JointTrajectoryPoint point4; + point4.time_from_start = rclcpp::Duration::from_seconds(3.0); + point4.positions.resize(joint_names.size()); - point4.positions[0] = 0.0; - point4.positions[1] = 0.0; - point4.positions[2] = M_PI/2; - point4.positions[3] = 0.0; - point4.positions[4] = 0.0; - point4.positions[5] = 0.0; + point4.positions[0] = 0.0; + point4.positions[1] = 0.0; + point4.positions[2] = M_PI/2; + point4.positions[3] = 0.0; + point4.positions[4] = 0.0; + point4.positions[5] = 0.0; - points.push_back(point); - points.push_back(point2); - points.push_back(point3); - points.push_back(point4); + points.push_back(point); + points.push_back(point2); + points.push_back(point3); + points.push_back(point4); - rclcpp_action::Client::SendGoalOptions opt; - opt.goal_response_callback = [](std::shared_ptr> goal){ - if(goal->get_status()==rclcpp_action::GoalStatus::STATUS_ACCEPTED){ - RCLCPP_DEBUG( - node->get_logger(), "common_goal_response accepted: %f", - rclcpp::Clock().now().seconds()); - }else{ - RCLCPP_DEBUG( - node->get_logger(), "common_goal_response rejected: %f", - rclcpp::Clock().now().seconds()); + rclcpp_action::Client::SendGoalOptions opt; + opt.goal_response_callback = [](std::shared_ptr> goal){ + if(goal->get_status()==rclcpp_action::GoalStatus::STATUS_ACCEPTED){ + RCLCPP_DEBUG( + node->get_logger(), "common_goal_response accepted: %f", + rclcpp::Clock().now().seconds()); + }else{ + RCLCPP_DEBUG( + node->get_logger(), "common_goal_response rejected: %f", + rclcpp::Clock().now().seconds()); + } + }; + //opt.goal_response_callback = [](auto && PH1) { return common_goal_response(std::forward(PH1)); }; + opt.result_callback = [](auto && PH1) { return common_result_response(std::forward(PH1)); }; + opt.feedback_callback = [](auto && PH1, auto && PH2) { return common_feedback(std::forward(PH1), std::forward(PH2)); }; + + control_msgs::action::FollowJointTrajectory_Goal goal_msg; + goal_msg.goal_time_tolerance = rclcpp::Duration::from_seconds(1.0); + goal_msg.trajectory.joint_names = joint_names; + goal_msg.trajectory.points = points; + + auto goal_handle_future = action_client->async_send_goal(goal_msg, opt); + + if (rclcpp::spin_until_future_complete(node, goal_handle_future) != rclcpp::FutureReturnCode::SUCCESS){ + RCLCPP_ERROR(node->get_logger(), "send goal call failed :("); + return 1; } - }; - //opt.goal_response_callback = [](auto && PH1) { return common_goal_response(std::forward(PH1)); }; - opt.result_callback = [](auto && PH1) { return common_result_response(std::forward(PH1)); }; - opt.feedback_callback = [](auto && PH1, auto && PH2) { return common_feedback(std::forward(PH1), std::forward(PH2)); }; + RCLCPP_ERROR(node->get_logger(), "send goal call ok :)"); - control_msgs::action::FollowJointTrajectory_Goal goal_msg; - goal_msg.goal_time_tolerance = rclcpp::Duration::from_seconds(1.0); - goal_msg.trajectory.joint_names = joint_names; - goal_msg.trajectory.points = points; + const rclcpp_action::ClientGoalHandle::SharedPtr& goal_handle = goal_handle_future.get(); + if (!goal_handle) { + RCLCPP_ERROR(node->get_logger(), "Goal was rejected by server"); + return 1; + } + RCLCPP_ERROR(node->get_logger(), "Goal was accepted by server"); - auto goal_handle_future = action_client->async_send_goal(goal_msg, opt); + // Wait for the server to be done with the goal + auto result_future = action_client->async_get_result(goal_handle); + RCLCPP_INFO(node->get_logger(), "Waiting for result"); + if (rclcpp::spin_until_future_complete(node, result_future) != rclcpp::FutureReturnCode::SUCCESS){ + RCLCPP_ERROR(node->get_logger(), "get result call failed :("); + return 1; + } - if (rclcpp::spin_until_future_complete(node, goal_handle_future) != - rclcpp::FutureReturnCode::SUCCESS) - { - RCLCPP_ERROR(node->get_logger(), "send goal call failed :("); - return 1; - } - RCLCPP_ERROR(node->get_logger(), "send goal call ok :)"); - - const rclcpp_action::ClientGoalHandle::SharedPtr& - goal_handle = goal_handle_future.get(); - if (!goal_handle) { - RCLCPP_ERROR(node->get_logger(), "Goal was rejected by server"); - return 1; - } - RCLCPP_ERROR(node->get_logger(), "Goal was accepted by server"); - - // Wait for the server to be done with the goal - auto result_future = action_client->async_get_result(goal_handle); - RCLCPP_INFO(node->get_logger(), "Waiting for result"); - if (rclcpp::spin_until_future_complete(node, result_future) != - rclcpp::FutureReturnCode::SUCCESS) - { - RCLCPP_ERROR(node->get_logger(), "get result call failed :("); - return 1; - } - - std::cout << "async_send_goal" << std::endl; + std::cout << "async_send_goal" << std::endl; - rclcpp::shutdown(); + rclcpp::shutdown(); - return 0; + return 0; } \ No newline at end of file diff --git a/src/ign_world/world/world.sdf b/src/ign_world/world/world.sdf index 867372c..eaa9a8c 100644 --- a/src/ign_world/world/world.sdf +++ b/src/ign_world/world/world.sdf @@ -20,7 +20,7 @@ 0.01 0.001 - -0.5 0.1 -0.9 + -0.1 0.1 -0.9 diff --git a/src/iisy_config/config/joint_limits.yaml b/src/iisy_config/config/joint_limits.yaml index 64ec7c1..f22eec7 100644 --- a/src/iisy_config/config/joint_limits.yaml +++ b/src/iisy_config/config/joint_limits.yaml @@ -2,39 +2,39 @@ # For beginners, we downscale velocity and acceleration limits. # You can always specify higher scaling factors (<= 1.0) in your motion requests. # Increase the values below to 1.0 to always move at maximum speed. -default_velocity_scaling_factor: 0.1 -default_acceleration_scaling_factor: 0.1 +default_velocity_scaling_factor: 1 +default_acceleration_scaling_factor: 1 # Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration] # Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits] joint_limits: base_link1_joint: has_velocity_limits: true - max_velocity: 1.7453292519943 + max_velocity: 3.49065850399 has_acceleration_limits: false max_acceleration: 0 base_rot: has_velocity_limits: true - max_velocity: 1.7453292519943 + max_velocity: 3.49065850399 has_acceleration_limits: false max_acceleration: 0 link1_link2_joint: has_velocity_limits: true - max_velocity: 1.7453292519943 + max_velocity: 3.49065850399 has_acceleration_limits: false max_acceleration: 0 link2_link3_joint: has_velocity_limits: true - max_velocity: 1.7453292519943 + max_velocity: 5.23598775598 has_acceleration_limits: false max_acceleration: 0 link2_rot: has_velocity_limits: true - max_velocity: 1.7453292519943 + max_velocity: 5.23598775598 has_acceleration_limits: false max_acceleration: 0 link3_rot: has_velocity_limits: true - max_velocity: 1.7453292519943 + max_velocity: 6.98131700798 has_acceleration_limits: false max_acceleration: 0 \ No newline at end of file diff --git a/src/iisy_config/config/ros2_controllers.yaml b/src/iisy_config/config/ros2_controllers.yaml index 1e794bf..65e5032 100644 --- a/src/iisy_config/config/ros2_controllers.yaml +++ b/src/iisy_config/config/ros2_controllers.yaml @@ -24,7 +24,7 @@ arm_controller: state_interfaces: - position - velocity - open_loop_control: true + constraints: stopped_velocity_tolerance: 0.01 goal_time: 0.0 \ No newline at end of file