#include <vpViper.h>

Inheritance diagram for vpViper:

Public Member Functions
	vpViper ()

virtual	~vpViper ()

vpHomogeneousMatrix	getForwardKinematics (const vpColVector &q)

unsigned int	getInverseKinematicsWrist (const vpHomogeneousMatrix &fMw, vpColVector &q, const bool &verbose=false)

unsigned int	getInverseKinematics (const vpHomogeneousMatrix &fMc, vpColVector &q, const bool &verbose=false)

vpHomogeneousMatrix	get_fMc (const vpColVector &q)

void	get_fMw (const vpColVector &q, vpHomogeneousMatrix &fMw)

void	get_wMe (vpHomogeneousMatrix &wMe)

void	get_eMc (vpHomogeneousMatrix &eMc)

void	get_fMe (const vpColVector &q, vpHomogeneousMatrix &fMe)

void	get_fMc (const vpColVector &q, vpHomogeneousMatrix &fMc)

void	get_cMe (vpHomogeneousMatrix &cMe)

void	get_cVe (vpVelocityTwistMatrix &cVe)

void	get_fJw (const vpColVector &q, vpMatrix &fJw)

void	get_fJe (const vpColVector &q, vpMatrix &fJe)

void	get_eJe (const vpColVector &q, vpMatrix &eJe)

vpColVector	getJointMin ()

vpColVector	getJointMax ()

double	getCoupl56 ()

Static Public Attributes
static const unsigned int	njoint = 6

Protected Attributes
vpHomogeneousMatrix	eMc

vpTranslationVector	etc

vpRxyzVector	erc

double	a1

double	d1

double	a2

double	a3

double	d4

double	d6

double	c56

vpColVector	joint_max

vpColVector	joint_min

Friends
VISP_EXPORT std::ostream &	operator<< (std::ostream &os, const vpViper &viper)

Detailed Description

Modelisation of the ADEPT Viper robot.

This robot has six degrees of freedom.

The non modified Denavit-Hartenberg representation of the robot is given in the table below, where $q_1^*, \ldots, q_6^*$ are the variable joint positions.

$\begin{tabular}{|c|c|c|c|c|} \hline Joint & $a_i$ & $d_i$ & $\alpha_i$ & $\theta_i$ \\ \hline 1 & $a_1$ & $d_1$ & $-\pi/2$ & $q_1^*$ \\ 2 & $a_2$ & 0 & 0 & $q_2^*$ \\ 3 & $a_3$ & 0 & $-\pi/2$ & $q_3^* - \pi$ \\ 4 & 0 & $d_4$ & $\pi/2$ & $q_4^*$ \\ 5 & 0 & 0 & $-\pi/2$ & $q_5^*$ \\ 6 & 0 & 0 & 0 & $q_6^*-\pi$ \\ 7 & 0 & $d_6$ & 0 & 0 \\ \hline \end{tabular}$

In this modelisation, different frames have to be considered.

${\cal F}_f$ : the reference frame, also called world frame,
${\cal F}_w$ : the wrist frame located at the intersection of the last three rotations, with $^f{\bf M}_w = ^0{\bf M}_6$ ,
${\cal F}_e$ : the end-effector frame, with $^f{\bf M}_e = 0{\bf M}_7$ ,
${\cal F}_c$ : the camera frame, with $^f{\bf M}_c = ^f{\bf M}_e \; ^e{\bf M}_c$ where $^e{\bf M}_c$ is the result of a calibration stage.

The forward kinematics of the robot is implemented in get_fMw(), get_fMe() and get_fMc().

The robot forward jacobian used to compute the cartesian velocities from joint ones is given and implemented in get_fJw(), get_fJe() and get_eJe().

Definition at line 111 of file vpViper.h.

Constructor & Destructor Documentation

vpViper::vpViper ( )

Default constructor.

Definition at line 70 of file vpViper.cpp.

References a1, a2, a3, c56, d1, d4, d6, eMc, joint_max, joint_min, njoint, vpMath::rad(), vpColVector::resize(), and vpHomogeneousMatrix::setIdentity().

virtual vpViper::~vpViper ( )

