search

lagadic / visp

Open Source Visual Servoing Platform
https://visp.inria.fr/
GNU General Public License v2.0
709 stars 287 forks source link

How to transform the velocity computed in IBVS from \thetaU to Quaternion #272

Closed TokyoClod closed 6 years ago

TokyoClod commented 6 years ago

Form the function vpColVector vpServo::computeControlLaw() I get the velocity in camera frame in the form of thetaU
I am wondering how to transform the velocity from thetaU form to Quaternion form so I can use it in ros.

fspindle commented 6 years ago

The vpColVector returned by vpServo::computeControlLaw() is a velocity twist (3 translations velocities vx,vy,y, + 3 rotations velocities wx,wy,wz) corresponding to the camera velocity.

In ROS you can convert this data structure:

geometry_msgs::TwistStampedConstPtr msg;
servo.setServo(vpServo::EYEINHAND_CAMERA);
...
vpColVector vc = servo.computeControlLaw();
msg->twist.linear.x = vc[0];
msg->twist.linear.y = vc[1];
msg->twist.linear.z = vc[2];

msg->twist.angular.x = vc[3];
msg->twist.angular.y = vc[4];
msg->twist.angular.z = vc[5];
TokyoClod commented 6 years ago

@fspindle From the description in http://visp-doc.inria.fr/doxygen/visp-daily/classvpSimulatorCamera.html#a19368e64873f93c807a1bfe0c6f102df I think the form of rotations velocities is vpThetaUVector, so I am wondering if it is right just pass the vc[3]...vc[5] to geometry_msgs::Twist

fspindle commented 6 years ago

The doc http://visp-doc.inria.fr/doxygen/visp-daily/classvpSimulatorCamera.html#a19368e64873f93c807a1bfe0c6f102df is completely wrong. I will fix it.

vpThetaUVectorrepresents an angular position, nor a velocity. You may do as described in my previous post.