Current pain

Works only in cartesian mode
Something about trajectory time being zero again
This commit is contained in:
Thomas Muller 2022-03-22 20:21:28 -04:00
parent 293f603a8b
commit 25a94b161d
4 changed files with 103 additions and 36 deletions

View file

@ -1,7 +1,16 @@
moveit_controller_manager: moveit_simple_controller_manager/MoveItSimpleControllerManager moveit_controller_manager: moveit_simple_controller_manager/MoveItSimpleControllerManager
moveit_simple_controller_manager: 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: controller_names:
- joint_trajectory_controller - joint_trajectory_controller
joint_trajectory_controller: joint_trajectory_controller:
action_ns: follow_joint_trajectory action_ns: follow_joint_trajectory
type: FollowJointTrajectory type: FollowJointTrajectory

View file

@ -34,7 +34,7 @@ def generate_launch_description():
executable='rviz2', executable='rviz2',
output='log', output='log',
arguments=['-d', join(launch_root, 'rviz', 'ar3.rviz')], 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 # Attatch robot to map
@ -50,21 +50,21 @@ def generate_launch_description():
package='move_group_interface', package='move_group_interface',
executable='move_group_interface_node', executable='move_group_interface_node',
output='screen', output='screen',
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')), #get_yaml(join(config_root, 'ompl_planning.yaml')),
get_yaml(join(config_root, 'trajectory.yaml')), #get_yaml(join(config_root, 'trajectory.yaml')),
get_yaml(join(config_root, 'planning_scene.yaml')), #get_yaml(join(config_root, 'planning_scene.yaml')),
get_yaml(join(config_root, "ar3_controllers.yaml")), #get_yaml(join(config_root, "ar3_controllers.yaml")),
{ "default_planning_pipeline": "ompl" }, #{ "default_planning_pipeline": "ompl" },
{ "planning_pipelines": {"pipeline_names":["ompl"], "ompl": {"planning_plugin": "ompl_interface/OMPLPlanner"} } }] #{ "planning_pipelines": {"pipeline_names":["ompl"], "ompl": {"planning_plugin": "ompl_interface/OMPLPlanner"} } }]
), ),
# Our joystick node # Our joystick node
Node( #Node(
package='joystickmove', # package='joystickmove',
executable='move', # executable='move',
output='screen', # output='screen',
), #),
# Create joystick controller and ComposableNodes for servo_server # Create joystick controller and ComposableNodes for servo_server
ComposableNodeContainer( ComposableNodeContainer(
@ -73,18 +73,18 @@ def generate_launch_description():
package="rclcpp_components", package="rclcpp_components",
executable="component_container", executable="component_container",
composable_node_descriptions=[ composable_node_descriptions=[
ComposableNode( #ComposableNode(
package="moveit_servo", # package="moveit_servo",
plugin="moveit_servo::ServoServer", # plugin="moveit_servo::ServoServer",
name="servo_server", # name="servo_server",
parameters=[ # parameters=[
{'moveit_servo': get_yaml(join(config_root, 'ar3_joy.yaml'))}, # {'moveit_servo': get_yaml(join(config_root, 'ar3_joy.yaml'))},
robot_description, # robot_description,
robot_description_semantic, # robot_description_semantic,
robot_description_kinematics, # robot_description_kinematics,
], # ],
extra_arguments=[{"use_intra_process_comms": True}], # extra_arguments=[{"use_intra_process_comms": True}],
), #),
ComposableNode( ComposableNode(
package="joy", package="joy",
plugin="joy::Joy", plugin="joy::Joy",
@ -134,7 +134,7 @@ def generate_launch_description():
get_yaml(join(config_root, 'trajectory.yaml')), get_yaml(join(config_root, 'trajectory.yaml')),
get_yaml(join(config_root, 'planning_scene.yaml')), get_yaml(join(config_root, 'planning_scene.yaml')),
get_yaml(join(config_root, "ar3_controllers.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"} } } { "planning_pipelines": {"pipeline_names":["ompl"], "ompl": {"planning_plugin": "ompl_interface/OMPLPlanner"} } }
], ],
), ),

View file

@ -9,13 +9,14 @@ from math import pi
from serial import Serial 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): 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): def __init__(self):
super().__init__('cobot_move') super().__init__('cobot_move')
@ -30,7 +31,7 @@ class CobotMove(Node):
self._cur = [0.0] * 6 self._cur = [0.0] * 6
# create a timer to update all the joint angles at once # 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 # Set joint angles independantly
base_topic = 'move/' base_topic = 'move/'
@ -91,6 +92,13 @@ class CobotMove(Node):
e = float(blocks[4][1:]) / J5_COUNTS_PER_RAD e = float(blocks[4][1:]) / J5_COUNTS_PER_RAD
f = float(blocks[5][1:]) / J6_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 # Publish those to the update/jX subscribers
self.s_j1.publish(Float32(data=a)) self.s_j1.publish(Float32(data=a))
self.s_j2.publish(Float32(data=b)) self.s_j2.publish(Float32(data=b))

