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.
vpMatrix pseudoInverse(double svThreshold=1e-6) 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.
void get_fMw(const vpColVector &q, vpHomogeneousMatrix &fMw) const
Initialize the position controller.
error that can be emited by ViSP classes.
vpHomogeneousMatrix inverse() const
unsigned int size() const
Return the number of elements of the 2D array.
void setPosition(const vpRobot::vpControlFrameType frame, const vpColVector &position)
void extract(vpRotationMatrix &R) const
VISP_EXPORT double measureTimeMs()
Implementation of a rotation matrix and operations on such kind of matrices.
vpHomogeneousMatrix get_fMc(const vpColVector &q) const
vpRobot::vpRobotStateType setRobotState(vpRobot::vpRobotStateType newState)
Initialize the velocity controller.
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)
const char * getMessage(void) const
vpTranslationVector getTranslationVector() const
void get_fMe(const vpColVector &q, vpHomogeneousMatrix &fMe) const
Implementation of column vector and the associated operations.
Implementation of a pose vector and operations on poses.
Implementation of a rotation vector as Euler angle minimal representation.
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.
vpRotationMatrix getRotationMatrix() const
void get_eJe(vpMatrix &eJe)
void getPosition(const vpRobot::vpControlFrameType frame, vpColVector &position)