Visual Servoing Platform  version 3.6.1 under development (2024-03-28)
vpRobotKinova Class Reference

#include <visp3/robot/vpRobotKinova.h>

+ Inheritance diagram for vpRobotKinova:

Public Types

enum  CommandLayer { CMD_LAYER_USB , CMD_LAYER_ETHERNET , CMD_LAYER_UNSET }
 
enum  vpRobotStateType {
  STATE_STOP , STATE_VELOCITY_CONTROL , STATE_POSITION_CONTROL , STATE_ACCELERATION_CONTROL ,
  STATE_FORCE_TORQUE_CONTROL
}
 
enum  vpControlFrameType {
  REFERENCE_FRAME , ARTICULAR_FRAME , JOINT_STATE = ARTICULAR_FRAME , END_EFFECTOR_FRAME ,
  CAMERA_FRAME , TOOL_FRAME = CAMERA_FRAME , MIXT_FRAME
}
 

Public Member Functions

 vpRobotKinova ()
 
virtual ~vpRobotKinova () vp_override
 
int connect ()
 
void get_eJe (vpMatrix &eJe) vp_override
 
void get_fJe (vpMatrix &fJe) vp_override
 
vpHomogeneousMatrix get_eMc () const
 
int getActiveDevice () const
 
int getNumDevices () const
 
void getDisplacement (const vpRobot::vpControlFrameType frame, vpColVector &q) vp_override
 
void getPosition (const vpRobot::vpControlFrameType frame, vpColVector &position) vp_override
 
void getPosition (const vpRobot::vpControlFrameType frame, vpPoseVector &pose)
 
void homing ()
 
void set_eMc (vpHomogeneousMatrix &eMc)
 
void setActiveDevice (int device)
 
void setCommandLayer (CommandLayer command_layer)
 
void setDoF (unsigned int dof)
 
void setPosition (const vpRobot::vpControlFrameType frame, const vpColVector &q) vp_override
 
void setPluginLocation (const std::string &plugin_location)
 
void setVelocity (const vpRobot::vpControlFrameType frame, const vpColVector &vel) vp_override
 
void setVerbose (bool verbose)
 
Inherited functionalities from vpRobot
double getMaxTranslationVelocity (void) const
 
double getMaxRotationVelocity (void) const
 
int getNDof () const
 
vpColVector getPosition (const vpRobot::vpControlFrameType frame)
 
virtual vpRobotStateType getRobotState (void) const
 
void setMaxRotationVelocity (double maxVr)
 
void setMaxTranslationVelocity (double maxVt)
 
virtual vpRobotStateType setRobotState (const vpRobot::vpRobotStateType newState)
 

Static Public Member Functions

Static Public Member Functions inherited from vpRobot
static vpColVector saturateVelocities (const vpColVector &v_in, const vpColVector &v_max, bool verbose=false)
 

Protected Member Functions

void closePlugin ()
 
void getJointPosition (vpColVector &q)
 
void init ()
 
void loadPlugin ()
 
void setCartVelocity (const vpRobot::vpControlFrameType frame, const vpColVector &v)
 
void setJointVelocity (const vpColVector &qdot)
 
Protected Member Functions Inherited from vpRobot
vpControlFrameType setRobotFrame (vpRobot::vpControlFrameType newFrame)
 
vpControlFrameType getRobotFrame (void) const
 

Protected Attributes

vpHomogeneousMatrix m_eMc
 
std::string m_plugin_location
 
bool m_verbose
 
bool m_plugin_loaded
 
int m_devices_count
 
KinovaDevice * m_devices_list
 
int m_active_device
 
CommandLayer m_command_layer
 
double maxTranslationVelocity
 
double maxRotationVelocity
 
int nDof
 
vpMatrix eJe
 
int eJeAvailable
 
vpMatrix fJe
 
int fJeAvailable
 
int areJointLimitsAvailable
 
double * qmin
 
double * qmax
 
bool verbose_
 

Static Protected Attributes

static const double maxTranslationVelocityDefault = 0.2
 
static const double maxRotationVelocityDefault = 0.7
 

Detailed Description

Interface for Kinova Jaco2 robot.

This class is a wrapper over Kinova Jaco SDK that could be downloaded from Kinova Robotics software resources by following the link under Gen2 7 DoF > SDK 1.5.1.

It allows to control Kinova Jaco2 robot Gen 2 with 7 DoF, 6 DoF and 4 DoF.

To select the degrees of freedom corresponding to your robot use setDoF().

Examples
servoKinovaJacoCart.cpp, and servoKinovaJacoJoint.cpp.

Definition at line 89 of file vpRobotKinova.h.

Member Enumeration Documentation

