sravan/RRRobot
1
0
Fork 0
mirror of https://github.com/EECS-467-W20-RRRobot-Project/RRRobot.git synced 2025-08-31 19:33:14 +00:00
Code Issues Packages Projects Releases Wiki Activity

Starting Custom IK Arm Controller

Browse Source 
- Add framework for sending joint trajectory commands via ARIAC interface
This commit is contained in:
Sravan Balaji
2020-04-25 00:10:53 -04:00
parent 9ea4101f35
commit 6937aa5074
1 changed files with 55 additions and 71 deletions
Download Patch File Download Diff File Expand all files Collapse all files

126
src/rrrobot_ws/src/rrrobot/src/arm_controller_node.cpp
View File

@@ -3,6 +3,7 @@
#include <algorithm>
#include <vector>
#include <string>
#include <memory>
#include <ros/ros.h>
@@ -10,94 +11,77 @@
#include <std_msgs/String.h>
#include <std_srvs/Trigger.h>
#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>
#include <moveit_msgs/DisplayRobotState.h>
#include <moveit_msgs/DisplayTrajectory.h>
#include <moveit_msgs/AttachedCollisionObject.h>
#include <moveit_msgs/CollisionObject.h>
// #include <moveit_visual_tools/moveit_visual_tools.h>
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<trajectory_msgs::JointTrajectory>("/ariac/arm1/arm/command", 10);
ArmController(ros::NodeHandle & node) : arm_has_been_zeroed_(false) {
arm_joint_trajectory_publisher_ = node.advertise<trajectory_msgs::JointTrajectory>("/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::string>(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);
}
};