View file

@ -15,6 +15,9 @@
#include <moveit/common_planning_interface_objects/common_objects.h> #include <moveit/common_planning_interface_objects/common_objects.h>
#include <moveit/move_group_interface/move_group_interface.h> #include <moveit/move_group_interface/move_group_interface.h>
#include <moveit/rdf_loader/rdf_loader.h> #include <moveit/rdf_loader/rdf_loader.h>
#include <moveit/robot_trajectory/robot_trajectory.h>
#include <moveit/trajectory_processing/iterative_time_parameterization.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>
#include "geometry_msgs/msg/pose_stamped.hpp" #include "geometry_msgs/msg/pose_stamped.hpp"
@ -23,18 +26,27 @@ const std::string PLANNING_GROUP = "ar3_arm";
class ScanNPlan : public rclcpp::Node { class ScanNPlan : public rclcpp::Node {
public: public:
ScanNPlan() : Node("scan_n_plan") { 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 // Create /move_pose subscriber
pose_sub_ = this->create_subscription<geometry_msgs::msg::PoseStamped>( pose_sub_ = this->create_subscription<geometry_msgs::msg::PoseStamped>(
"move_pose", "move_pose",
rclcpp::QoS(1), 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<geometry_msgs::msg::PoseStamped>( relative_pose_sub_ = this->create_subscription<geometry_msgs::msg::PoseStamped>(
"move_relative_pose", "move_relative_pose",
rclcpp::QoS(1), rclcpp::QoS(1),
std::bind(&ScanNPlan::relativeMoveCallback, this, std::placeholders::_1) std::bind(&ScanNPlan::relativeMoveCallback, this, std::placeholders::_1),
options
); );
*/
} }
void init() { void init() {
@ -46,6 +58,39 @@ private:
void moveCallback(geometry_msgs::msg::PoseStamped::SharedPtr p) { void moveCallback(geometry_msgs::msg::PoseStamped::SharedPtr p) {
RCLCPP_INFO(this->get_logger(), "GOT MSG"); 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<geometry_msgs::msg::Pose> 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); move_group_->setPoseTarget(p->pose);
moveit::planning_interface::MoveGroupInterface::Plan plan; moveit::planning_interface::MoveGroupInterface::Plan plan;
@ -78,6 +123,8 @@ private:
move_group_->move(); move_group_->move();
} }
private: private:
rclcpp::CallbackGroup::SharedPtr callback_group_;
rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr pose_sub_; rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr pose_sub_;
rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr relative_pose_sub_; rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr relative_pose_sub_;
@ -91,8 +138,11 @@ int main(int argc, char* argv[]) {
auto node = std::make_shared<ScanNPlan>(); auto node = std::make_shared<ScanNPlan>();
RCLCPP_INFO(node->get_logger(), "MoveGroup node started."); RCLCPP_INFO(node->get_logger(), "MoveGroup node started.");
rclcpp::executors::MultiThreadedExecutor executor;
node->init(); node->init();
rclcpp::spin(node); executor.add_node(node);
executor.spin();
// rclcpp::spin(node);
RCLCPP_INFO(node->get_logger(), "Terminating MoveGroup"); RCLCPP_INFO(node->get_logger(), "Terminating MoveGroup");
rclcpp::shutdown(); rclcpp::shutdown();