◆ CommandLayer

Enumerator
CMD_LAYER_USB 
CMD_LAYER_ETHERNET 
CMD_LAYER_UNSET 

Definition at line 92 of file vpRobotKinova.h.

◆ vpControlFrameType

Robot control frames.

Enumerator
REFERENCE_FRAME 

Corresponds to a fixed reference frame attached to the robot structure.

ARTICULAR_FRAME 

Corresponds to the joint state. This value is deprecated. You should rather use vpRobot::JOINT_STATE.

JOINT_STATE 

Corresponds to the joint state.

END_EFFECTOR_FRAME 

Corresponds to robot end-effector frame.

CAMERA_FRAME 

Corresponds to a frame attached to the camera mounted on the robot end-effector.

TOOL_FRAME 

Corresponds to a frame attached to the tool (camera, gripper...) mounted on the robot end-effector. This value is equal to vpRobot::CAMERA_FRAME.

MIXT_FRAME 

Corresponds to a "virtual" frame where translations are expressed in the reference frame, and rotations in the camera frame.

Definition at line 74 of file vpRobot.h.

◆ vpRobotStateType

enum vpRobot::vpRobotStateType
inherited

Robot control states.

Enumerator
STATE_STOP 

Stops robot motion especially in velocity and acceleration control.

STATE_VELOCITY_CONTROL 

Initialize the velocity controller.

STATE_POSITION_CONTROL 

Initialize the position controller.

STATE_ACCELERATION_CONTROL 

Initialize the acceleration controller.

STATE_FORCE_TORQUE_CONTROL 

Initialize the force/torque controller.

Definition at line 62 of file vpRobot.h.

Constructor & Destructor Documentation

◆ vpRobotKinova()

vpRobotKinova::vpRobotKinova ( )

Default constructor that consider a 6 DoF Jaco arm. Use setDoF() to change the degrees of freedom.

Definition at line 54 of file vpRobotKinova.cpp.

References init().

◆ ~vpRobotKinova()

vpRobotKinova::~vpRobotKinova ( )
virtual

Destructor.

Definition at line 64 of file vpRobotKinova.cpp.

References closePlugin(), and m_devices_list.

Member Function Documentation

◆ closePlugin()

void vpRobotKinova::closePlugin ( )
protected

Close plugin.

Definition at line 780 of file vpRobotKinova.cpp.

References m_plugin_loaded, and m_verbose.

Referenced by loadPlugin(), and ~vpRobotKinova().

◆ connect()

int vpRobotKinova::connect ( )

Connect to Kinova devices.

Returns
Number of devices that are connected.

Definition at line 817 of file vpRobotKinova.cpp.

References vpException::fatalError, loadPlugin(), m_devices_count, m_devices_list, m_plugin_loaded, m_verbose, and setActiveDevice().

◆ get_eJe()

void vpRobotKinova::get_eJe ( vpMatrix eJe)
virtual

Get the robot Jacobian expressed in the end-effector frame. This function is not implemented. In fact, we don't need it since we can control the robot in cartesian in end-effector frame.

Parameters
[out]eJe: End-effector frame Jacobian.

Implements vpRobot.

Definition at line 120 of file vpRobotKinova.cpp.

References vpRobot::eJe, vpException::fatalError, m_devices_count, and m_plugin_loaded.

◆ get_eMc()

vpHomogeneousMatrix vpRobotKinova::get_eMc ( ) const
inline

Return constant transformation between end-effector and tool frame. If your tool is a camera, this transformation is obtained by hand-eye calibration.

Definition at line 106 of file vpRobotKinova.h.

◆ get_fJe()

void vpRobotKinova::get_fJe ( vpMatrix fJe)
virtual

Get the robot Jacobian expressed in the robot reference frame. This function is not implemented. In fact, we don't need it since we can control the robot in cartesian in end-effector frame.

Parameters
[out]fJe: Base (or reference) frame Jacobian.

Implements vpRobot.

Definition at line 140 of file vpRobotKinova.cpp.

References vpException::fatalError, vpRobot::fJe, m_devices_count, and m_plugin_loaded.

◆ getActiveDevice()

int vpRobotKinova::getActiveDevice ( ) const
inline

Definition at line 108 of file vpRobotKinova.h.

◆ getDisplacement()

void vpRobotKinova::getDisplacement ( const vpRobot::vpControlFrameType  frame,
vpColVector q 
)
virtual

Get a displacement.

Parameters
[in]frame: Considered cartesian frame or joint state.
[out]q: Displacement in meter and rad.

Implements vpRobot.

Definition at line 674 of file vpRobotKinova.cpp.

References vpException::fatalError, m_devices_count, and m_plugin_loaded.

