42 #include <visp/vpConfig.h>
44 #ifdef VISP_HAVE_VIPER850
48 #include <sys/types.h>
51 #include <visp/vpRobotException.h>
52 #include <visp/vpExponentialMap.h>
53 #include <visp/vpDebug.h>
54 #include <visp/vpVelocityTwistMatrix.h>
55 #include <visp/vpThetaUVector.h>
56 #include <visp/vpRobot.h>
57 #include <visp/vpRobotViper850.h>
63 bool vpRobotViper850::robotAlreadyCreated =
false;
85 void emergencyStopViper850(
int signo)
87 std::cout <<
"Stop the Viper850 application by signal ("
88 << signo <<
"): " << (char)7 ;
92 std::cout <<
"SIGINT (stop by ^C) " << std::endl ; break ;
94 std::cout <<
"SIGBUS (stop due to a bus error) " << std::endl ; break ;
96 std::cout <<
"SIGSEGV (stop due to a segmentation fault) " << std::endl ; break ;
98 std::cout <<
"SIGKILL (stop by CTRL \\) " << std::endl ; break ;
100 std::cout <<
"SIGQUIT " << std::endl ; break ;
102 std::cout << signo << std::endl ;
106 PrimitiveSTOP_Viper850();
107 std::cout <<
"Robot was stopped\n";
112 fprintf(stdout,
"Application ");
114 kill(getpid(), SIGKILL);
177 vpRobotViper850::vpRobotViper850 (
bool verbose)
202 signal(SIGINT, emergencyStopViper850);
203 signal(SIGBUS, emergencyStopViper850) ;
204 signal(SIGSEGV, emergencyStopViper850) ;
205 signal(SIGKILL, emergencyStopViper850);
206 signal(SIGQUIT, emergencyStopViper850);
210 std::cout <<
"Open communication with MotionBlox.\n";
221 vpRobotViper850::robotAlreadyCreated =
true;
258 time_prev_getvel = 0;
259 first_time_getvel =
true;
264 first_time_getdis =
true;
268 Try( InitializeConnection(
verbose_) );
271 Try( InitializeNode_Viper850() );
273 Try( PrimitiveRESET_Viper850() );
279 UInt32 HIPowerStatus;
281 Try( PrimitiveSTATUS_Viper850(NULL, NULL, &EStopStatus, NULL, NULL, NULL,
287 std::cout <<
"Robot status: ";
288 switch(EStopStatus) {
291 if (HIPowerStatus == 0)
292 std::cout <<
"Power is OFF" << std::endl;
294 std::cout <<
"Power is ON" << std::endl;
299 if (HIPowerStatus == 0)
300 std::cout <<
"Power is OFF" << std::endl;
302 std::cout <<
"Power is ON" << std::endl;
304 case ESTOP_ACTIVATED:
306 std::cout <<
"Emergency stop is activated" << std::endl;
309 std::cout <<
"Sorry there is an error on the emergency chain." << std::endl;
310 std::cout <<
"You have to call Adept for maintenance..." << std::endl;
313 std::cout << std::endl;
330 if (TryStt == -20001)
331 printf(
"No connection detected. Check if the robot is powered on \n"
332 "and if the firewire link exist between the MotionBlox and this computer.\n");
333 else if (TryStt == -675)
334 printf(
" Timeout enabling power...\n");
338 PrimitivePOWEROFF_Viper850();
340 ShutDownConnection();
342 std::cout <<
"Cannot open connexion with the motionblox..." << std::endl;
344 "Cannot open connexion with the motionblox");
418 for (
unsigned int i=0; i < 3; i ++) {
419 eMc_pose[i] =
etc[i];
420 eMc_pose[i+3] =
erc[i];
423 Try( PrimitiveCONST_Viper850(eMc_pose) );
458 UInt32 HIPowerStatus;
459 Try( PrimitiveSTATUS_Viper850(NULL, NULL, NULL, NULL, NULL, NULL,
471 ShutDownConnection();
473 vpRobotViper850::robotAlreadyCreated =
false;
497 Try( PrimitiveSTOP_Viper850() );
503 std::cout <<
"Change the control mode from velocity to position control.\n";
504 Try( PrimitiveSTOP_Viper850() );
514 std::cout <<
"Change the control mode from stop to velocity control.\n";
547 Try( PrimitiveSTOP_Viper850() );
554 "Cannot stop robot motion.");
573 UInt32 HIPowerStatus;
575 bool firsttime =
true;
576 unsigned int nitermax = 10;
578 for (
unsigned int i=0; i<nitermax; i++) {
579 Try( PrimitiveSTATUS_Viper850(NULL, NULL, &EStopStatus, NULL, NULL, NULL,
581 if (EStopStatus == ESTOP_AUTO) {
585 else if (EStopStatus == ESTOP_MANUAL) {
589 else if (EStopStatus == ESTOP_ACTIVATED) {
592 std::cout <<
"Emergency stop is activated! \n"
593 <<
"Check the emergency stop button and push the yellow button before continuing." << std::endl;
596 fprintf(stdout,
"Remaining time %ds \r", nitermax-i);
601 std::cout <<
"Sorry there is an error on the emergency chain." << std::endl;
602 std::cout <<
"You have to call Adept for maintenance..." << std::endl;
604 ShutDownConnection();
609 if (EStopStatus == ESTOP_ACTIVATED)
610 std::cout << std::endl;
612 if (EStopStatus == ESTOP_ACTIVATED) {
613 std::cout <<
"Sorry, cannot power on the robot." << std::endl;
615 "Cannot power on the robot.");
618 if (HIPowerStatus == 0) {
619 fprintf(stdout,
"Power ON the Viper850 robot\n");
622 Try( PrimitivePOWERON_Viper850() );
629 "Cannot power off the robot.");
648 UInt32 HIPowerStatus;
649 Try( PrimitiveSTATUS_Viper850(NULL, NULL, NULL, NULL, NULL, NULL,
653 if (HIPowerStatus == 1) {
654 fprintf(stdout,
"Power OFF the Viper850 robot\n");
657 Try( PrimitivePOWEROFF_Viper850() );
664 "Cannot power off the robot.");
685 UInt32 HIPowerStatus;
686 Try( PrimitiveSTATUS_Viper850(NULL, NULL, NULL, NULL, NULL, NULL,
690 if (HIPowerStatus == 1) {
698 "Cannot get the power status.");
758 Try( PrimitiveACQ_POS_J_Viper850(position, ×tamp) );
762 for (
unsigned int i=0; i <
njoint; i++)
795 Try( PrimitiveACQ_POS_Viper850(position, ×tamp) );
799 for (
unsigned int i=0; i <
njoint; i++)
853 positioningVelocity = velocity;
864 return positioningVelocity;
953 "Modification of the robot state");
965 Try( PrimitiveACQ_POS_Viper850(q.
data, ×tamp) );
977 for (
unsigned int i=0; i < 3; i++) {
978 txyz[i] = position[i];
979 rxyz[i] = position[i+3];
995 Try( PrimitiveMOVE_J_Viper850(destination.
data, positioningVelocity) );
996 Try( WaitState_Viper850(ETAT_ATTENTE_AFMA6, 1000) );
1006 destination = position;
1011 Try( PrimitiveMOVE_J_Viper850(destination.
data, positioningVelocity) );
1012 Try( WaitState_Viper850(ETAT_ATTENTE_AFMA6, 1000) );
1018 vpRxyzVector rxyz(position[3],position[4],position[5]);
1022 for (
unsigned int i=0; i <3; i++) {
1023 destination[i] = position[i];
1026 int configuration = 0;
1029 Try( PrimitiveMOVE_C_Viper850(destination.
data, configuration,
1030 positioningVelocity) );
1031 Try( WaitState_Viper850(ETAT_ATTENTE_AFMA6, 1000) );
1037 vpERROR_TRACE (
"Positionning error. Mixt frame not implemented");
1039 "Positionning error: "
1040 "Mixt frame not implemented.");
1046 if (TryStt == InvalidPosition || TryStt == -1023)
1047 std::cout <<
" : Position out of range.\n";
1048 else if (TryStt == -3019) {
1050 std::cout <<
" : Joint position out of range.\n";
1052 std::cout <<
" : Cartesian position leads to a joint position out of range.\n";
1054 else if (TryStt < 0)
1055 std::cout <<
" : Unknown error (see Fabien).\n";
1056 else if (error == -1)
1057 std::cout <<
"Position out of range.\n";
1059 if (TryStt < 0 || error < 0) {
1062 "Position out of range.");
1143 position[0] = pos1 ;
1144 position[1] = pos2 ;
1145 position[2] = pos3 ;
1146 position[3] = pos4 ;
1147 position[4] = pos5 ;
1148 position[5] = pos6 ;
1208 "Bad position in filename.");
1296 Try( PrimitiveACQ_POS_J_Viper850(position.
data, ×tamp) );
1303 Try( PrimitiveACQ_POS_C_Viper850(position.
data, ×tamp) );
1307 for (
unsigned int i=3; i <6; i++)
1310 vpRzyzVector rzyz(position[3], position[4], position[5]);
1315 for (
unsigned int i=0; i <3; i++)
1316 position[i+3] = rxyz[i];
1326 vpERROR_TRACE (
"Cannot get position in mixt frame: not implemented");
1328 "Cannot get position in mixt frame: "
1338 "Cannot get position.");
1350 PrimitiveACQ_TIME_Viper850(×tamp);
1388 for (
unsigned int j=0;j<3;j++)
1389 RxyzVect[j]=posRxyz[j+3];
1394 for (
unsigned int j=0;j<3;j++)
1396 position[j]=posRxyz[j];
1397 position[j+3]=RtuVect[j];
1418 for (
unsigned int j=0;j<3;j++)
1419 RxyzVect[j]=posRxyz[j+3];
1424 for (
unsigned int j=0;j<3;j++)
1426 position[j]=posRxyz[j];
1427 position[j+3]=RtuVect[j];
1508 "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
1510 "Cannot send a velocity to the robot "
1511 "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
1524 for (
unsigned int i=0; i<3; i++)
1526 for (
unsigned int i=3; i<6; i++)
1537 for (
unsigned int i=0; i<6; i++)
1550 Try( PrimitiveMOVESPEED_CART_Viper850(vel_sat.
data, REPCAM_VIPER850) );
1558 Try( PrimitiveMOVESPEED_Viper850(vel_sat.
data) );
1563 std::cout <<
"Vitesse ref appliquee: " << vel_sat.
t();
1564 Try( PrimitiveMOVESPEED_CART_Viper850(vel_sat.
data, REPFIX_VIPER850) );
1573 "Case not taken in account.");
1580 if (TryStt == VelStopOnJoint) {
1581 UInt32 axisInJoint[
njoint];
1582 PrimitiveSTATUS_Viper850(NULL, NULL, NULL, NULL, NULL, axisInJoint, NULL);
1583 for (
unsigned int i=0; i <
njoint; i ++) {
1585 std::cout <<
"\nWarning: Velocity control stopped: axis "
1586 << i+1 <<
" on joint limit!" <<std::endl;
1590 printf(
"\n%s(%d): Error %d", __FUNCTION__, TryLine, TryStt);
1591 if (TryString != NULL) {
1593 printf(
" Error sentence %s\n", TryString);
1685 Try( PrimitiveACQ_POS_J_Viper850(q_cur.
data, ×tamp) );
1686 time_cur = timestamp;
1692 if ( ! first_time_getvel ) {
1697 cMc = fMc_prev_getvel.
inverse() * fMc_cur;
1706 velocity = (q_cur - q_prev_getvel)
1707 / (time_cur - time_prev_getvel);
1713 cMc = fMc_prev_getvel.
inverse() * fMc_cur;
1728 cMc = fMc_prev_getvel.
inverse() * fMc_cur;
1736 for (
unsigned int i=0; i < 3; i++) {
1738 velocity[i] = fMc_prev_getvel[i][3] - fMc_cur[i][3];
1740 velocity[i+3] = thetaU[i];
1744 velocity /= (time_cur - time_prev_getvel);
1750 first_time_getvel =
false;
1754 fMc_prev_getvel = fMc_cur;
1757 q_prev_getvel = q_cur;
1760 time_prev_getvel = time_cur;
1767 "Cannot get velocity.");
1931 fd = fopen(filename,
"r") ;
1935 char line[FILENAME_MAX];
1936 char dummy[FILENAME_MAX];
1938 bool sortie =
false;
1942 if (fgets (line, FILENAME_MAX, fd) != NULL) {
1943 if ( strncmp (line,
"#", 1) != 0) {
1945 if ( strncmp (line, head,
sizeof(head)-1) == 0) {
1957 while ( sortie !=
true );
1961 sscanf(line,
"%s %lf %lf %lf %lf %lf %lf",
1963 &q[0], &q[1], &q[2],
1964 &q[3], &q[4], &q[5]);
2001 fd = fopen(filename,
"w") ;
2006 #Viper - Position - Version 1.0\n\
2009 # Joint position in degrees\n\
2014 fprintf(fd,
"R: %lf %lf %lf %lf %lf %lf\n",
2066 vpRobotViper850::getCameraDisplacement(
vpColVector &displacement)
2082 vpRobotViper850::getArticularDisplacement(
vpColVector &displacement)
2121 Try( PrimitiveACQ_POS_Viper850(q, ×tamp) );
2122 for (
unsigned int i=0; i <
njoint; i ++) {
2126 if ( ! first_time_getdis ) {
2129 std::cout <<
"getDisplacement() CAMERA_FRAME not implemented\n";
2135 displacement = q_cur - q_prev_getdis;
2140 std::cout <<
"getDisplacement() REFERENCE_FRAME not implemented\n";
2146 std::cout <<
"getDisplacement() MIXT_FRAME not implemented\n";
2153 first_time_getdis =
false;
2157 q_prev_getdis = q_cur;
2163 "Cannot get velocity.");
2185 Try( PrimitiveTFS_BIAS_Viper850() );
2194 "Cannot bias the force/torque sensor.");
2244 Try( PrimitiveTFS_ACQ_Viper850(H.
data) );
2250 "Cannot get force/torque measures.");
2263 Try( PrimitiveGripper_Viper850(1) );
2264 std::cout <<
"Open the gripper..." << std::endl;
2269 "Cannot open the gripper.");
2283 Try( PrimitiveGripper_Viper850(0) );
2284 std::cout <<
"Close the gripper..." << std::endl;
2289 "Cannot close the gripper.");
void getPosition(const vpRobot::vpControlFrameType frame, vpColVector &position)
static vpColVector inverse(const vpHomogeneousMatrix &M)
Definition of the vpMatrix class.
double getPositioningVelocity(void)
Error that can be emited by the vpRobot class and its derivates.
static const vpToolType defaultTool
Default tool attached to the robot end effector.
The class provides a data structure for the homogeneous matrices as well as a set of operations on th...
void get_eJe(const vpColVector &q, vpMatrix &eJe)
void setVerbose(bool verbose)
virtual vpRobotStateType getRobotState(void)
void setPositioningVelocity(const double velocity)
double getMaxTranslationVelocity(void) const
Initialize the position controller.
vpRobot::vpRobotStateType setRobotState(vpRobot::vpRobotStateType newState)
class that defines a generic virtual robot
static vpColVector saturateVelocities(const vpColVector &v_in, const vpColVector &v_max, bool verbose=false)
void get_eJe(vpMatrix &eJe)
static int wait(double t0, double t)
static bool savePosFile(const char *filename, const vpColVector &q)
double getMaxRotationVelocity(void) const
vpThetaUVector buildFrom(const vpHomogeneousMatrix &M)
virtual vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState)
unsigned int getInverseKinematics(const vpHomogeneousMatrix &fMc, vpColVector &q)
The vpRotationMatrix considers the particular case of a rotation matrix.
void getDisplacement(vpRobot::vpControlFrameType frame, vpColVector &displacement)
double * data
address of the first element of the data array
static bool readPosFile(const char *filename, vpColVector &q)
Initialize the velocity controller.
void setToolType(vpViper850::vpToolType tool)
Set the current tool type.
vpVelocityTwistMatrix buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
void getForceTorque(vpColVector &H)
vpCameraParametersProjType
vpRowVector t() const
transpose of Vector
vpToolType
List of possible tools that can be attached to the robot end-effector.
Modelisation of the ADEPT Viper 850 robot.
void extract(vpRotationMatrix &R) const
Class that consider the particular case of twist transformation matrix that allows to transform a vel...
void get_fJe(vpMatrix &fJe)
static double rad(double deg)
void get_fJe(const vpColVector &q, vpMatrix &fJe)
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &velocity)
void biasForceTorqueSensor()
static double deg(double rad)
void getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &velocity)
Class that provides a data structure for the column vectors as well as a set of operations on these v...
Manual control mode activated when the dead man switch is in use.
static const double defaultPositioningVelocity
void setPosition(const vpRobot::vpControlFrameType frame, const vpColVector &position)
void get_cVe(vpVelocityTwistMatrix &cVe)
The pose is a complete representation of every rigid motion in the euclidian space.
vpHomogeneousMatrix inverse() const
void get_cMe(vpHomogeneousMatrix &cMe)
Class that consider the case of the Euler angle using the x-y-z convention, where are respectively ...
Emergency stop activated.
Class that consider the case of the Euler angles using the z-y-z convention, where are respectively...
virtual ~vpRobotViper850(void)
Automatic control mode (default).
Class that consider the case of a translation vector.
Class that consider the case of the parameterization for the rotation.
void move(const char *filename)
void get_cMe(vpHomogeneousMatrix &cMe)
static const unsigned int njoint
Number of joint.
vpHomogeneousMatrix get_fMc(const vpColVector &q)
void resize(const unsigned int i, const bool flagNullify=true)