ee_cart_imped_arm.cpp
Go to the documentation of this file.
00001 //For detailed comments, see the accompanying .hh file
00002 #include <ee_cart_imped_action/ee_cart_imped_arm.hh>
00003 
00004 EECartImpedArm::EECartImpedArm(std::string ns) {
00005   traj_client_ = new EECartImpedClient(ns+"/ee_cart_imped_action", true);
00006   while (ros::ok() && !traj_client_->waitForServer(ros::Duration(5.0))) {
00007     ROS_INFO("Waiting for the ee_cart_imped_action server");
00008   }
00009 }
00010 
00011 EECartImpedArm::~EECartImpedArm() {
00012   delete traj_client_;
00013 }
00014 
00015 void EECartImpedArm::startTrajectory(ee_cart_imped_msgs::EECartImpedGoal 
00016                                      goal, bool wait) {
00017   goal.header.stamp = ros::Time::now();
00018   traj_client_->sendGoal(goal);
00019   if (wait) {
00020     bool finishedwithintime = traj_client_->waitForResult(ros::Duration(200.0));
00021     if (!finishedwithintime) {
00022       traj_client_->cancelGoal();
00023       ROS_WARN("ee_cart_imped_action: Timed out while attempting to achieve goal");
00024     } else {
00025       actionlib::SimpleClientGoalState state = traj_client_->getState();
00026       if (state == actionlib::SimpleClientGoalState::SUCCEEDED) {
00027         ROS_INFO("ee_cart_imped_action: Successfully performed trajectory.");
00028       } else {
00029         ROS_WARN("ee_cart_imped_action: Failed to complete trajectory");
00030       }
00031     }
00032   }
00033 }
00034 
00035 void EECartImpedArm::stopTrajectory() {
00036   traj_client_->cancelAllGoals();
00037 }
00038 
00039 void EECartImpedArm::addTrajectoryPoint
00040 (ee_cart_imped_msgs::EECartImpedGoal &goal, 
00041  double x, double y,
00042  double z, double ox, double oy, double oz, double ow,
00043  double fx, double fy, double fz, double tx,
00044  double ty, double tz, bool iswfx, bool iswfy,
00045  bool iswfz, bool iswtx, bool iswty, bool iswtz,
00046  double ts, std::string frame_id) {
00047   unsigned int new_point = goal.trajectory.size();
00048   goal.trajectory.resize(goal.trajectory.size()+1);
00049   goal.trajectory[new_point].pose.position.x = x;
00050   goal.trajectory[new_point].pose.position.y = y;
00051   goal.trajectory[new_point].pose.position.z = z;
00052   goal.trajectory[new_point].pose.orientation.x = ox;
00053   goal.trajectory[new_point].pose.orientation.y = oy;
00054   goal.trajectory[new_point].pose.orientation.z = oz;
00055   goal.trajectory[new_point].pose.orientation.w = ow;
00056   goal.trajectory[new_point].wrench_or_stiffness.force.x = fx;
00057   goal.trajectory[new_point].wrench_or_stiffness.force.y = fy; 
00058   goal.trajectory[new_point].wrench_or_stiffness.force.z = fz;
00059   goal.trajectory[new_point].wrench_or_stiffness.torque.x = tx;
00060   goal.trajectory[new_point].wrench_or_stiffness.torque.y = ty;
00061   goal.trajectory[new_point].wrench_or_stiffness.torque.z = tz;
00062   goal.trajectory[new_point].isForceX = iswfx;
00063   goal.trajectory[new_point].isForceY = iswfy;
00064   goal.trajectory[new_point].isForceZ = iswfz;
00065   goal.trajectory[new_point].isTorqueX = iswtx;
00066   goal.trajectory[new_point].isTorqueY = iswty;
00067   goal.trajectory[new_point].isTorqueZ = iswtz;
00068   goal.trajectory[new_point].time_from_start = ros::Duration(ts);
00069   goal.trajectory[new_point].header.stamp = ros::Time(0);
00070   goal.trajectory[new_point].header.frame_id = frame_id;
00071 }


ee_cart_imped_action
Author(s): Jenny Barry, Mario Bollini, and Huan Liu
autogenerated on Sun Jan 5 2014 11:14:46