Ask Your Question

Revision history [back]

follow_joint_trajectoy action server example for taking moveit commands

I want to control an additional motor along with my UR5 using moveit. I have added new controller to the moveit config controllers.yaml called rail_controller and made necessary changes to the URDF like adding extra joint and transmissions.

 controller_list:
  - name: "arm_controller"
    action_ns: follow_joint_trajectory
    type: FollowJointTrajectory
    joints: [shoulder_pan_joint, shoulder_lift_joint, elbow_joint, wrist_1_joint, wrist_2_joint, wrist_3_joint]

  - name: "rail_controller"
    action_ns: follow_joint_trajectory
    type: FollowJointTrajectory
    default: true
    joints:
      - rail_joint

I think the next step is to write a node that can take FollowJointTrajectory type of command and pass it on to my motor.

Can i find an example for writing follow_joint_trajectoy action server that can take joint space commands from moveit and pass it forward to the actual motor.

Any help would be greatly appreciated, with so many types of controller documentation,I am unable to find the right resource to follow.

follow_joint_trajectoy action server example for taking moveit commands

I want to control an additional motor along with my UR5 using moveit. I have added new controller to the moveit config controllers.yaml called rail_controller and made necessary changes to the URDF like adding extra joint and transmissions.

 controller_list:
  - name: "arm_controller"
    action_ns: follow_joint_trajectory
    type: FollowJointTrajectory
    joints: [shoulder_pan_joint, shoulder_lift_joint, elbow_joint, wrist_1_joint, wrist_2_joint, wrist_3_joint]

  - name: "rail_controller"
    action_ns: follow_joint_trajectory
    type: FollowJointTrajectory
    default: true
    joints:
      - rail_joint

I think As the next step is to write a node I wrote an action server that can take FollowJointTrajectory type of command and pass it command. Here's is the file

#include <ros/ros.h>
#include <actionlib/server/simple_action_server.h>
#include <control_msgs/FollowJointTrajectoryAction.h>
#include <trajectory_msgs/JointTrajectory.h>

class RobotTrajectoryFollower
{
protected:

  ros::NodeHandle nh_;
  actionlib::SimpleActionServer<control_msgs::FollowJointTrajectoryAction> as_; 
  std::string action_name_;

public:

  RobotTrajectoryFollower(std::string name) :
    as_(nh_, name, false),
    action_name_(name)
  {
    //Register callback functions:
    as_.registerGoalCallback(boost::bind(&RobotTrajectoryFollower::goalCB, this));
    as_.registerPreemptCallback(boost::bind(&RobotTrajectoryFollower::preemptCB, this));

    as_.start();
  }

  ~RobotTrajectoryFollower(void)//Destructor
  {
  }

  void goalCB()
  {
     ROS_INFO("RAIL CONTROLLER ACTION-GOAL RECEIVED");
    // accept the new goal
    //goal_ = as_.acceptNewGoal()->samples;
  }

  void preemptCB()
  {
    ROS_INFO("%s: Preempted", action_name_.c_str());
    // set the action state to preempted
    as_.setPreempted();
  }
};


int main(int argc, char** argv)
{
  ros::init(argc, argv, "action_server");

  RobotTrajectoryFollower RobotTrajectoryFollower("/rail_controller/follow_joint_trajectory");
  ros::spin();

  return 0;
}

What should be the next step,so that I can forward the commands received on to my motor.

Can i find an example for writing follow_joint_trajectoy action this server that can take joint space commands callback from moveit and pass it forward followjointtrajectory client to the actual motor.

physical servo motor that I have. Any help would be greatly appreciated, I seem to have stuck here with so many types of controller documentation,I am unable to find the right resource to follow.

follow_joint_trajectoy action server example for taking moveit commands

I want to control an additional motor along with my UR5 using moveit. I have added new controller to the moveit config controllers.yaml called rail_controller and made necessary changes to the URDF like adding extra joint and transmissions.

 controller_list:
  - name: "arm_controller"
    action_ns: follow_joint_trajectory
    type: FollowJointTrajectory
    joints: [shoulder_pan_joint, shoulder_lift_joint, elbow_joint, wrist_1_joint, wrist_2_joint, wrist_3_joint]

  - name: "rail_controller"
    action_ns: follow_joint_trajectory
    type: FollowJointTrajectory
    default: true
    joints:
      - rail_joint

