Visual Servoing Platform  version 3.3.0
testVirtuoseAfma6.cpp
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2019 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2 of the License, or
9  * (at your option) any later version.
10  * See the file LICENSE.txt at the root directory of this source
11  * distribution for additional information about the GNU GPL.
12  *
13  * For using ViSP with software that can not be combined with the GNU
14  * GPL, please contact Inria about acquiring a ViSP Professional
15  * Edition License.
16  *
17  * See http://visp.inria.fr for more information.
18  *
19  * This software was developed at:
20  * Inria Rennes - Bretagne Atlantique
21  * Campus Universitaire de Beaulieu
22  * 35042 Rennes Cedex
23  * France
24  *
25  * If you have questions regarding the use of this file, please contact
26  * Inria at visp@inria.fr
27  *
28  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
29  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
30  *
31  * Description:
32  * Test for Virtuose SDK wrapper.
33  *
34  * Authors:
35  * Fabien Spindler
36  * Nicolò Pedemonte
37  *
38  *****************************************************************************/
39 
48 #include <visp3/core/vpTime.h>
49 #include <visp3/robot/vpRobotAfma6.h>
50 #include <visp3/robot/vpVirtuose.h>
51 
52 int main()
53 {
54 #if defined(VISP_HAVE_VIRTUOSE) && defined(VISP_HAVE_AFMA6)
55  vpRobotAfma6 robot;
56  try {
57  vpVirtuose virtuose;
58  virtuose.setVerbose(true);
59  virtuose.setCommandType(COMMAND_TYPE_IMPEDANCE);
60  virtuose.setPowerOn();
61  // virtuose.setSaturation(1.0f,0.0f);
62 
63  vpColVector virt_velocity;
64  vpColVector robot_velocity;
65  vpColVector robot_joint_position;
66  vpColVector robot_cart_position;
67  vpColVector robot_cart_position_init;
68  vpColVector force_feedback_robot(3);
69  float force_limit = 15;
70  int force_increase_rate = 500;
71 
72  double cube_size = 0.15;
73 
75  rMv[0][0] = rMv[0][2] = 0;
76  rMv[1][1] = rMv[1][2] = 0;
77  rMv[2][0] = rMv[2][1] = 0;
78  rMv[0][1] = rMv[1][0] = rMv[2][2] = -1;
79  std::cout << "rMv:\n" << rMv << std::endl;
80  vpVelocityTwistMatrix rVv(rMv);
81 
82  // Set the extrinsic camera parameters obtained with a perpective
83  // projection model including a distortion parameter
85  // Initialize the controller to position control
87  // Moves the robot in the joint space
88  vpColVector q(6, 0);
89  robot.setPositioningVelocity(10);
91 
92  robot.getPosition(vpRobot::REFERENCE_FRAME, robot_cart_position_init);
93  vpColVector min(3), max(3);
94  for (unsigned int i = 0; i < 3; i++) {
95  min[i] = robot_cart_position_init[i] - cube_size / 2;
96  max[i] = robot_cart_position_init[i] + cube_size / 2;
97  }
98  std::cout << "min: " << min.t() << std::endl;
99  std::cout << "max: " << max.t() << std::endl;
100 
101  // Initialize the controller to position control
103 
104  for (unsigned int iter = 0; iter < 10000; iter++) {
105  virt_velocity = virtuose.getVelocity();
106  std::cout << "Virtuose velocity: " << virt_velocity.t() << std::endl;
107 
108  robot.getPosition(vpRobot::REFERENCE_FRAME, robot_cart_position);
109 
110  for (int i = 0; i < 3; i++) {
111  if (robot_cart_position[i] >= max[i]) {
112  force_feedback_robot[i] = (max[i] - robot_cart_position[i]) * force_increase_rate;
113  if (force_feedback_robot[i] <= -force_limit)
114  force_feedback_robot[i] = -force_limit;
115  } else if (robot_cart_position[i] <= min[i]) {
116  force_feedback_robot[i] = (min[i] - robot_cart_position[i]) * force_increase_rate;
117  if (force_feedback_robot[i] >= force_limit)
118  force_feedback_robot[i] = force_limit;
119  } else
120  force_feedback_robot[i] = 0;
121  }
122  vpColVector force_feedback_virt = rMv.getRotationMatrix().inverse() * force_feedback_robot;
123 
124  // Printing force feedback
125  std::cout << "Force feedback: " << force_feedback_virt.t() << std::endl;
126 
127  robot_velocity = rVv * virt_velocity;
128  robot.setVelocity(vpRobot::CAMERA_FRAME, robot_velocity);
129 
130  // Set force feedback
131  vpColVector force_feedback(6, 0);
132  force_feedback.insert(0, force_feedback_virt);
133 
134  virtuose.setForce(force_feedback);
135 
136  vpTime::wait(10);
137  }
138  robot.stopMotion();
139  virtuose.setPowerOff();
140  std::cout << "The end" << std::endl;
141  } catch (const vpException &e) {
142  robot.stopMotion();
143  std::cout << "Catch an exception: " << e.getStringMessage() << std::endl;
144  }
145 #else
146  std::cout << "You should install Virtuose SDK to use this binary..." << std::endl;
147 #endif
148 }
vpRobot::STATE_VELOCITY_CONTROL
Initialize the velocity controller.
Definition: vpRobot.h:65
vpRobot::ARTICULAR_FRAME
Definition: vpRobot.h:77
vpException::getStringMessage
const std::string & getStringMessage(void) const
Send a reference (constant) related the error message (can be empty).
Definition: vpException.cpp:91
vpTime::wait
VISP_EXPORT int wait(double t0, double t)
Definition: vpTime.cpp:172
vpVirtuose::getVelocity
vpColVector getVelocity() const
Definition: vpVirtuose.cpp:508
vpColVector::t
vpRowVector t() const
Definition: vpColVector.cpp:781
vpVirtuose::setPowerOn
void setPowerOn()
Definition: vpVirtuose.cpp:913
vpRobotAfma6
Control of Irisa's gantry robot named Afma6.
Definition: vpRobotAfma6.h:210
vpSimulatorCamera::setPosition
void setPosition(const vpHomogeneousMatrix &wMc)
Definition: vpSimulatorCamera.cpp:241
vpVirtuose::setVerbose
void setVerbose(bool mode)
Definition: vpVirtuose.h:195
vpVirtuose::setCommandType
void setCommandType(const VirtCommandType &type)
Definition: vpVirtuose.cpp:708
vpColVector
Implementation of column vector and the associated operations.
Definition: vpColVector.h:129
vpVirtuose
Definition: vpVirtuose.h:142
vpRobot::STATE_POSITION_CONTROL
Initialize the position controller.
Definition: vpRobot.h:66
vpRobot::setRobotState
virtual vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState)
Definition: vpRobot.cpp:200
vpSimulatorCamera::getPosition
vpHomogeneousMatrix getPosition() const
Definition: vpSimulatorCamera.cpp:118
vpVirtuose::setForce
void setForce(const vpColVector &force)
Definition: vpVirtuose.cpp:728
vpVelocityTwistMatrix
Definition: vpVelocityTwistMatrix.h:165
vpRowVector::t
vpColVector t() const
Definition: vpRowVector.cpp:506
vpRobot::CAMERA_FRAME
Definition: vpRobot.h:81
vpRobot::REFERENCE_FRAME
Definition: vpRobot.h:75
vpVirtuose::setPowerOff
void setPowerOff()
Definition: vpVirtuose.cpp:900
vpHomogeneousMatrix
Implementation of an homogeneous matrix and operations on such kind of matrices.
Definition: vpHomogeneousMatrix.h:148
vpAfma6::TOOL_CCMOP
Definition: vpAfma6.h:118
vpSimulatorCamera::setVelocity
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
Definition: vpSimulatorCamera.cpp:197
vpCameraParameters::perspectiveProjWithDistortion
Definition: vpCameraParameters.h:241
vpHomogeneousMatrix::getRotationMatrix
vpRotationMatrix getRotationMatrix() const
Definition: vpHomogeneousMatrix.cpp:794
vpException
error that can be emited by ViSP classes.
Definition: vpException.h:70
vpRotationMatrix::inverse
vpRotationMatrix inverse() const
Definition: vpRotationMatrix.cpp:556