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