As the next step I wrote an action server that can take FollowJointTrajectory type of command. Here's is the file

#include <ros/ros.h>
#include <actionlib/server/simple_action_server.h>
#include <control_msgs/FollowJointTrajectoryAction.h>
#include <trajectory_msgs/JointTrajectory.h>

class RobotTrajectoryFollower
{
protected:

  ros::NodeHandle nh_;
  actionlib::SimpleActionServer<control_msgs::FollowJointTrajectoryAction> as_; 
  std::string action_name_;

public:

  RobotTrajectoryFollower(std::string name) :
    as_(nh_, name, false),
    action_name_(name)
  {
    //Register callback functions:
    as_.registerGoalCallback(boost::bind(&RobotTrajectoryFollower::goalCB, this));
    as_.registerPreemptCallback(boost::bind(&RobotTrajectoryFollower::preemptCB, this));

    as_.start();
  }

  ~RobotTrajectoryFollower(void)//Destructor
  {
  }

  void goalCB()
  {
     ROS_INFO("RAIL CONTROLLER ACTION-GOAL RECEIVED");
    // accept the new goal
    //goal_ = as_.acceptNewGoal()->samples;
  }

  void preemptCB()
  {
    ROS_INFO("%s: Preempted", action_name_.c_str());
    // set the action state to preempted
    as_.setPreempted();
  }
};


int main(int argc, char** argv)
{
  ros::init(argc, argv, "action_server");

  RobotTrajectoryFollower RobotTrajectoryFollower("/rail_controller/follow_joint_trajectory");
  ros::spin();

  return 0;
}

What should be the next step,so that I can forward the commands received on this server callback from moveit followjointtrajectory client to the actual physical servo motor that I have. Any help would be greatly appreciated, I seem to have stuck here with so many types of controller documentation,I am unable to find the right resource to follow.

follow_joint_trajectoy action server example for taking moveit commandsControlling a servo motor along with UR5 through moveit

I want to control an additional motor along with my UR5 using moveit. I have added new controller to the moveit config controllers.yaml called rail_controller and made necessary changes to the URDF like adding extra joint and transmissions.

 controller_list:
  - name: "arm_controller"
    action_ns: follow_joint_trajectory
    type: FollowJointTrajectory
    joints: [shoulder_pan_joint, shoulder_lift_joint, elbow_joint, wrist_1_joint, wrist_2_joint, wrist_3_joint]

  - name: "rail_controller"
    action_ns: follow_joint_trajectory
    type: FollowJointTrajectory
    default: true
    joints:
      - rail_joint

As the next step I wrote an action server that can take FollowJointTrajectory type of command. Here's is the file

#include <ros/ros.h>
#include <actionlib/server/simple_action_server.h>
#include <control_msgs/FollowJointTrajectoryAction.h>
#include <trajectory_msgs/JointTrajectory.h>

class RobotTrajectoryFollower
{
protected:

  ros::NodeHandle nh_;
  actionlib::SimpleActionServer<control_msgs::FollowJointTrajectoryAction> as_; 
  std::string action_name_;

public:

  RobotTrajectoryFollower(std::string name) :
    as_(nh_, name, false),
    action_name_(name)
  {
    //Register callback functions:
    as_.registerGoalCallback(boost::bind(&RobotTrajectoryFollower::goalCB, this));
    as_.registerPreemptCallback(boost::bind(&RobotTrajectoryFollower::preemptCB, this));

    as_.start();
  }

  ~RobotTrajectoryFollower(void)//Destructor
  {
  }

  void goalCB()
  {
     ROS_INFO("RAIL CONTROLLER ACTION-GOAL RECEIVED");
    // accept the new goal
    //goal_ = as_.acceptNewGoal()->samples;
  }

  void preemptCB()
  {
    ROS_INFO("%s: Preempted", action_name_.c_str());
    // set the action state to preempted
    as_.setPreempted();
  }
};


