From 6937aa5074849ad0510f96f2cbcaf10d4812ebbb Mon Sep 17 00:00:00 2001 From: Sravan Balaji Date: Sat, 25 Apr 2020 00:10:53 -0400 Subject: [PATCH] Starting Custom IK Arm Controller - Add framework for sending joint trajectory commands via ARIAC interface --- .../src/rrrobot/src/arm_controller_node.cpp | 126 ++++++++---------- 1 file changed, 55 insertions(+), 71 deletions(-) diff --git a/src/rrrobot_ws/src/rrrobot/src/arm_controller_node.cpp b/src/rrrobot_ws/src/rrrobot/src/arm_controller_node.cpp index 2847cc9..b8a6337 100644 --- a/src/rrrobot_ws/src/rrrobot/src/arm_controller_node.cpp +++ b/src/rrrobot_ws/src/rrrobot/src/arm_controller_node.cpp @@ -3,6 +3,7 @@ #include #include #include +#include #include @@ -10,94 +11,77 @@ #include #include -#include -#include - -#include -#include - -#include -#include - -// #include - using namespace std; -// namespace rvt = rviz_visual_tools; - class ArmController { public: - ArmController(ros::NodeHandle & node) : arm_1_has_been_zeroed_(false) { - arm_1_joint_trajectory_publisher_ = node.advertise("/ariac/arm1/arm/command", 10); + ArmController(ros::NodeHandle & node) : arm_has_been_zeroed_(false) { + arm_joint_trajectory_publisher_ = node.advertise("/ariac/arm1/arm/command", 10); + + joint_names = ["linear_arm_actuator_joint", + "shoulder_pan_joint", + "shoulder_lift_joint", + "elbow_joint", + "wrist_1_joint", + "wrist_2_joint", + "wrist_3_joint"]; } - void arm_1_joint_state_callback(const sensor_msgs::JointState::ConstPtr & joint_state_msg) { - ROS_INFO_STREAM_THROTTLE(10, "Joint States arm 1 (throttled to 0.1 Hz):\n" << *joint_state_msg); + void joint_state_callback(const sensor_msgs::JointState::ConstPtr & joint_state_msg) { + ROS_INFO_STREAM_THROTTLE(10, "Arm Joint States (throttled to 0.1 Hz):\n" << *joint_state_msg); + // ROS_INFO_STREAM("Joint States:\n" << *joint_state_msg); - arm_1_current_joint_states_ = *joint_state_msg; - if (!arm_1_has_been_zeroed_) { - arm_1_has_been_zeroed_ = true; + + arm_current_joint_states_ = *joint_state_msg; + + if (!arm_has_been_zeroed_) { + arm_has_been_zeroed_ = true; + ROS_INFO("Sending arm to zero joint positions..."); - send_arm_to_zero_state(arm_1_joint_trajectory_publisher_); + + float64[] positions = [0, 0, 0, 0, 0, 0, 0]; + float64 time_from_start = 0.001; + send_joint_trajectory(positions, time_from_start); } } - /// Create a JointTrajectory with all positions set to zero, and command the arm. - void send_arm_to_zero_state(ros::Publisher & joint_trajectory_publisher) { - // Create a message to send. - trajectory_msgs::JointTrajectory msg; - - // Fill the names of the joints to be controlled. - // Note that the vacuum_gripper_joint is not controllable. - msg.joint_names.clear(); - msg.joint_names.push_back("shoulder_pan_joint"); - msg.joint_names.push_back("shoulder_lift_joint"); - msg.joint_names.push_back("elbow_joint"); - msg.joint_names.push_back("wrist_1_joint"); - msg.joint_names.push_back("wrist_2_joint"); - msg.joint_names.push_back("wrist_3_joint"); - msg.joint_names.push_back("linear_arm_actuator_joint"); - // Create one point in the trajectory. - msg.points.resize(1); - // Resize the vector to the same length as the joint names. - // Values are initialized to 0. - msg.points[0].positions.resize(msg.joint_names.size(), 0.0); - // How long to take getting to the point (floating point seconds). - msg.points[0].time_from_start = ros::Duration(0.001); - ROS_INFO_STREAM("Sending command:\n" << msg); - joint_trajectory_publisher.publish(msg); - } - void target_callback(const geometry_msgs::Pose::ConstPtr & target_pose) { - // Setup - static const string PLANNING_GROUP = "manipulator"; - moveit::planning_interface::MoveGroupInterface move_group(PLANNING_GROUP); - moveit::planning_interface::PlanningSceneInterface planning_scene_interface; - const robot_state::JointModelGroup* joint_model_group = - move_group.getCurrentState()->getJointModelGroup(PLANNING_GROUP); - - // Getting Basic Information - ROS_INFO_NAMED("tutorial", "Planning frame: %s", move_group.getPlanningFrame().c_str()); - ROS_INFO_NAMED("tutorial", "End effector link: %s", move_group.getEndEffectorLink().c_str()); - ROS_INFO_NAMED("tutorial", "Available Planning Groups:"); - std::copy(move_group.getJointModelGroupNames().begin(), move_group.getJointModelGroupNames().end(), std::ostream_iterator(std::cout, ", ")); - - // Planning to a Pose Goal - move_group.setPoseTarget(*target_pose); + // TODO: Use IK to determine joint positions from target_pose - moveit::planning_interface::MoveGroupInterface::Plan my_plan; - bool success = (move_group.plan(my_plan) == - moveit::planning_interface::MoveItErrorCode::SUCCESS); - ROS_INFO_NAMED("tutorial", "Visualizing plan 1 (pose goal) %s", success ? "" : "FAILED"); - // Moving to a pose goal - move_group.move(); + // Send target joint trajectory + float64[] positions = [0, 0, 0, 0, 0, 0, 0]; + float64 time_from_start = 2; + send_joint_trajectory(positions, time_from_start); + + } private: - ros::Publisher arm_1_joint_trajectory_publisher_; - sensor_msgs::JointState arm_1_current_joint_states_; - bool arm_1_has_been_zeroed_; + bool arm_has_been_zeroed_; + ros::Publisher arm_joint_trajectory_publisher_; + sensor_msgs::JointState arm_current_joint_states_; + const string[] joint_names_; + + void send_joint_trajectory(const float64[] & positions, const float64 time_from_start) { + trajectory_msgs::JointTrajectory msg; + + // Fill the names of the joints to be controlled + msg.joint_names = joint_names_; + + // Create on point in the trajectory + msg.points.resize(1); + + // Set joint positions + msg.points[0].positions = positions; + + // How long to take getting to the point (floating point seconds) + msg.points[0].time_from_start = ros::Duration(time_from_start); + + ROS_INFO_STREAM("Sending command:\n" << msg); + + arm_joint_trajectory_publisher_.publish(msg); + } };