From 25a94b161d2d596ee1d8517c1d2eaf8679256e9d Mon Sep 17 00:00:00 2001 From: Thomas Date: Tue, 22 Mar 2022 20:21:28 -0400 Subject: [PATCH] Current pain Works only in cartesian mode Something about trajectory time being zero again --- src/ar3_config/config/ar3_controllers.yaml | 9 +++ src/ar3_config/launch/dev.launch.py | 52 ++++++++--------- src/cobot_platform/cobot_platform/move.py | 22 +++++--- .../src/move_group_interface_node.cpp | 56 ++++++++++++++++++- 4 files changed, 103 insertions(+), 36 deletions(-) diff --git a/src/ar3_config/config/ar3_controllers.yaml b/src/ar3_config/config/ar3_controllers.yaml index ee3c1b9..bd6382b 100644 --- a/src/ar3_config/config/ar3_controllers.yaml +++ b/src/ar3_config/config/ar3_controllers.yaml @@ -1,7 +1,16 @@ moveit_controller_manager: moveit_simple_controller_manager/MoveItSimpleControllerManager + moveit_simple_controller_manager: + trajectory_execution: + allowed_execution_duration_scaling: 1.2 + allowed_goal_duration_margin: 0.5 + allowed_start_tolerance: 0.01 + + moveit_controller_manager: moveit_simple_controller_manager/MoveItSimpleControllerManager + controller_names: - joint_trajectory_controller + joint_trajectory_controller: action_ns: follow_joint_trajectory type: FollowJointTrajectory diff --git a/src/ar3_config/launch/dev.launch.py b/src/ar3_config/launch/dev.launch.py index dc1ee54..fc29818 100644 --- a/src/ar3_config/launch/dev.launch.py +++ b/src/ar3_config/launch/dev.launch.py @@ -34,7 +34,7 @@ def generate_launch_description(): executable='rviz2', output='log', arguments=['-d', join(launch_root, 'rviz', 'ar3.rviz')], - parameters=[robot_description, robot_description_semantic, robot_description_kinematics] + parameters=[robot_description, robot_description_semantic, robot_description_kinematics, get_yaml(join(config_root, 'ompl_planning.yaml'))] ), # Attatch robot to map @@ -50,21 +50,21 @@ def generate_launch_description(): package='move_group_interface', executable='move_group_interface_node', output='screen', - parameters=[robot_description, robot_description_semantic, robot_description_kinematics, - get_yaml(join(config_root, 'ompl_planning.yaml')), - get_yaml(join(config_root, 'trajectory.yaml')), - get_yaml(join(config_root, 'planning_scene.yaml')), - get_yaml(join(config_root, "ar3_controllers.yaml")), - { "default_planning_pipeline": "ompl" }, - { "planning_pipelines": {"pipeline_names":["ompl"], "ompl": {"planning_plugin": "ompl_interface/OMPLPlanner"} } }] + parameters=[robot_description, robot_description_semantic, robot_description_kinematics] + #get_yaml(join(config_root, 'ompl_planning.yaml')), + #get_yaml(join(config_root, 'trajectory.yaml')), + #get_yaml(join(config_root, 'planning_scene.yaml')), + #get_yaml(join(config_root, "ar3_controllers.yaml")), + #{ "default_planning_pipeline": "ompl" }, + #{ "planning_pipelines": {"pipeline_names":["ompl"], "ompl": {"planning_plugin": "ompl_interface/OMPLPlanner"} } }] ), # Our joystick node - Node( - package='joystickmove', - executable='move', - output='screen', - ), + #Node( + # package='joystickmove', + # executable='move', + # output='screen', + #), # Create joystick controller and ComposableNodes for servo_server ComposableNodeContainer( @@ -73,18 +73,18 @@ def generate_launch_description(): package="rclcpp_components", executable="component_container", composable_node_descriptions=[ - ComposableNode( - package="moveit_servo", - plugin="moveit_servo::ServoServer", - name="servo_server", - parameters=[ - {'moveit_servo': get_yaml(join(config_root, 'ar3_joy.yaml'))}, - robot_description, - robot_description_semantic, - robot_description_kinematics, - ], - extra_arguments=[{"use_intra_process_comms": True}], - ), + #ComposableNode( + # package="moveit_servo", + # plugin="moveit_servo::ServoServer", + # name="servo_server", + # parameters=[ + # {'moveit_servo': get_yaml(join(config_root, 'ar3_joy.yaml'))}, + # robot_description, + # robot_description_semantic, + # robot_description_kinematics, + # ], + # extra_arguments=[{"use_intra_process_comms": True}], + #), ComposableNode( package="joy", plugin="joy::Joy", @@ -134,7 +134,7 @@ def generate_launch_description(): get_yaml(join(config_root, 'trajectory.yaml')), get_yaml(join(config_root, 'planning_scene.yaml')), get_yaml(join(config_root, "ar3_controllers.yaml")), - { "default_planning_pipeline": "move_group" }, + { "default_planning_pipeline": "ompl" }, { "planning_pipelines": {"pipeline_names":["ompl"], "ompl": {"planning_plugin": "ompl_interface/OMPLPlanner"} } } ], ), diff --git a/src/cobot_platform/cobot_platform/move.py b/src/cobot_platform/cobot_platform/move.py index be1befb..cacc9ce 100644 --- a/src/cobot_platform/cobot_platform/move.py +++ b/src/cobot_platform/cobot_platform/move.py @@ -9,13 +9,14 @@ from math import pi from serial import Serial +J1_COUNTS_PER_RAD = 1 / (2 * pi) * 400 * 10 * 4 +J2_COUNTS_PER_RAD = 1 / (2 * pi) * 400 * 50 +J3_COUNTS_PER_RAD = 1 / (2 * pi) * 400 * 50 +J4_COUNTS_PER_RAD = 1 / (2 * pi) * 400 * (13 + 212 / 289) * 2.8 +J5_COUNTS_PER_RAD = 1 / (2 * pi) * 800 * 10 +J6_COUNTS_PER_RAD = 1 / (2 * pi) * 400 * (19 + 38 / 187) + class CobotMove(Node): - J1_COUNTS_PER_RAD = 1 / (2 * pi) * 400 * 10 * 4 - J2_COUNTS_PER_RAD = 1 / (2 * pi) * 400 * 50 - J3_COUNTS_PER_RAD = 1 / (2 * pi) * 400 * 50 - J4_COUNTS_PER_RAD = 1 / (2 * pi) * 400 * (13 + 212 / 289) * 2.8 - J5_COUNTS_PER_RAD = 1 / (2 * pi) * 800 * 10 - J6_COUNTS_PER_RAD = 1 / (2 * pi) * 400 * (19 + 38 / 187) def __init__(self): super().__init__('cobot_move') @@ -30,7 +31,7 @@ class CobotMove(Node): self._cur = [0.0] * 6 # create a timer to update all the joint angles at once - self._timer = self.create_timer(1 / 20, self._dosomething) + self._timer = self.create_timer(1 / 40, self._dosomething) # Set joint angles independantly base_topic = 'move/' @@ -91,6 +92,13 @@ class CobotMove(Node): e = float(blocks[4][1:]) / J5_COUNTS_PER_RAD f = float(blocks[5][1:]) / J6_COUNTS_PER_RAD + a = float(self._cur[0] / J1_COUNTS_PER_RAD) + b = float(self._cur[1] / J2_COUNTS_PER_RAD) + c = float(self._cur[2] / J3_COUNTS_PER_RAD) + d = float(self._cur[3] / J4_COUNTS_PER_RAD) + e = float(self._cur[4] / J5_COUNTS_PER_RAD) + f = float(self._cur[5] / J6_COUNTS_PER_RAD) + # Publish those to the update/jX subscribers self.s_j1.publish(Float32(data=a)) self.s_j2.publish(Float32(data=b)) diff --git a/src/move_group_interface/src/move_group_interface_node.cpp b/src/move_group_interface/src/move_group_interface_node.cpp index f555a88..9f70e36 100644 --- a/src/move_group_interface/src/move_group_interface_node.cpp +++ b/src/move_group_interface/src/move_group_interface_node.cpp @@ -15,6 +15,9 @@ #include #include #include +#include +#include +#include #include "geometry_msgs/msg/pose_stamped.hpp" @@ -23,18 +26,27 @@ const std::string PLANNING_GROUP = "ar3_arm"; class ScanNPlan : public rclcpp::Node { public: ScanNPlan() : Node("scan_n_plan") { + callback_group_ = create_callback_group(rclcpp::CallbackGroupType::Reentrant); + + rclcpp::SubscriptionOptions options; + options.callback_group = callback_group_; + // Create /move_pose subscriber pose_sub_ = this->create_subscription( "move_pose", rclcpp::QoS(1), - std::bind(&ScanNPlan::moveCallback, this, std::placeholders::_1) + std::bind(&ScanNPlan::moveCallbackHowItShouldWork, this, std::placeholders::_1), + options ); + /* relative_pose_sub_ = this->create_subscription( "move_relative_pose", rclcpp::QoS(1), - std::bind(&ScanNPlan::relativeMoveCallback, this, std::placeholders::_1) + std::bind(&ScanNPlan::relativeMoveCallback, this, std::placeholders::_1), + options ); + */ } void init() { @@ -46,6 +58,39 @@ private: void moveCallback(geometry_msgs::msg::PoseStamped::SharedPtr p) { RCLCPP_INFO(this->get_logger(), "GOT MSG"); + move_group_->setStartStateToCurrentState(); + move_group_->setPlanningTime(20); + move_group_->setNumPlanningAttempts(50); + move_group_->setMaxVelocityScalingFactor(1.0); + move_group_->setMaxAccelerationScalingFactor(1.0); + + std::vector waypoints; + waypoints.push_back(p->pose); + moveit_msgs::msg::RobotTrajectory trajectory; + double fraction = move_group_->computeCartesianPath(waypoints, 0.01, 0.0, trajectory, true); + if(fraction >= 1.0) { + RCLCPP_INFO(this->get_logger(), "Did the carts %f%%", fraction * 100.); + + robot_trajectory::RobotTrajectory rt(move_group_->getRobotModel(), move_group_->getName()); + rt.setRobotTrajectoryMsg(*move_group_->getCurrentState(10), trajectory); + trajectory_processing::IterativeParabolicTimeParameterization iptp; + bool success = + iptp.computeTimeStamps(rt, 0.1, 0.1); + RCLCPP_INFO(this->get_logger(), "Got stamps %s", success ? "YES" : "NO"); + + moveit::planning_interface::MoveGroupInterface::Plan plan; + rt.getRobotTrajectoryMsg(plan.trajectory_); + + move_group_->execute(plan); + } + + + //move_group_->move(); + } + void moveCallbackHowItShouldWork(geometry_msgs::msg::PoseStamped::SharedPtr p) { + RCLCPP_INFO(this->get_logger(), "GOT MSG"); + + move_group_->setStartStateToCurrentState(); move_group_->setPoseTarget(p->pose); moveit::planning_interface::MoveGroupInterface::Plan plan; @@ -78,6 +123,8 @@ private: move_group_->move(); } private: + rclcpp::CallbackGroup::SharedPtr callback_group_; + rclcpp::Subscription::SharedPtr pose_sub_; rclcpp::Subscription::SharedPtr relative_pose_sub_; @@ -91,8 +138,11 @@ int main(int argc, char* argv[]) { auto node = std::make_shared(); RCLCPP_INFO(node->get_logger(), "MoveGroup node started."); + rclcpp::executors::MultiThreadedExecutor executor; node->init(); - rclcpp::spin(node); + executor.add_node(node); + executor.spin(); + // rclcpp::spin(node); RCLCPP_INFO(node->get_logger(), "Terminating MoveGroup"); rclcpp::shutdown();