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;
unsigned int size() const
Return the number of elements of the 2D array.
Implementation of column vector and the associated operations.
error that can be emited by ViSP classes.
const char * getMessage() const
Implementation of an homogeneous matrix and operations on such kind of matrices.
vpRotationMatrix getRotationMatrix() const
vpHomogeneousMatrix inverse() const
vpTranslationVector getTranslationVector() const
void buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
void extract(vpRotationMatrix &R) const
static double rad(double deg)
Implementation of a matrix and operations on matrices.
vpMatrix pseudoInverse(double svThreshold=1e-6) const
Implementation of a pose vector and operations on poses.
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
void get_eJe(vpMatrix &eJe)
@ STATE_POSITION_CONTROL
Initialize the position controller.
@ STATE_VELOCITY_CONTROL
Initialize the velocity controller.
@ STATE_STOP
Stops robot motion especially in velocity and acceleration control.
virtual vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState)
Implementation of a rotation matrix and operations on such kind of matrices.
Implementation of a rotation vector as Euler angle minimal representation.
Implementation of a rotation vector as axis-angle minimal representation.
Class that consider the case of a translation vector.
vpHomogeneousMatrix get_cMe() const
VISP_EXPORT double measureTimeMs()