Ask Your Question
0

Keyboard controller for PR2 arm

asked 2012-12-03 19:44:02 -0600

ccm gravatar image

updated 2014-11-22 17:05:23 -0600

ngrennan gravatar image

Hi, I am currently trying to write a pr2 arm controller that will allow me to control all the arm joints of pr2 by keyboard

I'm basing my code on this which will let me control r_arm_controller.

I tried modifying it but I still cannot achieve what I want. The keyboard controls that I wrote does not allow me increase the joint angle each time I press it, but instead remains at the fixed increment I tried to specify. Can someone teach me how do I change it so that I can control the arms to move anywhere I want just by keyboard without the hassle of going into the c++ code to change the values.

The code that I have written is below, I only wrote controls for the the first joint, r_shoulder_pan_joint.

Thank you so much.

#include <ros/ros.h>
#include <pr2_controllers_msgs/JointTrajectoryAction.h>
#include <actionlib/client/simple_action_client.h>
#include <iostream>
typedef actionlib::SimpleActionClient< pr2_controllers_msgs::JointTrajectoryAction > TrajClient;

char input()
  {std::cout << "Type a command and then press enter.  "
                "'l' to turn left, "
                "'r' to turn right\n";
   char input;
   std::cin >> input;
   return input;
   }

class RobotArm
{
private:
  // Action client for the joint trajectory action 
  // used to trigger the arm movement action
  TrajClient* traj_client_;

public:
  //! Initialize the action client and wait for action server to come up
  RobotArm() 
  {
    // tell the action client that we want to spin a thread by default
    traj_client_ = new TrajClient("r_arm_controller/joint_trajectory_action", true);

    // wait for action server to come up
    while(!traj_client_->waitForServer(ros::Duration(5.0))){
      ROS_INFO("Waiting for the joint_trajectory_action server");
    }
  }

  //! Clean up the action client
  ~RobotArm()
  {
    delete traj_client_;
  }

  //! Sends the command to start a given trajectory
  void startTrajectory(pr2_controllers_msgs::JointTrajectoryGoal goal)
  {
    // When to start the trajectory: 0.5s from now
    goal.trajectory.header.stamp = ros::Time::now() + ros::Duration(0.5);
    traj_client_->sendGoal(goal);
  }

  //! Generates a simple trajectory with two waypoints, used as an example
  /*! Note that this trajectory contains two waypoints, joined together
      as a single trajectory. Alternatively, each of these waypoints could
      be in its own trajectory - a trajectory can have one or more waypoints
      depending on the desired application.
  */
  pr2_controllers_msgs::JointTrajectoryGoal armExtensionTrajectory()
  {
    //initiate armextension
    std::cout << "armExtensionTrajectory initiated.\n";

    //our goal variable
    pr2_controllers_msgs::JointTrajectoryGoal goal;

    // First, the joint names, which apply to all waypoints
    goal.trajectory.joint_names.push_back("r_shoulder_pan_joint");
    goal.trajectory.joint_names.push_back("r_shoulder_lift_joint");
    goal.trajectory.joint_names.push_back("r_upper_arm_roll_joint");
    goal.trajectory.joint_names.push_back("r_elbow_flex_joint");
    goal.trajectory.joint_names.push_back("r_forearm_roll_joint");
    goal.trajectory.joint_names.push_back("r_wrist_flex_joint");
    goal.trajectory.joint_names.push_back("r_wrist_roll_joint");


    // We will have one waypoints in this goal trajectory
    goal.trajectory.points.resize(1);

    // First trajectory point
    // Positions
    int ind = 0;
    goal.trajectory.points[ind].positions.resize(1);
    std::cout << "Type a command and then press enter.  "
                "'l' to turn left, "
                "'r' to turn right, '.' to exit\n";
    char input;
    int repeat;

      std::cout << "Loop started. \n";
      std::cin >> input;
      if(input=='l')
      {
          goal.trajectory.points[ind].positions.resize(7);
          goal.trajectory.points[ind].positions[0] += 0.2;
          goal.trajectory.points[ind].positions[1] = 0.0;
          goal.trajectory.points ...
(more)
edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
0

answered 2013-03-06 05:17:11 -0600

Hello Chun Meng,

I hope u have solved this by now. The code is behaving so because each time you are passing the same goal. You need to add the value by which you need to move the joint to the current value of the joint. For this you need to first read the current joint state. This can be done by reading it from the topic : /r_arm_controller/state for the state of the r_arm.

Concretely these modification in your code should do the trick:-


...
#include <pr2_controllers_msgs/JointTrajectoryControllerState.h>
..
class RobotArm
{
....
//In the class add one more object
pr2_controllers_msgs::JointTrajectoryControllerState getArmState;
//Added this boolean variable for synchronization. I am sure there are better ways.
bool doUpdate;
...

//Add the callback function, which will be called while listening to the topic.
 void stateCallBack(const pr2_controllers_msgs::JointTrajectoryControllerState &getArmState)
  {
    doUpdate = false;
    this->getArmState = getArmState;
  }

....

  pr2_controllers_msgs::JointTrajectoryGoal armExtensionTrajectory()
  {
    ....
    std::cout << "Loop started. \n";
    std::cin >> input;

    //After this read the current state of the joint by listening to the topic
    ros::NodeHandle n;
    ros::Rate r(10);
    doUpdate = true;   //Setting true for synchronization

    //Subscribing to the topic
    ros::Subscriber sub = n.subscribe("/r_arm_controller/state", 1,&RobotArm::stateCallBack, this);
     while(doUpdate)    //Wait till the values are read
     {
        std::cout<<"Still updating so sleeping";
            ros::spinOnce();
        r.sleep();
     }
      }//Now the current values have been updated by callback fn in : getArmState
....
    //instead of this : goal.trajectory.points[ind].positions[0] += 0.2;
    //add 0.2 to current value
    goal.trajectory.points[ind].positions[0] = getArmState.actual.positions[0]+0.2;
    ....
    //Similarly do for r case
   }

}
 

I am sure there are better ways to do synchronization and updating using boost pointer, which I am reading now.

Also please go through : http://www.ros.org/wiki/roscpp/Overview if you haven't

Hope the answer helps.

Cheers

edit flag offensive delete link more

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower

Stats

Asked: 2012-12-03 19:44:02 -0600

Seen: 328 times

Last updated: Mar 06 '13