◆ getJointPosition()

void vpRobotKinova::getJointPosition ( vpColVector q)
protected

Get robot joint positions.

Warning
We consider here that the robot has only 6 dof, but from the Jaco SDK it could be 7. Should be improved.
Parameters
[in]q: Joint position in rad.

Definition at line 442 of file vpRobotKinova.cpp.

References vpException::fatalError, vpRobot::nDof, vpMath::rad(), and vpColVector::resize().

Referenced by getPosition().

◆ getMaxRotationVelocity()

◆ getMaxTranslationVelocity()

◆ getNDof()

int vpRobot::getNDof ( ) const
inlineinherited

Return robot degrees of freedom number.

Examples
servoPololuPtuPoint2DJointVelocity.cpp.

Definition at line 143 of file vpRobot.h.

◆ getNumDevices()

int vpRobotKinova::getNumDevices ( ) const
inline

Definition at line 109 of file vpRobotKinova.h.

◆ getPosition() [1/3]

vpColVector vpRobot::getPosition ( const vpRobot::vpControlFrameType  frame)
inherited

Return the current robot position in the specified frame.

Definition at line 213 of file vpRobot.cpp.

References vpRobot::getPosition().

◆ getPosition() [2/3]

void vpRobotKinova::getPosition ( const vpRobot::vpControlFrameType  frame,
vpColVector position 
)
virtual

Get robot position.

Parameters
[in]frame: Considered cartesian frame or joint state.
[out]position: Either joint or cartesian position. When frame is set to vpRobot::JOINT_STATE, position contains joint angles expressed in rad, while when frame is set to vpRobot::END_EFFECTOR_FRAME position contains the cartesian position of the end-effector in the robot base frame as a 6-dim vector, with first the 3 translations expressed in meter, and then the 3 Euler rotations Rxyz expressed in radians.

The following code shows how to use this function and convert the resulting position into an homogeneous matrix that gives the transformation between the robot base frame and the end-effector:

vpColVector position;
...
robot.getPosition(vpRobot::END_EFFECTOR_FRAME, position);
vpTranslationVector wte; // reference frame to end-effector frame translations
vpRxyzVector wre; // reference frame to end-effector frame rotations
// Update the transformation between reference frame and end-effector frame
for (unsigned int i=0; i < 3; i++) {
wte[i] = position[i]; // tx, ty, tz
wre[i] = position[i+3]; // ry, ry, rz
}
// Create a rotation matrix from the Rxyz rotation angles
vpRotationMatrix wRe(wre); // reference frame to end-effector frame rotation matrix
// Create reference frame to end-effector frame transformation in terms of an homogeneous matrix
vpHomogeneousMatrix wMe(wte, wRe);
Implementation of column vector and the associated operations.
Definition: vpColVector.h:163
Implementation of an homogeneous matrix and operations on such kind of matrices.
@ END_EFFECTOR_FRAME
Definition: vpRobot.h:81
Implementation of a rotation matrix and operations on such kind of matrices.
Implementation of a rotation vector as Euler angle minimal representation.
Definition: vpRxyzVector.h:176
Class that consider the case of a translation vector.

Implements vpRobot.

Definition at line 510 of file vpRobotKinova.cpp.

References vpRobot::END_EFFECTOR_FRAME, vpException::fatalError, getJointPosition(), vpRobot::JOINT_STATE, m_devices_count, m_plugin_loaded, and vpColVector::resize().

Referenced by getPosition(), and setCartVelocity().

◆ getPosition() [3/3]

void vpRobotKinova::getPosition ( const vpRobot::vpControlFrameType  frame,
vpPoseVector pose 
)

Get robot position.

Parameters
[in]frame: Type of cartesian position to retrieve. Admissible value is:
[out]pose: Cartesian position of the end-effector in the robot base frame as a 6-dim pose vector, with first the 3 translations expressed in meter, and then the 3 rotations in radian as a $\theta {\bf u}$ vector (see vpThetaUVector).

The following code shows how to use this function and convert the resulting position into an homogeneous matrix that gives the transformation between the robot base frame and the end-effector:

...
robot.getPosition(vpRobot::END_EFFECTOR_FRAME, pose);
// Create reference frame to end-effector frame transformation in terms of an homogeneous matrix
Implementation of a pose vector and operations on poses.
Definition: vpPoseVector.h:189

Definition at line 556 of file vpRobotKinova.cpp.

References vpException::fatalError, getPosition(), and vpRobot::JOINT_STATE.

◆ getRobotFrame()

vpControlFrameType vpRobot::getRobotFrame ( void  ) const
inlineprotectedinherited

◆ getRobotState()