inlinevirtual

Definition at line 115 of file vpViper.h.

Member Function Documentation

void vpViper::get_cMe ( vpHomogeneousMatrix & cMe )

Get the geometric transformation between the camera frame and the end-effector frame. This transformation is constant and correspond to the extrinsic camera parameters estimated by calibration.

Parameters

cMe	: Transformation between the camera frame and the end-effector frame.

See also: get_eMc()

Definition at line 931 of file vpViper.cpp.

References eMc, and vpHomogeneousMatrix::inverse().

Referenced by vpSimulatorViper850::get_cMe(), vpRobotViper650::get_cMe(), vpRobotViper850::get_cMe(), get_cVe(), vpSimulatorViper850::get_cVe(), vpRobotViper650::get_cVe(), and vpRobotViper850::get_cVe().

void vpViper::get_cVe ( vpVelocityTwistMatrix & cVe )

Get the twist transformation $^c{\bf V}_e$ from camera frame to end-effector frame. This transformation allows to compute a velocity expressed in the end-effector frame into the camera frame.

$^c{\bf V}_e = \left(\begin{array}{cc} ^c{\bf R}_e & [^c{\bf t}_e]_\times ^c{\bf R}_e\\ {\bf 0}_{3\times 3} & ^c{\bf R}_e \end{array} \right)$

Parameters

cVe	: Twist transformation $^c{\bf V}_e$ .

Definition at line 952 of file vpViper.cpp.

References vpVelocityTwistMatrix::buildFrom(), and get_cMe().

void vpViper::get_eJe	(	const vpColVector &	q,
		vpMatrix &	eJe
	)

Get the robot jacobian ${^e}{\bf J}_e$ which gives the velocity of the origin of the end-effector frame expressed in end-effector frame.

${^e}{\bf J}_e = \left[\begin{array}{cc} {^w}{\bf R}_f & {[{^e}{\bf t}_w}]_\times \; {^w}{\bf R}_f \\ 0_{3\times3} & {^w}{\bf R}_f \end{array} \right] \; {^f}{\bf J}_w$

Parameters

q	: A six-dimension vector that contains the joint positions of the robot expressed in radians.
eJe	: Robot jacobian ${^e}{\bf J}_e$ that express the velocity of the end-effector in the robot end-effector frame.

See also: get_fJw()

Definition at line 985 of file vpViper.cpp.

References vpHomogeneousMatrix::extract(), get_fJw(), get_fMw(), get_wMe(), vpRotationMatrix::inverse(), vpHomogeneousMatrix::inverse(), and vpTranslationVector::skew().

Referenced by vpSimulatorViper850::computeArticularVelocity(), vpSimulatorViper850::get_eJe(), vpRobotViper650::get_eJe(), vpRobotViper850::get_eJe(), and vpSimulatorViper850::getVelocity().

void vpViper::get_eMc ( vpHomogeneousMatrix & eMc )

Get the geometric transformation between the end-effector frame and the camera frame. This transformation is constant and correspond to the extrinsic camera parameters estimated by calibration.

Parameters

eMc	: Transformation between the the end-effector frame and the camera frame.

See also: get_cMe()

Definition at line 914 of file vpViper.cpp.

References eMc.

Referenced by getInverseKinematics().

void vpViper::get_fJe	(	const vpColVector &	q,
		vpMatrix &	fJe
	)

Get the robot jacobian ${^f}{\bf J}_e$ which gives the velocity of the origin of the end-effector frame expressed in the robot reference frame also called fix frame.

${^f}{\bf J}_e = \left[\begin{array}{cc} I_{3\times3} & [{^f}{\bf R}_w \; {^e}{\bf t}_w]_\times \\ 0_{3\times3} & I_{3\times3} \end{array} \right] {^f}{\bf J}_w$

Parameters

q	: A six-dimension vector that contains the joint positions of the robot expressed in radians.
fJe	: Robot jacobian ${^f}{\bf J}_e$ that express the velocity of the end-effector in the robot reference frame.

See also: get_fJw

Definition at line 1177 of file vpViper.cpp.

