Go to the documentation of this file.00001
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 }