int main(int argc, char** argv)
{
  ros::init(argc, argv, "action_server");

  RobotTrajectoryFollower RobotTrajectoryFollower("/rail_controller/follow_joint_trajectory");
  ros::spin();

  return 0;
}

What should be the next step,so that I can forward the commands received on this server callback from moveit followjointtrajectory client to the actual physical servo motor that I have. Any help would be greatly appreciated, I seem to have stuck here with so many types of controller documentation,I am unable to find the right resource to follow.

Controlling a industrial servo motor along with UR5 through moveit

I want to control an additional motor along with my UR5 using moveit. I have added new controller to the moveit config controllers.yaml called rail_controller and made necessary changes to the URDF like adding extra joint and transmissions.

 controller_list:
  - name: "arm_controller"
    action_ns: follow_joint_trajectory
    type: FollowJointTrajectory
    joints: [shoulder_pan_joint, shoulder_lift_joint, elbow_joint, wrist_1_joint, wrist_2_joint, wrist_3_joint]

  - name: "rail_controller"
    action_ns: follow_joint_trajectory
    type: FollowJointTrajectory
    default: true
    joints:
      - rail_joint

As the next step I wrote an action server that can take FollowJointTrajectory type of command. Here's is the file

#include <ros/ros.h>
#include <actionlib/server/simple_action_server.h>
#include <control_msgs/FollowJointTrajectoryAction.h>
#include <trajectory_msgs/JointTrajectory.h>

class RobotTrajectoryFollower
{
protected:

  ros::NodeHandle nh_;
  actionlib::SimpleActionServer<control_msgs::FollowJointTrajectoryAction> as_; 
  std::string action_name_;

public:

  RobotTrajectoryFollower(std::string name) :
    as_(nh_, name, false),
    action_name_(name)
  {
    //Register callback functions:
    as_.registerGoalCallback(boost::bind(&RobotTrajectoryFollower::goalCB, this));
    as_.registerPreemptCallback(boost::bind(&RobotTrajectoryFollower::preemptCB, this));

    as_.start();
  }

  ~RobotTrajectoryFollower(void)//Destructor
  {
  }

  void goalCB()
  {
     ROS_INFO("RAIL CONTROLLER ACTION-GOAL RECEIVED");
    // accept the new goal
    //goal_ = as_.acceptNewGoal()->samples;
  }

  void preemptCB()
  {
    ROS_INFO("%s: Preempted", action_name_.c_str());
    // set the action state to preempted
    as_.setPreempted();
  }
};


int main(int argc, char** argv)
{
  ros::init(argc, argv, "action_server");

  RobotTrajectoryFollower RobotTrajectoryFollower("/rail_controller/follow_joint_trajectory");
  ros::spin();

  return 0;
}

What should be the next step,so that I can forward the commands received on this server callback from moveit followjointtrajectory client to the actual physical servo motor that I have. Any help would be greatly appreciated, I seem to have stuck here with so many types of controller documentation,I am unable to find the right resource to follow.

Controlling a industrial servo motor through moveit

I want to control an additional motor along with my UR5 using moveit. I have added new controller to the moveit config controllers.yaml called rail_controller and made necessary changes to the URDF like adding extra joint and transmissions.

 controller_list:
  - name: "arm_controller"
    action_ns: follow_joint_trajectory
    type: FollowJointTrajectory
    joints: [shoulder_pan_joint, shoulder_lift_joint, elbow_joint, wrist_1_joint, wrist_2_joint, wrist_3_joint]

  - name: "rail_controller"
    action_ns: follow_joint_trajectory
    type: FollowJointTrajectory
    default: true
    joints:
      - rail_joint

As the next step I wrote an action server that can take FollowJointTrajectory type of command. Here's is the file

#include <ros/ros.h>
#include <actionlib/server/simple_action_server.h>
#include <control_msgs/FollowJointTrajectoryAction.h>
#include <trajectory_msgs/JointTrajectory.h>

class RobotTrajectoryFollower
{
protected:

  ros::NodeHandle nh_;
  actionlib::SimpleActionServer<control_msgs::FollowJointTrajectoryAction> as_; 
  std::string action_name_;

public:

  RobotTrajectoryFollower(std::string name) :
    as_(nh_, name, false),
    action_name_(name)
  {
    //Register callback functions:
    as_.registerGoalCallback(boost::bind(&RobotTrajectoryFollower::goalCB, this));
    as_.registerPreemptCallback(boost::bind(&RobotTrajectoryFollower::preemptCB, this));

    as_.start();
  }

  ~RobotTrajectoryFollower(void)//Destructor
  {
  }

  void goalCB()
  {
     ROS_INFO("RAIL CONTROLLER ACTION-GOAL RECEIVED");
    // accept the new goal
    //goal_ = as_.acceptNewGoal()->samples;
  }

  void preemptCB()
  {
    ROS_INFO("%s: Preempted", action_name_.c_str());
    // set the action state to preempted
    as_.setPreempted();
  }
};


int main(int argc, char** argv)
{
  ros::init(argc, argv, "action_server");

  RobotTrajectoryFollower RobotTrajectoryFollower("/rail_controller/follow_joint_trajectory");
  ros::spin();

  return 0;
}

What should be the next step,so that I can forward the commands received on this server callback from moveit followjointtrajectory client to the actual physical servo motor that I have. Any help would be greatly appreciated, I seem to have stuck here with so many types of controller documentation,I am unable to find the right resource to follow.

Controlling a industrial servo motor through moveit

I want to control an additional motor along with my UR5 using moveit. I have added new controller to the moveit config controllers.yaml called rail_controller and made necessary changes to the URDF like adding extra joint and transmissions.

 controller_list:
  - name: "arm_controller"
    action_ns: follow_joint_trajectory
    type: FollowJointTrajectory
    joints: [shoulder_pan_joint, shoulder_lift_joint, elbow_joint, wrist_1_joint, wrist_2_joint, wrist_3_joint]

  - name: "rail_controller"
    action_ns: follow_joint_trajectory
    type: FollowJointTrajectory
    default: true
    joints:
      - rail_joint

As the next step I wrote an action server that can take FollowJointTrajectory type of command. Here's is the file

#include <ros/ros.h>
#include <actionlib/server/simple_action_server.h>
#include <control_msgs/FollowJointTrajectoryAction.h>
#include <trajectory_msgs/JointTrajectory.h>

class RobotTrajectoryFollower
{
protected:

  ros::NodeHandle nh_;
  actionlib::SimpleActionServer<control_msgs::FollowJointTrajectoryAction> as_; 
  std::string action_name_;

public:

  RobotTrajectoryFollower(std::string name) :
    as_(nh_, name, false),
    action_name_(name)
  {
    //Register callback functions:
    as_.registerGoalCallback(boost::bind(&RobotTrajectoryFollower::goalCB, this));
    as_.registerPreemptCallback(boost::bind(&RobotTrajectoryFollower::preemptCB, this));

    as_.start();
  }

  ~RobotTrajectoryFollower(void)//Destructor
  {
  }

  void goalCB()
  {
     ROS_INFO("RAIL CONTROLLER ACTION-GOAL RECEIVED");
    // accept the new goal
    //goal_ = as_.acceptNewGoal()->samples;
  }

  void preemptCB()
  {
    ROS_INFO("%s: Preempted", action_name_.c_str());
    // set the action state to preempted
    as_.setPreempted();
  }
};


int main(int argc, char** argv)
{
  ros::init(argc, argv, "action_server");

  RobotTrajectoryFollower RobotTrajectoryFollower("/rail_controller/follow_joint_trajectory");
  ros::spin();

  return 0;
}

What should be the next step,so that I can forward the commands received on this server callback from moveit followjointtrajectory client to the actual physical servo motor that I have. Any help would be greatly appreciated, I seem to have stuck here with so many types of controller documentation,I am unable to find the right resource to follow.

Controlling a an industrial servo motor through moveit

