# Keyboard controller for PR2 arm

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
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 ...
edit retag close merge delete

Sort by » oldest newest most voted

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

Cheers

more