virtual vpRobotStateType vpRobot::getRobotState ( void  ) const
inlinevirtualinherited

Definition at line 153 of file vpRobot.h.

Referenced by vpRobotBiclops::getPosition(), vpRobotBiclops::getVelocity(), vpRobotCamera::setPosition(), vpSimulatorCamera::setPosition(), vpRobotAfma4::setPosition(), vpRobotAfma6::setPosition(), vpRobotFranka::setPosition(), vpRobotUniversalRobots::setPosition(), vpRobotViper650::setPosition(), vpRobotViper850::setPosition(), vpRobotBiclops::setPosition(), vpRobotPololuPtu::setPosition(), vpRobotPtu46::setPosition(), vpSimulatorAfma6::setPosition(), vpSimulatorViper850::setPosition(), vpRobotBiclops::setRobotState(), vpRobotPololuPtu::setRobotState(), vpSimulatorAfma6::setRobotState(), vpSimulatorViper850::setRobotState(), vpRobotAfma4::setRobotState(), vpRobotAfma6::setRobotState(), vpRobotFlirPtu::setRobotState(), vpRobotFranka::setRobotState(), vpRobotPtu46::setRobotState(), vpRobotUniversalRobots::setRobotState(), vpRobotViper650::setRobotState(), vpRobotViper850::setRobotState(), vpRobotBiclops::setVelocity(), vpRobotPololuPtu::setVelocity(), vpRobotPtu46::setVelocity(), vpRobotCamera::setVelocity(), vpRobotFlirPtu::setVelocity(), vpRobotFranka::setVelocity(), setVelocity(), vpRobotTemplate::setVelocity(), vpRobotUniversalRobots::setVelocity(), vpSimulatorCamera::setVelocity(), vpSimulatorPioneer::setVelocity(), vpSimulatorPioneerPan::setVelocity(), vpRobotAfma4::setVelocity(), vpRobotAfma6::setVelocity(), vpRobotViper650::setVelocity(), vpRobotViper850::setVelocity(), vpSimulatorAfma6::setVelocity(), vpSimulatorViper850::setVelocity(), vpRobotFlirPtu::stopMotion(), vpRobotFranka::stopMotion(), vpRobotViper650::stopMotion(), vpRobotViper850::stopMotion(), vpSimulatorAfma6::stopMotion(), and vpSimulatorViper850::stopMotion().

◆ homing()

void vpRobotKinova::homing ( )

Move the robot to home position.

Definition at line 798 of file vpRobotKinova.cpp.

References vpException::fatalError, m_devices_count, m_plugin_loaded, and m_verbose.

◆ init()

void vpRobotKinova::init ( void  )
protectedvirtual

◆ loadPlugin()

void vpRobotKinova::loadPlugin ( )
protected

Load functions from Jaco SDK plugin.

  • When command layer is set to CMD_LAYER_USB we load Kinova.API.USBCommandLayerUbuntu.so or CommandLayerWindows.dll respectively on unix-like or Windows platform.
  • When command layer is set to CMD_LAYER_ETHERNET we load Kinova.API.EthCommandLayerUbuntu.so or CommandLayerEthernet.dll respectively on unix-like or Windows platform.

There is setPluginLocation() that allows to modify the default location of the plugin set to "./".

See also
setPluginLocation(), setCommandLayer()

Definition at line 699 of file vpRobotKinova.cpp.

References closePlugin(), CMD_LAYER_UNSET, CMD_LAYER_USB, vpIoTools::createFilePath(), vpException::fatalError, m_command_layer, m_plugin_loaded, m_plugin_location, and m_verbose.

Referenced by connect().

◆ saturateVelocities()

vpColVector vpRobot::saturateVelocities ( const vpColVector v_in,
const vpColVector v_max,
bool  verbose = false 
)
staticinherited

Saturate velocities.

Parameters
v_in: Vector of input velocities to saturate. Translation velocities should be expressed in m/s while rotation velocities in rad/s.
v_max: Vector of maximal allowed velocities. Maximal translation velocities should be expressed in m/s while maximal rotation velocities in rad/s.
verbose: Print a message indicating which axis causes the saturation.
Returns
Saturated velocities.
Exceptions
vpRobotException::dimensionError: If the input vectors have different dimensions.

The code below shows how to use this static method in order to saturate a velocity skew vector.