I want to control an additional motor along with my UR5 using moveit. I have added new controller to the moveit config controllers.yaml called rail_controller and made necessary changes to the URDF like adding extra joint and transmissions.

 controller_list:
  - name: "arm_controller"
    action_ns: follow_joint_trajectory
    type: FollowJointTrajectory
    joints: [shoulder_pan_joint, shoulder_lift_joint, elbow_joint, wrist_1_joint, wrist_2_joint, wrist_3_joint]

  - name: "rail_controller"
    action_ns: follow_joint_trajectory
    type: FollowJointTrajectory
    default: true
    joints:
      - rail_joint

As the next step I wrote an action server that can take FollowJointTrajectory type of command. Here's is the file

#include <ros/ros.h>
#include <actionlib/server/simple_action_server.h>
#include <control_msgs/FollowJointTrajectoryAction.h>
#include <trajectory_msgs/JointTrajectory.h>

class RobotTrajectoryFollower
{
protected:

  ros::NodeHandle nh_;
  actionlib::SimpleActionServer<control_msgs::FollowJointTrajectoryAction> as_; 
  std::string action_name_;

public:

  RobotTrajectoryFollower(std::string name) :
    as_(nh_, name, false),
    action_name_(name)
  {
    //Register callback functions:
    as_.registerGoalCallback(boost::bind(&RobotTrajectoryFollower::goalCB, this));
    as_.registerPreemptCallback(boost::bind(&RobotTrajectoryFollower::preemptCB, this));

    as_.start();
  }

  ~RobotTrajectoryFollower(void)//Destructor
  {
  }

  void goalCB()
  {
     ROS_INFO("RAIL CONTROLLER ACTION-GOAL RECEIVED");
    // accept the new goal
    //goal_ = as_.acceptNewGoal()->samples;
  }

  void preemptCB()
  {
    ROS_INFO("%s: Preempted", action_name_.c_str());
    // set the action state to preempted
    as_.setPreempted();
  }
};


int main(int argc, char** argv)
{
  ros::init(argc, argv, "action_server");

  RobotTrajectoryFollower RobotTrajectoryFollower("/rail_controller/follow_joint_trajectory");
  ros::spin();

  return 0;
}

What should be the next step,so that I can forward the commands received on this server callback from moveit followjointtrajectory client to the actual physical servo motor that I have. Any help would be greatly appreciated, I seem to have stuck here with so many types of controller documentation,I am unable to find the right resource to follow.


Edit: Thanks for the prompt reply.

So, I read about ros_control and modified the ros_control_boilerplate to have my railcontroller configured instead of the rrbot.
The code for my setup is here https://drive.google.com/drive/folders/0B3-QK1vsHSXhd3pKcS1VX3lqTUk?usp=sharing

Here's the railrobot_controllers.yaml as defined in ros_control package:

# ros_control_boilerplate Settings -----------------------
# Settings for ros_control control loop
generic_hw_control_loop:
  loop_hz: 300
  cycle_time_error_threshold: 0.01

# Settings for ros_control hardware interface
hardware_interface:
   joints:
      - rail_joint

# Publish all joint states ----------------------------------
# Creates the /joint_states topic necessary in ROS
joint_state_controller:
   type:         joint_state_controller/JointStateController
   publish_rate: 50

# Joint Trajectory Controller -------------------------------
# For detailed explanations of parameter see http://wiki.ros.org/joint_trajectory_controller
position_trajectory_controller:
   type: position_controllers/JointTrajectoryController
   # These joints can likely just be copied from the hardware_interface list above
   joints:
      - rail_joint

# Individual Position Controllers ---------------------------------------
# Allows to send individual ROS msg of Float64 to each joint separately
rail_joint_position_controller:
   type: position_controllers/JointPositionController
   joint: joint1
   pid: {p: 100.0, i: 0.01, d: 10.0}

What I cant seem to figure out how the controller above defined here in ros_control_boilerplate/railrobot_control/config/railrobot_controllers.yaml ,will communicate with the follow joint trajectory action server that will in turn connect to moveit.

My understanding of ros_control and it's integration with moveit is quite amateur at the moment.I would appreciate if you can help me fill these missing pieces of knowledge.