jaco_comm.cpp
Go to the documentation of this file.
00001 
00046 #include <ros/ros.h>
00047 #include "jaco_driver/jaco_comm.h"
00048 
00049 #define PI 3.14159265358
00050 
00051 namespace jaco
00052 {
00053 
00054 JacoComm::JacoComm(JacoAngles home) : software_stop(false), home_position(home)
00055 {
00056         boost::recursive_mutex::scoped_lock lock(api_mutex);
00057         /* Connecting to Jaco Arm */
00058         ROS_INFO("Initiating Library");
00059         API = new JacoAPI();
00060         ROS_INFO("Initiating API");
00061 
00062         int api_result = 0; //stores result from the API
00063         ros::Duration(1.0).sleep();
00064 
00065         api_result = (API->InitAPI());
00066 
00067         /* 
00068         A common result that may be returned is "1014", which means communications
00069         could not be established with the arm.  This often means the arm is not turned on, 
00070         or the InitAPI command was initiated before the arm had fully booted up.
00071         */
00072 
00073         if (api_result != 1)
00074         {
00075                 /* Failed to contact arm */
00076                 ROS_FATAL("Could not initialize arm");
00077                 ROS_FATAL("Jaco_InitAPI returned: %d", api_result);
00078                 #ifndef DEBUG_WITHOUT_ARM
00079                 ros::shutdown();
00080                 #endif
00081         } 
00082         else
00083         {
00084                 ROS_INFO("API Initialized Successfully!");
00085         }
00086 }
00087 
00088 JacoComm::~JacoComm()
00089 {
00090         API->CloseAPI();
00091 }
00092 
00099 bool JacoComm::HomeState(void)
00100 {
00101         boost::recursive_mutex::scoped_lock lock(api_mutex);
00102         const float tolerance = 2.0; //dead zone for angles (degrees)
00103 
00104         AngularPosition cur_angles; //holds the current angles of the arm
00105         API->GetAngularPosition(cur_angles); //update current arm angles
00106 
00107         return home_position.Compare(JacoAngles(cur_angles.Actuators), tolerance);
00108 }
00109 
00120 void JacoComm::HomeArm(void)
00121 {
00122         boost::recursive_mutex::scoped_lock lock(api_mutex);
00123         if (Stopped())
00124         {
00125                 return;
00126         }
00127 
00128         if (HomeState())
00129         {
00130                 ROS_INFO("Arm is already in \"home\" position");
00131                 return;
00132         }
00133 
00134         API->StartControlAPI();
00135 
00136         JoystickCommand home_command;
00137         memset(&home_command, 0, sizeof(home_command)); //zero structure
00138 
00139         home_command.ButtonValue[2] = 1;
00140         API->SendJoystickCommand(home_command);
00141 
00142         WaitForHome(25);
00143 
00144         home_command.ButtonValue[2] = 0;
00145         API->SendJoystickCommand(home_command);
00146 }
00147 
00153 void JacoComm::InitializeFingers(void)
00154 {
00155         FingerAngles fingers_home;
00156 
00157         // Set the fingers fully "open." This is required to initialize the fingers.
00158         SetFingers(fingers_home, 5);
00159         ros::Duration(3.0).sleep();
00160 
00161         // Set the fingers to "half-open"
00162         fingers_home.Finger1 = 40;
00163         fingers_home.Finger2 = 40;
00164         fingers_home.Finger3 = 40;
00165         SetFingers(fingers_home, 5);
00166 }
00167 
00173 void JacoComm::SetAngles(JacoAngles &angles, int timeout, bool push)
00174 {
00175         boost::recursive_mutex::scoped_lock lock(api_mutex);
00176         if (Stopped())
00177                 return;
00178 
00179         TrajectoryPoint Jaco_Position;
00180 
00181         memset(&Jaco_Position, 0, sizeof(Jaco_Position)); //zero structure
00182 
00183         if (push == true)
00184         {
00185                 API->EraseAllTrajectories();
00186                 API->StopControlAPI();
00187         }
00188         
00189         API->StartControlAPI();
00190         API->SetAngularControl();
00191         
00192         //Jaco_Position.LimitationsActive = false;
00193         Jaco_Position.Position.Delay = 0.0;
00194         Jaco_Position.Position.Type = ANGULAR_POSITION;
00195 
00196         Jaco_Position.Position.Actuators = angles; 
00197 
00198         API->SendAdvanceTrajectory(Jaco_Position);
00199 }
00200 
00206 void JacoComm::SetPosition(JacoPose &position, int timeout, bool push)
00207 {
00208         boost::recursive_mutex::scoped_lock lock(api_mutex);
00209         if (Stopped())
00210                 return;
00211 
00212         TrajectoryPoint Jaco_Position;
00213 
00214         memset(&Jaco_Position, 0, sizeof(Jaco_Position)); //zero structure
00215 
00216         if (push == true)
00217         {
00218                 API->EraseAllTrajectories();
00219                 API->StopControlAPI();
00220         }
00221 
00222         API->StartControlAPI();
00223         API->SetCartesianControl();
00224 
00225         //Jaco_Position.LimitationsActive = false;
00226         Jaco_Position.Position.Delay = 0.0;
00227         Jaco_Position.Position.Type = CARTESIAN_POSITION;
00228 
00229         Jaco_Position.Position.CartesianPosition = position;
00230         //Jaco_Position.Position.CartesianPosition.ThetaZ += 0.0001; // A workaround for a bug in the Kinova API
00231 
00232         API->SendBasicTrajectory(Jaco_Position);
00233 }
00234 
00238 void JacoComm::SetFingers(FingerAngles &fingers, int timeout, bool push)
00239 {
00240         boost::recursive_mutex::scoped_lock lock(api_mutex);
00241         if (Stopped())
00242                 return;
00243 
00244         TrajectoryPoint Jaco_Position;
00245         memset(&Jaco_Position, 0, sizeof(Jaco_Position)); //zero structure
00246 
00247         if (push == true)
00248         {
00249                 API->EraseAllTrajectories();
00250                 API->StopControlAPI();
00251         }
00252 
00253         API->StartControlAPI();
00254 
00255         Jaco_Position.Position.HandMode = POSITION_MODE;
00256         Jaco_Position.Position.Fingers = fingers;
00257 
00258         API->SendAdvanceTrajectory(Jaco_Position);
00259         ROS_DEBUG("Sending Fingers");
00260 }
00261 
00265 void JacoComm::SetVelocities(AngularInfo joint_vel)
00266 {
00267         boost::recursive_mutex::scoped_lock lock(api_mutex);
00268         if (Stopped())
00269                 return;
00270 
00271         TrajectoryPoint Jaco_Velocity;
00272 
00273         memset(&Jaco_Velocity, 0, sizeof(Jaco_Velocity)); //zero structure
00274 
00275         API->StartControlAPI();
00276         Jaco_Velocity.Position.Type = ANGULAR_VELOCITY;
00277 
00278         // confusingly, velocity is passed in the position struct
00279         Jaco_Velocity.Position.Actuators = joint_vel;
00280 
00281         API->SendAdvanceTrajectory(Jaco_Velocity);
00282 }
00283 
00287 void JacoComm::SetCartesianVelocities(CartesianInfo velocities)
00288 {
00289         boost::recursive_mutex::scoped_lock lock(api_mutex);
00290         if (Stopped())
00291         {
00292                 API->EraseAllTrajectories();
00293                 return;
00294         }
00295 
00296         TrajectoryPoint Jaco_Velocity;
00297 
00298         memset(&Jaco_Velocity, 0, sizeof(Jaco_Velocity)); //zero structure
00299 
00300         API->StartControlAPI();
00301         Jaco_Velocity.Position.Type = CARTESIAN_VELOCITY;
00302 
00303         // confusingly, velocity is passed in the position struct
00304         Jaco_Velocity.Position.CartesianPosition = velocities;
00305 
00306         API->SendAdvanceTrajectory(Jaco_Velocity);
00307 }
00308 
00315 void JacoComm::SetConfig(ClientConfigurations config)
00316 {
00317         boost::recursive_mutex::scoped_lock lock(api_mutex);
00318         API->SetClientConfigurations(config);
00319 }
00320 
00324 void JacoComm::GetAngles(JacoAngles &angles)
00325 {
00326         boost::recursive_mutex::scoped_lock lock(api_mutex);
00327 
00328         AngularPosition Jaco_Position;
00329         API->GetAngularPosition(Jaco_Position);
00330 
00331         angles = Jaco_Position.Actuators;
00332 }
00333 
00337 void JacoComm::GetPosition(JacoPose &position)
00338 {
00339         boost::recursive_mutex::scoped_lock lock(api_mutex);
00340         CartesianPosition Jaco_Position;
00341 
00342         memset(&Jaco_Position, 0, sizeof(Jaco_Position)); //zero structure
00343 
00344         API->GetCartesianPosition(Jaco_Position);
00345 
00346         position = JacoPose(Jaco_Position.Coordinates);
00347 }
00348 
00352 void JacoComm::GetFingers(FingerAngles &fingers)
00353 {
00354         boost::recursive_mutex::scoped_lock lock(api_mutex);
00355         CartesianPosition Jaco_Position;
00356 
00357         API->GetCartesianPosition(Jaco_Position);
00358 
00359         fingers = Jaco_Position.Fingers;
00360 }
00361 
00365 void JacoComm::GetConfig(ClientConfigurations &config)
00366 {
00367         boost::recursive_mutex::scoped_lock lock(api_mutex);
00368         memset(&config, 0, sizeof(config)); //zero structure
00369         API->GetClientConfigurations(config);
00370 }
00371 
00375 void JacoComm::PrintAngles(JacoAngles &angles)
00376 {
00377         ROS_INFO("Jaco Arm Angles (Degrees)");
00378         ROS_INFO("Joint 1 = %f", angles.Actuator1);
00379         ROS_INFO("Joint 2 = %f", angles.Actuator2);
00380         ROS_INFO("Joint 3 = %f", angles.Actuator3);
00381 
00382         ROS_INFO("Joint 4 = %f", angles.Actuator4);
00383         ROS_INFO("Joint 5 = %f", angles.Actuator5);
00384         ROS_INFO("Joint 6 = %f", angles.Actuator6);
00385 }
00386 
00390 void JacoComm::PrintPosition(JacoPose &position)
00391 {
00392         ROS_INFO("Jaco Arm Position (Meters)");
00393         ROS_INFO("X = %f", position.X);
00394         ROS_INFO("Y = %f", position.Y);
00395         ROS_INFO("Z = %f", position.Z);
00396 
00397         ROS_INFO("Jaco Arm Rotations (Radians)");
00398         ROS_INFO("Theta X = %f", position.ThetaX);
00399         ROS_INFO("Theta Y = %f", position.ThetaY);
00400         ROS_INFO("Theta Z = %f", position.ThetaZ);
00401 }
00402 
00406 void JacoComm::PrintFingers(FingersPosition fingers)
00407 {
00408         ROS_INFO("Jaco Arm Finger Positions");
00409         ROS_INFO("Finger 1 = %f", fingers.Finger1);
00410         ROS_INFO("Finger 2 = %f", fingers.Finger2);
00411         ROS_INFO("Finger 3 = %f", fingers.Finger3);
00412 }
00413 
00417 void JacoComm::PrintConfig(ClientConfigurations config)
00418 {
00419         ROS_INFO("Jaco Config");
00420         ROS_INFO("ClientID = %s", config.ClientID);
00421         ROS_INFO("ClientName = %s", config.ClientName);
00422         ROS_INFO("Organization = %s", config.Organization);
00423         ROS_INFO("Serial = %s", config.Serial);
00424         ROS_INFO("Model = %s", config.Model);
00425         ROS_INFO("MaxLinearSpeed = %f", config.MaxLinearSpeed);
00426         ROS_INFO("MaxAngularSpeed = %f", config.MaxAngularSpeed);
00427         ROS_INFO("MaxLinearAcceleration = %f", config.MaxLinearAcceleration);
00428         ROS_INFO("MaxForce = %f", config.MaxForce);
00429         ROS_INFO("Sensibility = %f", config.Sensibility);
00430         ROS_INFO("DrinkingHeight = %f", config.DrinkingHeight);
00431         ROS_INFO("ComplexRetractActive = %d", config.ComplexRetractActive);
00432         ROS_INFO("RetractedPositionAngle = %f", config.RetractedPositionAngle);
00433         ROS_INFO("RetractedPositionCount = %d", config.RetractedPositionCount);
00434         ROS_INFO("DrinkingDistance = %f", config.DrinkingDistance);
00435         ROS_INFO("Fingers2and3Inverted = %d", config.Fingers2and3Inverted);
00436         ROS_INFO("DrinkingLength = %f", config.DrinkingLenght);
00437         ROS_INFO("DeletePreProgrammedPositionsAtRetract = %d", config.DeletePreProgrammedPositionsAtRetract);
00438         ROS_INFO("EnableFlashErrorLog = %d", config.EnableFlashErrorLog);
00439         ROS_INFO("EnableFlashPositionLog = %d", config.EnableFlashPositionLog);
00440 }
00441 
00442 void JacoComm::Stop()
00443 {
00444         boost::recursive_mutex::scoped_lock lock(api_mutex);
00445         software_stop = true;
00446 
00447         API->StartControlAPI();
00448 
00449         JoystickCommand home_command;
00450         memset(&home_command, 0, sizeof(home_command)); //zero structure
00451 
00452         home_command.ButtonValue[2] = 1;
00453         API->SendJoystickCommand(home_command);
00454         API->EraseAllTrajectories();
00455         ros::Duration(0.05).sleep();
00456 
00457         home_command.ButtonValue[2] = 0;
00458         API->SendJoystickCommand(home_command);
00459 }
00460 
00461 void JacoComm::Start()
00462 {
00463         boost::recursive_mutex::scoped_lock lock(api_mutex);
00464         software_stop = false;
00465 
00466         API->StartControlAPI();
00467 
00468 }
00469 
00470 bool JacoComm::Stopped()
00471 {
00472         return software_stop;
00473 }
00474 
00480 void JacoComm::WaitForHome(int timeout)
00481 {
00482         double start_secs;
00483         double current_sec;
00484 
00485         //If ros is still running use rostime, else use system time
00486         if (ros::ok())
00487         {
00488                 start_secs = ros::Time::now().toSec();
00489                 current_sec = ros::Time::now().toSec();
00490         } 
00491         else
00492         {
00493                 start_secs = (double) time(NULL);
00494                 current_sec = (double) time(NULL);
00495         }
00496 
00497         //while we have not timed out
00498         while ((current_sec - start_secs) < timeout)
00499         {
00500                 ros::Duration(0.5).sleep();
00501                 
00502                 //If ros is still running use rostime, else use system time
00503                 if (ros::ok())
00504                 {
00505                         current_sec = ros::Time::now().toSec();
00506             ros::spinOnce();
00507                 } 
00508                 else
00509                 {
00510                         current_sec = (double) time(NULL);
00511                 }
00512 
00513                 if (HomeState())
00514                 {
00515                         ros::Duration(1.0).sleep();  // Grants a bit more time for the arm to "settle"
00516                         return;
00517                 }
00518         }
00519 
00520         ROS_WARN("Timed out waiting for arm to return \"home\"");
00521 }
00522 
00523 } // namespace jaco


jaco_driver
Author(s): Jeff Schmidt (Clearpath), Alex Bencz (Clearpath), Matt DeDonato (WPI)
autogenerated on Mon Jan 6 2014 11:23:43