#include <iostream>
#include <visp3/robot/vpRobot.h>
int main()
{
// Set a velocity skew vector
v[0] = 0.1; // vx in m/s
v[1] = 0.2; // vy
v[2] = 0.3; // vz
v[3] = vpMath::rad(10); // wx in rad/s
v[4] = vpMath::rad(-10); // wy
v[5] = vpMath::rad(20); // wz
// Set the maximal allowed velocities
vpColVector v_max(6);
for (int i=0; i<3; i++)
v_max[i] = 0.3; // in translation (m/s)
for (int i=3; i<6; i++)
v_max[i] = vpMath::rad(10); // in rotation (rad/s)
// Compute the saturated velocity skew vector
vpColVector v_sat = vpRobot::saturateVelocities(v, v_max, true);
std::cout << "v : " << v.t() << std::endl;
std::cout << "v max: " << v_max.t() << std::endl;
std::cout << "v sat: " << v_sat.t() << std::endl;
return 0;
}
vpRowVector t() const
static double rad(double deg)
Definition: vpMath.h:127
static vpColVector saturateVelocities(const vpColVector &v_in, const vpColVector &v_max, bool verbose=false)
Definition: vpRobot.cpp:160

Definition at line 160 of file vpRobot.cpp.

References vpException::dimensionError, and vpArray2D< Type >::size().

Referenced by vpRobotCamera::setVelocity(), vpRobotFlirPtu::setVelocity(), vpRobotFranka::setVelocity(), setVelocity(), vpRobotPioneer::setVelocity(), vpRobotTemplate::setVelocity(), vpRobotUniversalRobots::setVelocity(), vpSimulatorCamera::setVelocity(), vpSimulatorPioneer::setVelocity(), vpSimulatorPioneerPan::setVelocity(), vpRobotAfma4::setVelocity(), vpRobotAfma6::setVelocity(), vpRobotViper650::setVelocity(), and vpRobotViper850::setVelocity().

◆ set_eMc()

void vpRobotKinova::set_eMc ( vpHomogeneousMatrix eMc)
inline

Set constant transformation between end-effector and tool frame. If your tool is a camera, this transformation is obtained by hand-eye calibration.

Definition at line 120 of file vpRobotKinova.h.

◆ setActiveDevice()

void vpRobotKinova::setActiveDevice ( int  device)

Set active device.

Parameters
[in]device: Device id corresponding to the active device. The first device has id 0. The last device is is given by getNumDevices() - 1. By default, the active device is the first one.

To know how many devices are connected, use getNumDevices().

See also
getActiveDevice()

Definition at line 855 of file vpRobotKinova.cpp.

References vpException::badValue, vpException::fatalError, m_active_device, m_devices_count, m_devices_list, and m_plugin_loaded.

Referenced by connect().

◆ setCartVelocity()

void vpRobotKinova::setCartVelocity ( const vpRobot::vpControlFrameType  frame,
const vpColVector v 
)
protected

Send to the controller a 6-dim velocity twist vector expressed in a Cartesian frame.

Parameters
[in]frame: Cartesian control frame. Units are m/s for translation and rad/s for rotation velocities.
  • In CAMERA_FRAME or TOOL_FRAME, we consider that v 6-dim velocity twist vector contains translation and rotation velocities expressed in the camera or tool frame respectively.
  • In END_EFFECTOR_FRAME, we consider that v 6-dim velocity twist vector contains translation and rotation velocities expressed in the end-effector frame.
  • In MIXT_FRAME, we consider that v 6-dim velocity twist vector contains translation velocities expressed in the base frame and rotation velocities expressed in the effector frame.
[in]v: 6-dim velocity twist vector that contains 3 translation velocities followed by 3 rotation velocities. Units are m/s for translation and rad/s for rotation velocities.

Definition at line 175 of file vpRobotKinova.cpp.

References vpRobot::CAMERA_FRAME, vpRobot::END_EFFECTOR_FRAME, vpMatrix::eye(), vpException::fatalError, getPosition(), vpMatrix::insert(), vpRobot::JOINT_STATE, m_eMc, vpRobot::MIXT_FRAME, vpRobot::REFERENCE_FRAME, and vpArray2D< Type >::size().

Referenced by setVelocity().

◆ setCommandLayer()

void vpRobotKinova::setCommandLayer ( CommandLayer  command_layer)
inline

Set command layer indicating if the robot is controlled throw USB or Ethernet.

Parameters
[in]command_layer: Layer used to control the robot.

Definition at line 126 of file vpRobotKinova.h.

◆ setDoF()

void vpRobotKinova::setDoF ( unsigned int  dof)

Specify Jaco robot degrees of freedom.

Parameters
dof: Possible values are 4, 6 or 7 corresponding to the degrees of freedom of your Kinova Jaco robot.

Definition at line 77 of file vpRobotKinova.cpp.

References vpException::fatalError, and vpRobot::nDof.

◆ setJointVelocity()

void vpRobotKinova::setJointVelocity ( const vpColVector qdot)
protected

Send a joint velocity to the controller.