References vpHomogeneousMatrix::extract(), get_fJw(), get_fMw(), get_wMe(), and vpHomogeneousMatrix::inverse().

Referenced by vpSimulatorViper850::computeArticularVelocity(), vpSimulatorViper850::get_fJe(), vpRobotViper650::get_fJe(), vpRobotViper850::get_fJe(), and vpSimulatorViper850::getVelocity().

void vpViper::get_fJw	(	const vpColVector &	q,
		vpMatrix &	fJw
	)

Get the robot jacobian ${^f}{\bf J}_w$ which express the velocity of the origin of the wrist frame in the robot reference frame also called fix frame.

${^f}J_w = \left(\begin{array}{cccccc} J_{11} & J_{12} & J_{13} & 0 & 0 & 0 \\ J_{21} & J_{22} & J_{23} & 0 & 0 & 0 \\ 0 & J_{32} & J_{33} & 0 & 0 & 0 \\ 0 & -s1 & -s1 & c1s23 & J_{45} & J_{46} \\ 0 & c1 & c1 & s1s23 & J_{55} & J_{56} \\ 1 & 0 & 0 & c23 & s23s4 & J_{56} \\ \end{array} \right)$

with

$\begin{array}{l} J_{11} = -s1(-c23a3+s23d4+a1+a2c2) \\ J_{21} = c1(-c23a3+s23d4+a1+a2c2) \\ J_{12} = c1(s23a3+c23d4-a2s2) \\ J_{22} = s1(s23a3+c23d4-a2s2) \\ J_{32} = c23a3-s23d4-a2c2 \\ J_{13} = c1(a3(s2c3+c2s3)+(-s2s3+c2c3)d4)\\ J_{23} = s1(a3(s2c3+c2s3)+(-s2s3+c2c3)d4)\\ J_{33} = -a3(s2s3-c2c3)-d4(s2c3+c2s3)\\ J_{45} = -c23c1s4-s1c4\\ J_{55} = c1c4-c23s1s4\\ J_{46} = (c1c23c4-s1s4)s5+c1s23c5\\ J_{56} = (s1c23c4+c1s4)s5+s1s23c5\\ J_{66} = -s23c4s5+c23c5\\ \end{array}$

Parameters

q	: A six-dimension vector that contains the joint positions of the robot expressed in radians.
fJw	: Robot jacobian ${^f}{\bf J}_w$ that express the velocity of the point w (origin of the wrist frame) in the robot reference frame.

See also: get_fJe(), get_eJe()

Definition at line 1071 of file vpViper.cpp.

References a1, a2, a3, d4, and vpMatrix::resize().

Referenced by get_eJe(), and get_fJe().

vpHomogeneousMatrix vpViper::get_fMc ( const vpColVector & q )

Compute the forward kinematics (direct geometric model) as an homogeneous matrix.

By forward kinematics we mean here the position and the orientation of the camera relative to the base frame given the joint positions of all the six joints.

$^f{\bf M}_c = ^f{\bf M}_e \; ^e{\bf M}_c$

This method is the same than getForwardKinematics(const vpColVector & q).

Parameters

q	: Vector of six joint positions expressed in radians.

Returns: The homogeneous matrix corresponding to the direct geometric model which expresses the transformation between the base frame and the camera frame (fMc).

See also: getForwardKinematics(const vpColVector & q), get_fMe(), get_eMc()

Examples:: testRobotViper850Pose.cpp.

Definition at line 614 of file vpViper.cpp.

Referenced by vpSimulatorViper850::compute_fMi(), getForwardKinematics(), vpSimulatorViper850::getPosition(), vpRobotViper850::getVelocity(), vpRobotViper650::getVelocity(), vpSimulatorViper850::setPosition(), vpRobotViper650::setPosition(), and vpRobotViper850::setPosition().

void vpViper::get_fMc	(	const vpColVector &	q,
		vpHomogeneousMatrix &	fMc
	)

Compute the forward kinematics (direct geometric model) as an homogeneous matrix.

By forward kinematics we mean here the position and the orientation of the camera relative to the base frame given the six joint positions.

$^f{\bf M}_c = ^f{\bf M}_e \; {^e}{\bf M}_c$

