 |
Visual Servoing Platform
version 3.3.0
|
46 #include <visp3/robot/vpRobotViper850.h>
48 #ifdef VISP_HAVE_VIPER850
59 for (
unsigned int i = 0; i < 3; i++) {
60 if (std::fabs(t1[i] - t2[i]) > epsilon)
62 if (std::fabs(tu1[i] - tu2[i]) > epsilon)
70 for (
unsigned int i = 0; i < q1.
size(); i++) {
71 if (std::fabs(q1[i] - q2[i]) > epsilon) {
90 eMt[0][1] = -sqrt(2)/2;
91 eMt[1][1] = -sqrt(2)/2;
94 eMt[0][2] = -sqrt(2)/2;
95 eMt[1][2] = sqrt(2)/2;
107 std::cout <<
"eMt:\n" << eMt << std::endl;
110 std::cout <<
"Connection to Viper 850 robot" << std::endl;
125 std::cout <<
"q: " << q.
t() << std::endl;
128 robot.get_fMw(q, fMw);
129 robot.get_fMe(q, fMe);
130 robot.get_fMc(q, fMt);
133 std::cout <<
"fMw:\n" << fMw << std::endl;
134 std::cout <<
"fMe:\n" << fMe << std::endl;
135 std::cout <<
"fMt:\n" << fMt << std::endl;
136 std::cout <<
"eMc:\n" << cMe.
inverse() << std::endl;
142 std::cout <<
"eMt_:\n" << eMt_ << std::endl;
145 std::cout <<
"Compare pose eMt and eMt_:" << std::endl;
146 if (!pose_equal(eMt, eMt_, 1e-4)) {
147 std::cout <<
" Error: Pose eMt differ" << std::endl;
148 std::cout <<
"\nTest failed" << std::endl;
151 std::cout <<
" They are the same, we can continue" << std::endl;
156 std::cout <<
"eMc:\n" << cMe.
inverse() << std::endl;
158 std::cout <<
"Compare pose eMt and eMc:" << std::endl;
159 if (!pose_equal(eMt, cMe.
inverse(), 1e-4)) {
160 std::cout <<
" Error: Pose eMc differ" << std::endl;
161 std::cout <<
"\nTest failed" << std::endl;
164 std::cout <<
" They are the same, we can continue" << std::endl;
174 for (
unsigned int i = 0; i < 3; i++) {
175 f_t_t[i] = f_pose_t[i];
176 f_rxyz_t[i] = f_pose_t[i + 3];
179 std::cout <<
"fMt_ (from ref frame):\n" << fMt_ << std::endl;
181 std::cout <<
"Compare pose fMt and fMt_:" << std::endl;
182 if (!pose_equal(fMt, fMt_, 1e-4)) {
183 std::cout <<
" Error: Pose fMt differ" << std::endl;
184 std::cout <<
"\nTest failed" << std::endl;
187 std::cout <<
" They are the same, we can continue" << std::endl;
193 robot.getInverseKinematics(fMt, q1);
195 std::cout <<
"Move robot in joint (the robot should not move)" << std::endl;
201 std::cout <<
"Reach joint position q2: " << q2.
t() << std::endl;
203 std::cout <<
"Compare joint position q and q2:" << std::endl;
204 if (!joint_equal(q, q2, 1e-4)) {
205 std::cout <<
" Error: Joint position differ" << std::endl;
206 std::cout <<
"\nTest failed" << std::endl;
209 std::cout <<
" They are the same, we can continue" << std::endl;
217 for (
unsigned int i = 0; i < 3; i++) {
218 f_pose_t[i] = f_t_t[i];
219 f_pose_t[i + 3] = f_rxyz_t[i];
222 std::cout <<
"Move robot in reference frame (the robot should not move)" << std::endl;
226 std::cout <<
"Reach joint position q3: " << q3.
t() << std::endl;
227 std::cout <<
"Compare joint position q and q3:" << std::endl;
228 if (!joint_equal(q, q3, 1e-4)) {
229 std::cout <<
" Error: Joint position differ" << std::endl;
230 std::cout <<
"\nTest failed" << std::endl;
233 std::cout <<
" They are the same, we can continue" << std::endl;
245 robot.getInverseKinematics(fMt_, q);
247 std::cout <<
"fMt_:\n" << fMt_ << std::endl;
249 std::cout <<
"Move robot in joint position to reach fMt_" << std::endl;
258 std::cout <<
"Compare pose fMt_ and fpt_:" << std::endl;
260 std::cout <<
" Error: Pose fMt_ differ" << std::endl;
261 std::cout <<
"\nTest failed" << std::endl;
264 std::cout <<
" They are the same, we can continue" << std::endl;
275 std::cout <<
"Move robot in camera velocity" << std::endl;
297 std::cout <<
"Move robot in joint velocity" << std::endl;
316 std::cout <<
"Move robot in camera velocity" << std::endl;
337 std::cout <<
"Move robot in joint velocity" << std::endl;
354 robot.get_fMc(q, fMt);
361 robot.getInverseKinematics(fMt * tMt, q);
363 std::cout <<
"Move robot in joint position" << std::endl;
367 std::cout <<
"The end" << std::endl;
368 std::cout <<
"Test succeed" << std::endl;
370 std::cout <<
"Test failed with exception: " << e.
getMessage() << std::endl;
377 std::cout <<
"The real Viper850 robot controller is not available." << std::endl;
Initialize the velocity controller.
static double rad(double deg)
void setPosition(const vpHomogeneousMatrix &wMc)
Class that consider the case of a translation vector.
Implementation of a rotation vector as axis-angle minimal representation.
Implementation of column vector and the associated operations.
void extract(vpRotationMatrix &R) const
Implementation of a matrix and operations on matrices.
VISP_EXPORT double measureTimeMs()
Initialize the position controller.
vpTranslationVector getTranslationVector() const
virtual vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState)
vpHomogeneousMatrix getPosition() const
Implementation of a rotation matrix and operations on such kind of matrices.
Implementation of a pose vector and operations on poses.
void buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
void get_eJe(vpMatrix &eJe)
unsigned int size() const
Return the number of elements of the 2D array.
const char * getMessage(void) const
vpMatrix pseudoInverse(double svThreshold=1e-6) const
vpHomogeneousMatrix inverse() const
Implementation of an homogeneous matrix and operations on such kind of matrices.
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
vpRotationMatrix getRotationMatrix() const
error that can be emited by ViSP classes.
Stops robot motion especially in velocity and acceleration control.
Implementation of a rotation vector as Euler angle minimal representation.