Parameters
[in]qdot: Joint velocities vector. Units are rad/s for a robot arm joint velocities.

Definition at line 305 of file vpRobotKinova.cpp.

References vpMath::deg(), vpException::dimensionError, vpException::fatalError, vpRobot::nDof, and vpArray2D< Type >::size().

Referenced by setVelocity().

◆ setMaxRotationVelocity()

void vpRobot::setMaxRotationVelocity ( double  w_max)
inherited

Set the maximal rotation velocity that can be sent to the robot during a velocity control.

Parameters
w_max: Maximum rotational velocity expressed in rad/s.
Examples
servoMomentPoints.cpp, servoSimu4Points.cpp, servoSimuSphere.cpp, testFeatureSegment.cpp, testFrankaJointVelocityLimits.cpp, and testRobotFlirPtu.cpp.

Definition at line 257 of file vpRobot.cpp.

References vpRobot::maxRotationVelocity.

Referenced by vpRobotViper650::setMaxRotationVelocity(), vpRobotViper850::setMaxRotationVelocity(), and vpSimulatorAfma6::setPosition().

◆ setMaxTranslationVelocity()

void vpRobot::setMaxTranslationVelocity ( double  v_max)
inherited

Set the maximal translation velocity that can be sent to the robot during a velocity control.

Parameters
v_max: Maximum translation velocity expressed in m/s.
Examples
servoMomentPoints.cpp, servoSimu4Points.cpp, servoSimuSphere.cpp, simulateCircle2DCamVelocity.cpp, simulateFourPoints2DCartesianCamVelocity.cpp, simulateFourPoints2DPolarCamVelocity.cpp, and testFeatureSegment.cpp.

Definition at line 236 of file vpRobot.cpp.

References vpRobot::maxTranslationVelocity.

Referenced by vpSimulatorAfma6::setPosition().

◆ setPluginLocation()

void vpRobotKinova::setPluginLocation ( const std::string &  plugin_location)
inline
Parameters
[in]plugin_locationPath to Jaco SDK plugins (ie. Kinova.API.USBCommandLayerUbuntu.so on unix-like platform or CommandLayerWindows.dll on Windows platform). By default this location is empty, meaning that we suppose that the plugins are located in the same folder as the binary that want to use them.

Definition at line 135 of file vpRobotKinova.h.

◆ setPosition()

void vpRobotKinova::setPosition ( const vpRobot::vpControlFrameType  frame,
const vpColVector q 
)
virtual

Set a position to reach.

Parameters
[in]frame: Considered cartesian frame or joint state.
[in]q: Position to reach.

Implements vpRobot.

Definition at line 583 of file vpRobotKinova.cpp.

References vpMath::deg(), vpRobot::END_EFFECTOR_FRAME, vpException::fatalError, vpRobot::JOINT_STATE, m_devices_count, m_plugin_loaded, m_verbose, vpRobot::nDof, vpArray2D< Type >::size(), and vpColVector::t().

◆ setRobotFrame()

◆ setRobotState()

vpRobot::vpRobotStateType vpRobot::setRobotState ( const vpRobot::vpRobotStateType  newState)
virtualinherited

Reimplemented in vpRobotViper850, vpRobotViper650, vpRobotUniversalRobots, vpRobotPtu46, vpRobotFranka, vpRobotFlirPtu, vpRobotAfma6, vpRobotAfma4, vpSimulatorViper850, vpSimulatorAfma6, vpRobotPololuPtu, and vpRobotBiclops.