Parameters

q	: Vector of six joint positions expressed in radians.
fMc	The homogeneous matrix $^f{\bf M}_c$ corresponding to the direct geometric model which expresses the transformation between the fix frame and the camera frame.

See also: get_fMe(), get_eMc()

Definition at line 644 of file vpViper.cpp.

References eMc, and get_fMe().

void vpViper::get_fMe	(	const vpColVector &	q,
		vpHomogeneousMatrix &	fMe
	)

Compute the forward kinematics (direct geometric model) as an homogeneous matrix ${^f}{\bf M}_e$ .

By forward kinematics we mean here the position and the orientation of the end effector with respect to the base frame given the motor positions of all the six joints.

${^f}M_e = \left(\begin{array}{cccc} r_{11} & r_{12} & r_{13} & t_x \\ r_{21} & r_{22} & r_{23} & t_y \\ r_{31} & r_{32} & r_{33} & t_z \\ \end{array} \right)$

with

$\begin{array}{l} r_{11} = c1(c23(c4c5c6-s4s6)-s23s5c6)-s1(s4c5c6+c4s6) \\ r_{21} = -s1(c23(-c4c5c6+s4s6)+s23s5c6)+c1(s4c5c6+c4s6) \\ r_{31} = s23(s4s6-c4c5c6)-c23s5c6 \\ \\ r_{12} = -c1(c23(c4c5s6+s4c6)-s23s5s6)+s1(s4c5s6-c4c6)\\ r_{22} = -s1(c23(c4c5s6+s4c6)-s23s5s6)-c1(s4c5s6-c4c6)\\ r_{32} = s23(c4c5s6+s4c6)+c23s5s6\\ \\ r_{13} = c1(c23c4s5+s23c5)-s1s4s5\\ r_{23} = s1(c23c4s5+s23c5)+c1s4s5\\ r_{33} = -s23c4s5+c23c5\\ \\ t_x = c1(c23(c4s5d6-a3)+s23(c5d6+d4)+a1+a2c2)-s1s4s5d6\\ t_y = s1(c23(c4s5d6-a3)+s23(c5d6+d4)+a1+a2c2)+c1s4s5d6\\ t_z = s23(a3-c4s5d6)+c23(c5d6+d4)-a2s2+d1\\ \end{array}$

Parameters

q	: A 6-dimension vector that contains the 6 joint positions expressed in radians.
fMe	The homogeneous matrix ${^f}{\bf M}_e$ corresponding to the direct geometric model which expresses the transformation between the fix frame and the end effector frame.

Note that this transformation can also be computed by considering the wrist frame ${^f}{\bf M}_e = {^f}{\bf M}_w *{^w}{\bf M}_e$ .

#include <visp/vpViper.h>
int main()
{
  vpViper robot;
  vpColVector q(6); // The measured six joint positions
  vpHomogeneousMatrix fMe; // Transformation from fix frame to end-effector
  robot.get_fMe(q, fMe); // Get the forward kinematics
  // The forward kinematics can also be computed by considering the wrist frame
  vpHomogeneousMatrix fMw; // Transformation from fix frame to wrist frame
  robot.get_fMw(q, fMw);
  vpHomogeneousMatrix wMe; // Transformation from wrist frame to end-effector
  robot.get_wMe(wMe); // Constant transformation
  // Compute the forward kinematics
  fMe = fMw * wMe;
}

Examples:: testRobotViper850.cpp, and testViper850.cpp.

Definition at line 731 of file vpViper.cpp.

References a1, a2, a3, d1, d4, and d6.

Referenced by get_fMc().

void vpViper::get_fMw	(	const vpColVector &	q,
		vpHomogeneousMatrix &	fMw
	)

Compute the transformation between the fix frame and the wrist frame. The wrist frame is located on the intersection of the 3 last rotations.

Parameters

q	: A 6-dimension vector that contains the 6 joint positions expressed in radians.
fMw	The homogeneous matrix corresponding to the transformation between the fix frame and the wrist frame (fMw).

