0
$\begingroup$

Rosanswers logo

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[ind].positions[2] = 0.0;
          goal.trajectory.points[ind].positions[3] = 0.0;
          goal.trajectory.points[ind].positions[4] = 0.0;
          goal.trajectory.points[ind].positions[5] = 0.0;
          goal.trajectory.points[ind].positions[6] = 0.0;
          std::cout << "you typed " << input << std::endl;
          // Velocities
          goal.trajectory.points[ind].velocities.resize(7);
          for (size_t j = 0; j < 7; ++j)
          {
            goal.trajectory.points[ind].velocities[j] = 0.0;
          }
          // To be reached 1 second after starting along the trajectory
          goal.trajectory.points[ind].time_from_start = ros::Duration(1.0);
      }
        
      else if(input=='r')
      {
          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[ind].positions[2] = 0.0;
          goal.trajectory.points[ind].positions[3] = 0.0;
          goal.trajectory.points[ind].positions[4] = 0.0;
          goal.trajectory.points[ind].positions[5] = 0.0;
          goal.trajectory.points[ind].positions[6] = 0.0;
          std::cout << "you typed " << input << std::endl;
          // Velocities
          goal.trajectory.points[ind].velocities.resize(7);
          for (size_t j = 0; j < 7; ++j)
          {
            goal.trajectory.points[ind].velocities[j] = 0.0;
          }
          // To be reached 1 second after starting along the trajectory
          goal.trajectory.points[ind].time_from_start = ros::Duration(1.0);

      } 
      else if(input=='.')
      {
        std::cout << "you typed " << input << "exiting..." << std::endl;
        repeat=0;
      }
      else if(input!='.' && input!='r' && input!='l')
      {
        std::cout << "unknown command.";
      }
      //we are done; return the goal
      return goal;
      
    
  }
   //! Returns the current state of the action
  actionlib::SimpleClientGoalState getState()
  {
    return traj_client_->getState();
  }
};




int main(int argc, char** argv)
{
  // Init the ROS node
  ros::init(argc, argv, "robot_driver");

  RobotArm arm;

  int cont=1;
  while (cont)
  {
    // Start the trajectory
    arm.startTrajectory(arm.armExtensionTrajectory());
    // Wait for trajectory completion
    while(!arm.getState().isDone() && ros::ok())
    {
      usleep(50000);
    }
    std::cout << "press 1 to continue, 0 to end.\n";
    std::cin >> cont;
  }
  return 0;
}

Originally posted by ccm on ROS Answers with karma: 226 on 2012-12-03

Post score: 0

$\endgroup$

1 Answer 1

0
$\begingroup$

Rosanswers logo

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 > 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

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


Originally posted by aknirala with karma: 339 on 2013-03-06

This answer was ACCEPTED on the original site

Post score: 0

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.