Examples
UniversalRobotsMoveToPosition.cpp, UniversalRobotsSavePosition.cpp, frankaMoveToPosition.cpp, frankaSavePosition.cpp, moveAfma4.cpp, moveBiclops.cpp, movePtu46.cpp, photometricVisualServoing.cpp, photometricVisualServoingWithoutVpServo.cpp, servoAfma4Point2DArtVelocity.cpp, servoAfma4Point2DCamVelocity.cpp, servoAfma4Point2DCamVelocityKalman.cpp, servoAfma62DhalfCamVelocity.cpp, servoAfma6AprilTagIBVS.cpp, servoAfma6AprilTagPBVS.cpp, servoAfma6Cylinder2DCamVelocity.cpp, servoAfma6Cylinder2DCamVelocitySecondaryTask.cpp, servoAfma6Ellipse2DCamVelocity.cpp, servoAfma6FourPoints2DArtVelocity.cpp, servoAfma6FourPoints2DCamVelocityLs_cur.cpp, servoAfma6FourPoints2DCamVelocityLs_des.cpp, servoAfma6Line2DCamVelocity.cpp, servoAfma6MegaposePBVS.cpp, servoAfma6Point2DArtVelocity.cpp, servoAfma6Point2DCamVelocity.cpp, servoAfma6Points2DCamVelocityEyeToHand.cpp, servoAfma6Segment2DCamVelocity.cpp, servoAfma6SquareLines2DCamVelocity.cpp, servoAfma6TwoLines2DCamVelocity.cpp, servoBiclopsPoint2DArtVelocity.cpp, servoFlirPtuIBVS.cpp, servoFrankaIBVS.cpp, servoFrankaPBVS.cpp, servoKinovaJacoCart.cpp, servoKinovaJacoJoint.cpp, servoPololuPtuPoint2DJointVelocity.cpp, servoPtu46Point2DArtVelocity.cpp, servoSimuAfma6FourPoints2DCamVelocity.cpp, servoSimuViper850FourPoints2DCamVelocity.cpp, servoUniversalRobotsIBVS.cpp, servoUniversalRobotsPBVS.cpp, servoViper650FourPoints2DArtVelocityLs_cur.cpp, servoViper650FourPoints2DCamVelocityLs_cur-SR300.cpp, servoViper650FourPoints2DCamVelocityLs_cur.cpp, servoViper650Point2DCamVelocity.cpp, servoViper850FourPoints2DArtVelocityLs_cur.cpp, servoViper850FourPoints2DArtVelocityLs_des.cpp, servoViper850FourPoints2DCamVelocityLs_cur.cpp, servoViper850FourPointsKinect.cpp, servoViper850Point2DArtVelocity-jointAvoidance-basic.cpp, servoViper850Point2DArtVelocity-jointAvoidance-gpa.cpp, servoViper850Point2DArtVelocity-jointAvoidance-large.cpp, servoViper850Point2DArtVelocity.cpp, servoViper850Point2DCamVelocity.cpp, servoViper850Point2DCamVelocityKalman.cpp, testFrankaCartForceTorque-2.cpp, testFrankaCartForceTorque.cpp, testFrankaCartVelocity-2.cpp, testFrankaCartVelocity-3.cpp, testFrankaCartVelocity.cpp, testFrankaJointPosition.cpp, testFrankaJointTorque.cpp, testFrankaJointVelocity-2.cpp, testFrankaJointVelocity-3.cpp, testFrankaJointVelocity.cpp, testFrankaJointVelocityLimits.cpp, testRobotFlirPtu.cpp, testRobotViper650-frames.cpp, testRobotViper850-frames.cpp, testUniversalRobotsCartPosition.cpp, testUniversalRobotsCartVelocity.cpp, testUniversalRobotsJointPosition.cpp, testUniversalRobotsJointVelocity.cpp, testVirtuoseAfma6.cpp, tutorial-flir-ptu-ibvs.cpp, tutorial-ibvs-4pts-wireframe-robot-afma6.cpp, and tutorial-ibvs-4pts-wireframe-robot-viper.cpp.

Definition at line 198 of file vpRobot.cpp.

Referenced by vpRobotCamera::setPosition(), vpSimulatorCamera::setPosition(), vpRobotBiclops::setRobotState(), vpRobotPololuPtu::setRobotState(), vpSimulatorAfma6::setRobotState(), vpSimulatorViper850::setRobotState(), vpRobotAfma4::setRobotState(), vpRobotAfma6::setRobotState(), vpRobotFlirPtu::setRobotState(), vpRobotFranka::setRobotState(), vpRobotPtu46::setRobotState(), vpRobotUniversalRobots::setRobotState(), vpRobotViper650::setRobotState(), vpRobotViper850::setRobotState(), vpRobotCamera::setVelocity(), vpSimulatorCamera::setVelocity(), vpSimulatorPioneer::setVelocity(), vpSimulatorPioneerPan::setVelocity(), vpSimulatorAfma6::stopMotion(), and vpSimulatorViper850::stopMotion().

◆ setVelocity()

void vpRobotKinova::setVelocity ( const vpRobot::vpControlFrameType  frame,
const vpColVector vel 
)
virtual

Send to the controller a velocity in a given frame.

Parameters
[in]frame: Control frame in which the velocity vel is expressed. In cartesian control frames, units are m/s for translation and rad/s for rotation velocities.
  • In CAMERA_FRAME or TOOL_FRAME, we consider that vel 6-dim velocity twist vector contains translation and rotation velocities expressed in the camera or tool frame respectively.
  • In END_EFFECTOR_FRAME, we consider that vel 6-dim velocity twist vector contains translation and rotation velocities expressed in the end-effector frame.
  • In MIXT_FRAME, we consider that vel 6-dim velocity twist vector contains translation velocities expressed in the base frame and rotation velocities expressed in the effector frame. To send a joint velocity, use rather JOINT_STATE. Units are rad/s for a robot arm joint velocities.