${^f}M_w = \left(\begin{array}{cccc} r_{11} & r_{12} & r_{13} & t_x \\ r_{21} & r_{22} & r_{23} & t_y \\ r_{31} & r_{32} & r_{33} & t_z \\ \end{array} \right)$

with

$\begin{array}{l} r_{11} = c1(c23(c4c5c6-s4s6)-s23s5c6)-s1(s4c5c6+c4s6) \\ r_{21} = -s1(c23(-c4c5c6+s4s6)+s23s5c6)+c1(s4c5c6+c4s6) \\ r_{31} = s23(s4s6-c4c5c6)-c23s5c6 \\ \\ r_{12} = -c1(c23(c4c5s6+s4c6)-s23s5s6)+s1(s4c5s6-c4c6)\\ r_{22} = -s1(c23(c4c5s6+s4c6)-s23s5s6)-c1(s4c5s6-c4c6)\\ r_{32} = s23(c4c5s6+s4c6)+c23s5s6\\ \\ r_{13} = c1(c23c4s5+s23c5)-s1s4s5\\ r_{23} = s1(c23c4s5+s23c5)+c1s4s5\\ r_{33} = -s23c4s5+c23c5\\ \\ t_x = c1(-c23a3+s23d4+a1+a2c2)\\ t_y = s1(-c23a3+s23d4+a1+a2c2)\\ t_z = s23a3+c23d4-a2s2+d1\\ \end{array}$

Definition at line 827 of file vpViper.cpp.

References a1, a2, a3, d1, and d4.

Referenced by get_eJe(), and get_fJe().

void vpViper::get_wMe ( vpHomogeneousMatrix & wMe )

Return the transformation between the wrist frame and the end-effector. The wrist frame is located on the intersection of the 3 last rotations.

Parameters

wMe	The homogeneous matrix corresponding to the transformation between the wrist frame and the end-effector frame (wMe).

Definition at line 893 of file vpViper.cpp.

References d6, and vpHomogeneousMatrix::setIdentity().

Referenced by get_eJe(), get_fJe(), and getInverseKinematics().

double vpViper::getCoupl56 ( )

Return the coupling factor between join 5 and joint 6.

This factor should be only useful when motor positions are considered. Since the positions returned by the robot are joint positions which takes into account the coupling factor, it has not to be considered in the modelization of the robot.

Definition at line 1248 of file vpViper.cpp.

References c56.

vpHomogeneousMatrix vpViper::getForwardKinematics ( const vpColVector & q )

Compute the forward kinematics (direct geometric model) as an homogeneous matrix.

By forward kinematics we mean here the position and the orientation of the camera relative to the base frame given the six joint positions.

This method is the same than get_fMc(const vpColVector & q).

Parameters

q	: A six dimension vector corresponding to the robot joint positions expressed in radians.

Returns: The homogeneous matrix $^f{\bf M}_c$ corresponding to the direct geometric model which expresses the transformation between the base frame and the camera frame.

See also: get_fMc(const vpColVector & q); getInverseKinematics()

Definition at line 128 of file vpViper.cpp.

References get_fMc().

unsigned int vpViper::getInverseKinematics	(	const vpHomogeneousMatrix &	fMc,
		vpColVector &	q,
		const bool &	verbose = `false`
	)

Compute the inverse kinematics (inverse geometric model).

By inverse kinematics we mean here the six joint values given the position and the orientation of the camera frame relative to the base frame.

Parameters

fMc	: Homogeneous matrix $^f{\bf M}_c$ describing the transformation from base frame to the camera frame.
q	: In input, a six dimension vector corresponding to the current joint positions expressed in radians. In output, the solution of the inverse kinematics, ie. the joint positions corresponding to $^f{\bf M}_c$ .
verbose	: Add extra printings.

Returns: Add printings if no solution was found.; The number of solutions (1 to 8) of the inverse geometric model. O, if no solution can be found.

The code below shows how to compute the inverse geometric model:

vpColVector q1(6), q2(6);
vpHomogeneousMatrix fMc;
vpViper robot;
// Get the current joint position of the robot
robot.getPosition(vpRobot::ARTICULAR_FRAME, q1);
// Compute the pose of the camera in the reference frame using the
// direct geometric model
fMc = robot.getForwardKinematics(q1);
// this is similar to  fMc = robot.get_fMc(q1);
// or robot.get_fMc(q1, fMc);
// Compute the inverse geometric model
int nbsol; // number of solutions (0, 1 to 8) of the inverse geometric model
// get the nearest solution to the current joint position
nbsol = robot.getInverseKinematics(fMc, q1);
if (nbsol == 0)
  std::cout << "No solution of the inverse geometric model " << std::endl;
else if (nbsol >= 1)
  std::cout << "Nearest solution: " << q1 << std::endl;

See also: getForwardKinematics(), getInverseKinematicsWrist

Definition at line 576 of file vpViper.cpp.

References eMc, get_eMc(), get_wMe(), getInverseKinematicsWrist(), and vpHomogeneousMatrix::inverse().

Referenced by vpSimulatorViper850::initialiseCameraRelativeToObject(), vpSimulatorViper850::setPosition(), vpRobotViper650::setPosition(), and vpRobotViper850::setPosition().

unsigned int vpViper::getInverseKinematicsWrist	(	const vpHomogeneousMatrix &	fMw,
		vpColVector &	q,
		const bool &	verbose = `false`
	)

Compute the inverse kinematics (inverse geometric model).

By inverse kinematics we mean here the six joint values given the position and the orientation of the camera frame relative to the base frame.

Parameters

fMw	: Homogeneous matrix $^f{\bf M}_w$ describing the transformation from base frame to the wrist frame.
q	: In input, a six dimension vector corresponding to the current joint positions expressed in radians. In output, the solution of the inverse kinematics, ie. the joint positions corresponding to $^f{\bf M}_w$ .
verbose	: Add extra printings.

Returns: Add printings if no solution was found.; The number of solutions (1 to 8) of the inverse geometric model. O, if no solution can be found.

The code below shows how to compute the inverse geometric model:

vpColVector q1(6), q2(6);
vpHomogeneousMatrix fMw;
vpViper robot;
// Get the current joint position of the robot
robot.getPosition(vpRobot::ARTICULAR_FRAME, q1);
// Compute the pose of the wrist in the reference frame using the
// direct geometric model
robot.get_fMw(q1, fMw);
// Compute the inverse geometric model
int nbsol; // number of solutions (0, 1 to 8) of the inverse geometric model
// get the nearest solution to the current joint position
nbsol = robot.getInverseKinematicsWrist(fMw, q1);
if (nbsol == 0)
  std::cout << "No solution of the inverse geometric model " << std::endl;
else if (nbsol >= 1)
  std::cout << "Nearest solution: " << q1 << std::endl;

See also: getForwardKinematics(), getInverseKinematics()

Definition at line 231 of file vpViper.cpp.

References a1, a2, a3, d1, d4, vpMatrix::getRows(), njoint, vpMath::rad(), vpColVector::resize(), and vpMath::sqr().

Referenced by getInverseKinematics().

vpColVector vpViper::getJointMax ( )

Get maximal joint values.

Returns: A 6-dimension vector that contains the maximal joint values for the 6 dof. All the values are expressed in radians.

Examples:: servoViper850Point2DArtVelocity-jointAvoidance-basic.cpp, and servoViper850Point2DArtVelocity-jointAvoidance-gpa.cpp.

Definition at line 1232 of file vpViper.cpp.

References joint_max.

vpColVector vpViper::getJointMin ( )

Get minimal joint values.

Returns: A 6-dimension vector that contains the minimal joint values for the 6 dof. All the values are expressed in radians.

Examples:: servoViper850Point2DArtVelocity-jointAvoidance-basic.cpp, and servoViper850Point2DArtVelocity-jointAvoidance-gpa.cpp.

Definition at line 1219 of file vpViper.cpp.

References joint_min.

Friends And Related Function Documentation

VISP_EXPORT std::ostream& operator<<	(	std::ostream &	os,
		const vpViper &	viper
	)

friend

Print on the output stream os the robot parameters (joint min/max, coupling factor between axis 5 and 6, hand-to-eye constant homogeneous matrix $^e{\bf M}_c$ .