46 #include <visp3/robot/vpRobotViper650.h> 48 #ifdef VISP_HAVE_VIPER650 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 650 robot" << std::endl;
125 std::cout <<
"q: " << q.
t() << std::endl;
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;
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;
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;
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 Viper650 robot controller is not available." << std::endl;
Implementation of a matrix and operations on matrices.
void get_fMw(const vpColVector &q, vpHomogeneousMatrix &fMw) const
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &velocity)
Implementation of an homogeneous matrix and operations on such kind of matrices.
Control of Irisa's Viper S650 robot named Viper650.
Initialize the position controller.
error that can be emited by ViSP classes.
unsigned int size() const
Return the number of elements of the 2D array.
void setPosition(const vpRobot::vpControlFrameType frame, const vpColVector &position)
VISP_EXPORT double measureTimeMs()
Implementation of a rotation matrix and operations on such kind of matrices.
vpRobot::vpRobotStateType setRobotState(vpRobot::vpRobotStateType newState)
Initialize the velocity controller.
vpRotationMatrix getRotationMatrix() const
vpTranslationVector getTranslationVector() const
void extract(vpRotationMatrix &R) const
vpHomogeneousMatrix get_fMc(const vpColVector &q) const
void get_fMe(const vpColVector &q, vpHomogeneousMatrix &fMe) const
unsigned int getInverseKinematics(const vpHomogeneousMatrix &fMc, vpColVector &q, const bool &verbose=false) const
void buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
static double rad(double deg)
Stops robot motion especially in velocity and acceleration control.
Implementation of column vector and the associated operations.
Implementation of a pose vector and operations on poses.
vpHomogeneousMatrix inverse() const
Implementation of a rotation vector as Euler angle minimal representation.
vpMatrix pseudoInverse(double svThreshold=1e-6) const
const char * getMessage() const
void get_cMe(vpHomogeneousMatrix &cMe) const
Class that consider the case of a translation vector.
Implementation of a rotation vector as axis-angle minimal representation.
void get_eJe(vpMatrix &eJe)
void getPosition(const vpRobot::vpControlFrameType frame, vpColVector &position)