[in]vel: Vector that contains the velocity to apply to the robot. In cartesian control frames, 6-dim velocity twist vector that contains 3 translation velocities followed by 3 rotation velocities. When a joint velocities vector is given, 6-dim vector corresponding to joint velocities.

Implements vpRobot.

Definition at line 370 of file vpRobotKinova.cpp.

References vpException::dimensionError, vpRobot::END_EFFECTOR_FRAME, vpException::fatalError, vpRobot::getMaxRotationVelocity(), vpRobot::getMaxTranslationVelocity(), vpRobot::getRobotState(), vpRobot::JOINT_STATE, m_devices_count, m_plugin_loaded, vpRobot::MIXT_FRAME, vpRobot::nDof, vpRobot::REFERENCE_FRAME, vpRobot::saturateVelocities(), setCartVelocity(), setJointVelocity(), vpArray2D< Type >::size(), vpRobot::STATE_VELOCITY_CONTROL, vpRobot::TOOL_FRAME, and vpRobotException::wrongStateError.

◆ setVerbose()

void vpRobotKinova::setVerbose ( bool  verbose)
inline

Enable or disable verbose mode to print to stdout additional information.

Parameters
[in]verbose: true to enable verbose, false to disable. By default verbose mode is disabled.

Definition at line 142 of file vpRobotKinova.h.

Member Data Documentation

◆ areJointLimitsAvailable

int vpRobot::areJointLimitsAvailable
protectedinherited

Definition at line 112 of file vpRobot.h.

Referenced by vpRobot::operator=().

◆ eJe

◆ eJeAvailable

int vpRobot::eJeAvailable
protectedinherited

is the robot Jacobian expressed in the end-effector frame available

Definition at line 106 of file vpRobot.h.

Referenced by vpRobot::operator=().

◆ fJe

vpMatrix vpRobot::fJe
protectedinherited

◆ fJeAvailable

int vpRobot::fJeAvailable
protectedinherited

is the robot Jacobian expressed in the robot reference frame available

Definition at line 110 of file vpRobot.h.

Referenced by vpRobot::operator=().

◆ m_active_device

int vpRobotKinova::m_active_device
protected

Definition at line 159 of file vpRobotKinova.h.

Referenced by setActiveDevice().

◆ m_command_layer

CommandLayer vpRobotKinova::m_command_layer
protected

Definition at line 160 of file vpRobotKinova.h.

Referenced by loadPlugin().

◆ m_devices_count

int vpRobotKinova::m_devices_count
protected

◆ m_devices_list

KinovaDevice* vpRobotKinova::m_devices_list
protected

Definition at line 158 of file vpRobotKinova.h.

Referenced by connect(), init(), setActiveDevice(), and ~vpRobotKinova().

◆ m_eMc

vpHomogeneousMatrix vpRobotKinova::m_eMc
protected

Constant transformation between end-effector and tool (or camera) frame.

Definition at line 153 of file vpRobotKinova.h.

Referenced by setCartVelocity().

◆ m_plugin_loaded

bool vpRobotKinova::m_plugin_loaded
protected

◆ m_plugin_location

std::string vpRobotKinova::m_plugin_location
protected

Definition at line 154 of file vpRobotKinova.h.

Referenced by loadPlugin().

◆ m_verbose

bool vpRobotKinova::m_verbose
protected

Definition at line 155 of file vpRobotKinova.h.

Referenced by closePlugin(), connect(), homing(), loadPlugin(), and setPosition().

◆ maxRotationVelocity

◆ maxRotationVelocityDefault

const double vpRobot::maxRotationVelocityDefault = 0.7
staticprotectedinherited

Definition at line 99 of file vpRobot.h.

Referenced by vpRobotFlirPtu::init(), init(), and vpRobotTemplate::init().

◆ maxTranslationVelocity

double vpRobot::maxTranslationVelocity
protectedinherited

◆ maxTranslationVelocityDefault

const double vpRobot::maxTranslationVelocityDefault = 0.2
staticprotectedinherited

Definition at line 97 of file vpRobot.h.

Referenced by vpRobotFlirPtu::init(), init(), and vpRobotTemplate::init().

◆ nDof

◆ qmax

double* vpRobot::qmax
protectedinherited

Definition at line 114 of file vpRobot.h.

Referenced by vpRobot::operator=(), and vpRobot::~vpRobot().

◆ qmin

double* vpRobot::qmin
protectedinherited

Definition at line 113 of file vpRobot.h.

Referenced by vpRobot::operator=(), and vpRobot::~vpRobot().

◆ verbose_