39 #include <visp3/core/vpConfig.h> 41 #ifdef VISP_HAVE_VIPER650 45 #include <sys/types.h> 48 #include <visp3/core/vpDebug.h> 49 #include <visp3/core/vpExponentialMap.h> 50 #include <visp3/core/vpIoTools.h> 51 #include <visp3/core/vpThetaUVector.h> 52 #include <visp3/core/vpVelocityTwistMatrix.h> 53 #include <visp3/robot/vpRobot.h> 54 #include <visp3/robot/vpRobotException.h> 55 #include <visp3/robot/vpRobotViper650.h> 61 bool vpRobotViper650::robotAlreadyCreated =
false;
83 void emergencyStopViper650(
int signo)
85 std::cout <<
"Stop the Viper650 application by signal (" << signo <<
"): " << (char)7;
88 std::cout <<
"SIGINT (stop by ^C) " << std::endl;
91 std::cout <<
"SIGBUS (stop due to a bus error) " << std::endl;
94 std::cout <<
"SIGSEGV (stop due to a segmentation fault) " << std::endl;
97 std::cout <<
"SIGKILL (stop by CTRL \\) " << std::endl;
100 std::cout <<
"SIGQUIT " << std::endl;
103 std::cout << signo << std::endl;
107 PrimitiveSTOP_Viper650();
108 std::cout <<
"Robot was stopped\n";
113 fprintf(stdout,
"Application ");
115 kill(getpid(), SIGKILL);
200 signal(SIGINT, emergencyStopViper650);
201 signal(SIGBUS, emergencyStopViper650);
202 signal(SIGSEGV, emergencyStopViper650);
203 signal(SIGKILL, emergencyStopViper650);
204 signal(SIGQUIT, emergencyStopViper650);
208 std::cout <<
"Open communication with MotionBlox.\n";
220 vpRobotViper650::robotAlreadyCreated =
true;
258 time_prev_getvel = 0;
259 first_time_getvel =
true;
264 first_time_getdis =
true;
267 Try(InitializeConnection(
verbose_));
270 Try(InitializeNode_Viper650());
272 Try(PrimitiveRESET_Viper650());
275 Try(PrimitiveREMOVE_JOINT6_LIMITS_Viper650(0));
281 UInt32 HIPowerStatus;
283 Try(PrimitiveSTATUS_Viper650(NULL, NULL, &EStopStatus, NULL, NULL, NULL, &HIPowerStatus));
288 std::cout <<
"Robot status: ";
289 switch (EStopStatus) {
292 if (HIPowerStatus == 0)
293 std::cout <<
"Power is OFF" << std::endl;
295 std::cout <<
"Power is ON" << std::endl;
300 if (HIPowerStatus == 0)
301 std::cout <<
"Power is OFF" << std::endl;
303 std::cout <<
"Power is ON" << std::endl;
305 case ESTOP_ACTIVATED:
307 std::cout <<
"Emergency stop is activated" << std::endl;
310 std::cout <<
"Sorry there is an error on the emergency chain." << std::endl;
311 std::cout <<
"You have to call Adept for maintenance..." << std::endl;
314 std::cout << std::endl;
332 if (TryStt == -20001)
333 printf(
"No connection detected. Check if the robot is powered on \n" 334 "and if the firewire link exist between the MotionBlox and this " 336 else if (TryStt == -675)
337 printf(
" Timeout enabling power...\n");
341 PrimitivePOWEROFF_Viper650();
343 ShutDownConnection();
345 std::cout <<
"Cannot open connection with the motionblox..." << std::endl;
430 for (
unsigned int i = 0; i < 3; i++) {
431 eMc_pose[i] =
etc[i];
432 eMc_pose[i + 3] =
erc[i];
435 Try(PrimitiveCONST_Viper650(eMc_pose));
510 for (
unsigned int i = 0; i < 3; i++) {
511 eMc_pose[i] =
etc[i];
512 eMc_pose[i + 3] =
erc[i];
515 Try(PrimitiveCONST_Viper650(eMc_pose));
576 for (
unsigned int i = 0; i < 3; i++) {
577 eMc_pose[i] =
etc[i];
578 eMc_pose[i + 3] =
erc[i];
581 Try(PrimitiveCONST_Viper650(eMc_pose));
606 for (
unsigned int i = 0; i < 3; i++) {
607 eMc_pose[i] =
etc[i];
608 eMc_pose[i + 3] =
erc[i];
611 Try(PrimitiveCONST_Viper650(eMc_pose));
638 for (
unsigned int i = 0; i < 3; i++) {
639 eMc_pose[i] =
etc[i];
640 eMc_pose[i + 3] =
erc[i];
643 Try(PrimitiveCONST_Viper650(eMc_pose));
667 UInt32 HIPowerStatus;
668 Try(PrimitiveSTATUS_Viper650(NULL, NULL, NULL, NULL, NULL, NULL, &HIPowerStatus));
679 ShutDownConnection();
681 vpRobotViper650::robotAlreadyCreated =
false;
701 Try(PrimitiveSTOP_Viper650());
708 std::cout <<
"Change the control mode from velocity to position control.\n";
709 Try(PrimitiveSTOP_Viper650());
719 std::cout <<
"Change the control mode from stop to velocity control.\n";
750 Try(PrimitiveSTOP_Viper650());
774 UInt32 HIPowerStatus;
776 bool firsttime =
true;
777 unsigned int nitermax = 10;
779 for (
unsigned int i = 0; i < nitermax; i++) {
780 Try(PrimitiveSTATUS_Viper650(NULL, NULL, &EStopStatus, NULL, NULL, NULL, &HIPowerStatus));
781 if (EStopStatus == ESTOP_AUTO) {
784 }
else if (EStopStatus == ESTOP_MANUAL) {
787 }
else if (EStopStatus == ESTOP_ACTIVATED) {
790 std::cout <<
"Emergency stop is activated! \n" 791 <<
"Check the emergency stop button and push the yellow " 792 "button before continuing." 796 fprintf(stdout,
"Remaining time %us \r", nitermax - i);
800 std::cout <<
"Sorry there is an error on the emergency chain." << std::endl;
801 std::cout <<
"You have to call Adept for maintenance..." << std::endl;
803 ShutDownConnection();
808 if (EStopStatus == ESTOP_ACTIVATED)
809 std::cout << std::endl;
811 if (EStopStatus == ESTOP_ACTIVATED) {
812 std::cout <<
"Sorry, cannot power on the robot." << std::endl;
816 if (HIPowerStatus == 0) {
817 fprintf(stdout,
"Power ON the Viper650 robot\n");
820 Try(PrimitivePOWERON_Viper650());
844 UInt32 HIPowerStatus;
845 Try(PrimitiveSTATUS_Viper650(NULL, NULL, NULL, NULL, NULL, NULL, &HIPowerStatus));
848 if (HIPowerStatus == 1) {
849 fprintf(stdout,
"Power OFF the Viper650 robot\n");
852 Try(PrimitivePOWEROFF_Viper650());
878 UInt32 HIPowerStatus;
879 Try(PrimitiveSTATUS_Viper650(NULL, NULL, NULL, NULL, NULL, NULL, &HIPowerStatus));
882 if (HIPowerStatus == 1) {
942 Try(PrimitiveACQ_POS_J_Viper650(position, ×tamp));
946 for (
unsigned int i = 0; i <
njoint; i++)
975 Try(PrimitiveACQ_POS_Viper650(position, ×tamp));
979 for (
unsigned int i = 0; i <
njoint; i++)
1119 "Modification of the robot state");
1131 Try(PrimitiveACQ_POS_Viper650(q.
data, ×tamp));
1143 for (
unsigned int i = 0; i < 3; i++) {
1144 txyz[i] = position[i];
1145 rxyz[i] = position[i + 3];
1161 Try(PrimitiveMOVE_J_Viper650(destination.
data, positioningVelocity));
1162 Try(WaitState_Viper650(ETAT_ATTENTE_VIPER650, 1000));
1171 destination = position;
1177 Try(PrimitiveMOVE_J_Viper650(destination.
data, positioningVelocity));
1178 Try(WaitState_Viper650(ETAT_ATTENTE_VIPER650, 1000));
1183 vpRxyzVector rxyz(position[3], position[4], position[5]);
1187 for (
unsigned int i = 0; i < 3; i++) {
1188 destination[i] = position[i];
1191 int configuration = 0;
1195 Try(PrimitiveMOVE_C_Viper650(destination.
data, configuration, positioningVelocity));
1196 Try(WaitState_Viper650(ETAT_ATTENTE_VIPER650, 1000));
1202 "Mixt frame not implemented.");
1206 "Mixt frame not implemented.");
1211 if (TryStt == InvalidPosition || TryStt == -1023)
1212 std::cout <<
" : Position out of range.\n";
1214 else if (TryStt == -3019) {
1216 std::cout <<
" : Joint position out of range.\n";
1218 std::cout <<
" : Cartesian position leads to a joint position out of " 1220 }
else if (TryStt < 0)
1221 std::cout <<
" : Unknown error (see Fabien).\n";
1222 else if (error == -1)
1223 std::cout <<
"Position out of range.\n";
1225 if (TryStt < 0 || error < 0) {
1299 const double pos3,
const double pos4,
const double pos5,
const double pos6)
1452 Try(PrimitiveACQ_POS_J_Viper650(position.
data, ×tamp));
1460 Try(PrimitiveACQ_POS_J_Viper650(q.
data, ×tamp));
1471 for (
unsigned int i = 0; i < 3; i++) {
1472 position[i] = fMc[i][3];
1473 position[i + 3] = rxyz[i];
1532 for (
unsigned int j = 0; j < 3; j++)
1533 RxyzVect[j] = posRxyz[j + 3];
1538 for (
unsigned int j = 0; j < 3; j++) {
1539 position[j] = posRxyz[j];
1540 position[j + 3] = RtuVect[j];
1567 PrimitiveACQ_TIME_Viper650(×tamp);
1653 "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
1655 "Cannot send a velocity to the robot " 1656 "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
1670 for (
unsigned int i = 0; i < 3; i++)
1672 for (
unsigned int i = 3; i < 6; i++)
1685 for (
unsigned int i = 0; i < 6; i++)
1688 for (
unsigned int i = 0; i < 5; i++)
1703 Try(PrimitiveMOVESPEED_CART_Viper650(vel_sat.
data, REPCAM_VIPER650));
1713 Try(PrimitiveMOVESPEED_CART_Viper650(v_c.
data, REPCAM_VIPER650));
1721 Try(PrimitiveMOVESPEED_Viper650(vel_sat.
data));
1727 Try(PrimitiveMOVESPEED_CART_Viper650(vel_sat.
data, REPFIX_VIPER650));
1736 "Case not taken in account.");
1743 if (TryStt == VelStopOnJoint) {
1744 UInt32 axisInJoint[
njoint];
1745 PrimitiveSTATUS_Viper650(NULL, NULL, NULL, NULL, NULL, axisInJoint, NULL);
1746 for (
unsigned int i = 0; i <
njoint; i++) {
1748 std::cout <<
"\nWarning: Velocity control stopped: axis " << i + 1 <<
" on joint limit!" << std::endl;
1751 printf(
"\n%s(%d): Error %d", __FUNCTION__, TryLine, TryStt);
1752 if (TryString != NULL) {
1754 printf(
" Error sentence %s\n", TryString);
1838 Try(PrimitiveACQ_POS_J_Viper650(q_cur.
data, ×tamp));
1839 time_cur = timestamp;
1845 if (!first_time_getvel) {
1850 cMc = fMc_prev_getvel.
inverse() * fMc_cur;
1859 velocity = (q_cur - q_prev_getvel) / (time_cur - time_prev_getvel);
1865 cMc = fMc_prev_getvel.
inverse() * fMc_cur;
1880 cMc = fMc_prev_getvel.
inverse() * fMc_cur;
1888 for (
unsigned int i = 0; i < 3; i++) {
1890 velocity[i] = fMc_prev_getvel[i][3] - fMc_cur[i][3];
1892 velocity[i + 3] = thetaU[i];
1896 velocity /= (time_cur - time_prev_getvel);
1901 "vpRobotViper650::getVelocity() not implemented in end-effector"));
1905 first_time_getvel =
false;
1909 fMc_prev_getvel = fMc_cur;
1912 q_prev_getvel = q_cur;
1915 time_prev_getvel = time_cur;
2081 std::ifstream fd(filename.c_str(), std::ios::in);
2083 if (!fd.is_open()) {
2088 std::string key(
"R:");
2089 std::string id(
"#Viper650 - Position");
2090 bool pos_found =
false;
2095 while (std::getline(fd, line)) {
2098 if (!(line.compare(0,
id.size(), id) == 0)) {
2099 std::cout <<
"Error: this position file " << filename <<
" is not for Viper650 robot" << std::endl;
2103 if ((line.compare(0, 1,
"#") == 0)) {
2106 if ((line.compare(0, key.size(), key) == 0)) {
2109 if (chain.size() <
njoint + 1)
2111 if (chain.size() <
njoint + 1)
2114 std::istringstream ss(line);
2117 for (
unsigned int i = 0; i <
njoint; i++)
2130 std::cout <<
"Error: unable to find a position for Viper650 robot in " << filename << std::endl;
2164 fd = fopen(filename.c_str(),
"w");
2169 #Viper650 - Position - Version 1.00\n\ 2172 # Joint position in degrees\n\ 2238 Try(PrimitiveACQ_POS_Viper650(q, ×tamp));
2239 for (
unsigned int i = 0; i <
njoint; i++) {
2243 if (!first_time_getdis) {
2246 std::cout <<
"getDisplacement() CAMERA_FRAME not implemented\n";
2251 displacement = q_cur - q_prev_getdis;
2256 std::cout <<
"getDisplacement() REFERENCE_FRAME not implemented\n";
2261 std::cout <<
"getDisplacement() MIXT_FRAME not implemented\n";
2265 std::cout <<
"getDisplacement() END_EFFECTOR_FRAME not implemented\n";
2270 first_time_getdis =
false;
2274 q_prev_getdis = q_cur;
2300 Try(PrimitiveTFS_BIAS_Viper650());
2358 Try(PrimitiveTFS_ACQ_Viper650(H.
data));
2411 Try(PrimitiveTFS_ACQ_Viper650(H.
data));
2431 Try(PrimitiveREMOVE_JOINT6_LIMITS_Viper650(0));
2432 std::cout <<
"Enable joint limits on axis 6..." << std::endl;
2453 Try(PrimitiveREMOVE_JOINT6_LIMITS_Viper650(1));
2454 std::cout <<
"Warning: Disable joint limits on axis 6..." << std::endl;
2499 maxRotationVelocity_joint6 = w6_max;
2521 Try(PrimitivePneumaticGripper_Viper650(1));
2522 std::cout <<
"Open the pneumatic gripper..." << std::endl;
2539 Try(PrimitivePneumaticGripper_Viper650(0));
2540 std::cout <<
"Close the pneumatic gripper..." << std::endl;
2547 #elif !defined(VISP_BUILD_SHARED_LIBS) 2550 void dummy_vpRobotViper650(){};
static const double defaultPositioningVelocity
static vpColVector inverse(const vpHomogeneousMatrix &M)
Implementation of a matrix and operations on matrices.
vpRxyzVector buildFrom(const vpRotationMatrix &R)
void get_eJe(const vpColVector &q, vpMatrix &eJe) const
VISP_EXPORT int wait(double t0, double t)
Error that can be emited by the vpRobot class and its derivates.
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &velocity)
void getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &velocity)
virtual ~vpRobotViper650(void)
Implementation of an homogeneous matrix and operations on such kind of matrices.
void setVerbose(bool verbose)
void setPositioningVelocity(const double velocity)
vpColVector getForceTorque() const
vpCameraParameters::vpCameraParametersProjType projModel
double maxRotationVelocity
double getMaxTranslationVelocity(void) const
void setMaxRotationVelocityJoint6(double w6_max)
Initialize the position controller.
error that can be emited by ViSP classes.
Class that defines a generic virtual robot.
Automatic control mode (default).
static vpColVector saturateVelocities(const vpColVector &v_in, const vpColVector &v_max, bool verbose=false)
void get_cVe(vpVelocityTwistMatrix &cVe) const
Type * data
Address of the first element of the data array.
void setPosition(const vpRobot::vpControlFrameType frame, const vpColVector &position)
void closeGripper() const
double getMaxRotationVelocity(void) const
static bool savePosFile(const std::string &filename, const vpColVector &q)
vpThetaUVector buildFrom(const vpHomogeneousMatrix &M)
virtual vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState)
Implementation of a rotation matrix and operations on such kind of matrices.
static const vpToolType defaultTool
Default tool attached to the robot end effector.
vpRobot::vpRobotStateType setRobotState(vpRobot::vpRobotStateType newState)
Initialize the velocity controller.
Emergency stop activated.
virtual void set_eMc(const vpHomogeneousMatrix &eMc_)
vpVelocityTwistMatrix buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
vpCameraParametersProjType
VISP_EXPORT void sleepMs(double t)
Modelisation of the ADEPT Viper 650 robot.
void enableJoint6Limits() const
void extract(vpRotationMatrix &R) const
vpHomogeneousMatrix get_fMc(const vpColVector &q) const
double getPositioningVelocity(void) const
vpToolType
List of possible tools that can be attached to the robot end-effector.
unsigned int getInverseKinematics(const vpHomogeneousMatrix &fMc, vpColVector &q, const bool &verbose=false) const
static double rad(double deg)
void get_fJe(vpMatrix &fJe)
void get_fJe(const vpColVector &q, vpMatrix &fJe) const
void setMaxRotationVelocity(const double maxVr)
void get_cMe(vpHomogeneousMatrix &cMe) const
static double deg(double rad)
vpMatrix eJe
robot Jacobian expressed in the end-effector frame
void biasForceTorqueSensor() const
Implementation of column vector and the associated operations.
Implementation of a pose vector and operations on poses.
vpHomogeneousMatrix inverse() const
void move(const std::string &filename)
virtual vpRobotStateType getRobotState(void) const
void disableJoint6Limits() const
double getMaxRotationVelocityJoint6() const
Implementation of a rotation vector as Euler angle minimal representation.
void setMaxRotationVelocity(double w_max)
static bool readPosFile(const std::string &filename, vpColVector &q)
Implementation of a rotation vector as Euler angle minimal representation.
void getDisplacement(vpRobot::vpControlFrameType frame, vpColVector &displacement)
Function not implemented.
void get_cMe(vpHomogeneousMatrix &cMe) const
vpMatrix fJe
robot Jacobian expressed in the robot reference frame available
Class that consider the case of a translation vector.
Implementation of a rotation vector as axis-angle minimal representation.
static const unsigned int njoint
Number of joint.
void get_eJe(vpMatrix &eJe)
bool getPowerState() const
void set_eMc(const vpHomogeneousMatrix &eMc_)
void getPosition(const vpRobot::vpControlFrameType frame, vpColVector &position)
void resize(const unsigned int i, const bool flagNullify=true)