Kinova_API  v5.02.00
ROS communication with Kinova API
Public Member Functions | List of all members
kinova::KinovaComm Class Reference

Public Member Functions

 KinovaComm (const ros::NodeHandle &node_handle, boost::recursive_mutex &api_mutex, const bool is_movement_on_start)
 
void stopAPI ()
 
void startAPI ()
 
bool isStopped ()
 
void startForceControl ()
 
void stopForceControl ()
 
int robotType () const
 
void getQuickStatus (QuickStatus &quick_status)
 
void getConfig (ClientConfigurations &config)
 
void setConfig (const ClientConfigurations &config)
 
void printConfig (const ClientConfigurations &config)
 
void getControlType (int &controlType)
 
void getGeneralInformations (GeneralInformations &general_info)
 
void getSensorsInfo (SensorsInfo &sensor_Info)
 
void getForcesInfo (ForcesInfo &force_Info)
 
void getGripperStatus (Gripper &gripper_status)
 
void setAngularControl ()
 
void getAngularCommand (AngularPosition &angular_command)
 
void getJointAngles (KinovaAngles &angles)
 
void setJointAngles (const KinovaAngles &angles, int timeout=0, bool push=true)
 
void getJointVelocities (KinovaAngles &vels)
 
void setJointVelocities (const AngularInfo &joint_vel)
 
void getJointAccelerations (AngularAcceleration &joint_acc)
 
void getJointTorques (KinovaAngles &tqs)
 
void getJointCurrent (AngularPosition &anguler_current)
 
void setJointTorqueMinMax (AngularInfo &min, AngularInfo &max)
 
void printAngles (const KinovaAngles &angles)
 
void setCartesianControl ()
 
void getCartesianCommand (CartesianPosition &cartesian_command)
 
void getCartesianPosition (KinovaPose &position)
 
void setCartesianPosition (const KinovaPose &position, int timeout=0, bool push=true)
 
void setCartesianVelocities (const CartesianInfo &velocities)
 
float getMaxTranslationVelocity (void)
 
void setMaxTranslationVelocity (const float &max_trans_vel)
 
float getMaxOrientationVelocity (void)
 
void setMaxOrientationVelocity (const float &max_orient_vel)
 
void getCartesianForce (KinovaPose &position)
 
void setCartesianForceMinMax (const CartesianInfo &min, const CartesianInfo &max)
 
void setCartesianInertiaDamping (const CartesianInfo &inertia, const CartesianInfo &damping)
 
void printPosition (const KinovaPose &position)
 
void getUserCommand (UserPosition &user_position)
 
void getGlobalTrajectoryInfo (TrajectoryFIFO &trajectoryFIFO)
 
void eraseAllTrajectories (void)
 
int numFingers () const
 
void getFingerPositions (FingerAngles &fingers)
 
void setFingerPositions (const FingerAngles &fingers, int timeout=0, bool push=true)
 
void printFingers (const FingersPosition &fingers)
 
void homeArm (void)
 
bool isHomed (void)
 
void initFingers (void)
 
void setEndEffectorOffset (unsigned int status, float x, float y, float z)
 
void getEndEffectorOffset (unsigned int &status, float &x, float &y, float &z)
 

Member Function Documentation

bool kinova::KinovaComm::isStopped ( )

check if API lost the control of robot.

Returns
true if stopAPI() was called.
void kinova::KinovaComm::startForceControl ( )

This function activates the reactive force control for admittance control. Admittance control may be applied to joint or Cartesian depending to the control mode.

Warning
You can only use this function if your robotic device has torque sensors on it. Also, the robotic device must be in a standard vertical position.
int kinova::KinovaComm::robotType ( ) const

get robotType Index for robot type: JACOV1_ASSISTIVE = 0, MICO_6DOF_SERVICE = 1, MICO_4DOF_SERVICE = 2, JACOV2_6DOF_SERVICE = 3, JACOV2_4DOF_SERVICE = 4, MICO_6DOF_ASSISTIVE = 5, JACOV2_6DOF_ASSISTIVE = 6,

Returns
index of robot type
void kinova::KinovaComm::getQuickStatus ( QuickStatus &  quick_status)

This function gets information regarding some status flag of the robotical arm.

Parameters
quick_statusThis structure holds various informations but mostly it is flag status, such as robotype, retractType, forceControlStatus,
void kinova::KinovaComm::getConfig ( ClientConfigurations &  config)

obtain the current client configuration.

Parameters
configThis structure holds informations relative to the client, including serial number, robot model, limits for position, velocity, acceleration and force, etc.
void kinova::KinovaComm::setConfig ( const ClientConfigurations &  config)

This function set the client configurations of the robotical arm. The configuration data is stored on the arm itself.

Parameters
configconfig This structure holds informations relative to the client, including serial number, robot model, limits for position, velocity, acceleration and force, etc.
void kinova::KinovaComm::printConfig ( const ClientConfigurations &  config)

Dumps the client configuration onto the screen.

Parameters
configThis structure holds informations relative to the client, including serial number, robot model, limits for position, velocity, acceleration and force, etc.
void kinova::KinovaComm::getControlType ( int &  controlType)

get current control type The control in ROS is independent to Joystick control type. For example: even set robot in joint control though api in ROS, robot may still controlled in joint level by joystick.

Parameters
controlTypeCartesian control[0] or joint control[1]
void kinova::KinovaComm::getGeneralInformations ( GeneralInformations &  general_info)

get almost all information of the robotical arm.

Parameters
general_infoincludes: power statistics, sensor infos, robot position and command, etc.
void kinova::KinovaComm::getSensorsInfo ( SensorsInfo &  sensor_Info)

This function returns information about the robotical arm's sensors. (Voltage, Total current, Temperature, acceleration)

Parameters
Thestructure containing the sensor's informations
void kinova::KinovaComm::getForcesInfo ( ForcesInfo &  force_Info)

This function gets information regarding all forces.

Parameters
force_InfoA struct containing the information about the forces. Joint torque and end-effector wrench in Nm and N.
void kinova::KinovaComm::getGripperStatus ( Gripper &  gripper_status)

This function gets informations about the robotical arm's gripper. Some information may be missing, it is still in development.

Parameters
gripper_statusA struct containing the information of the gripper. Most information of each fingers, including model, motion, force, limits, etc.
void kinova::KinovaComm::getAngularCommand ( AngularPosition &  angular_command)

This function get the angular command of all actuators.

Parameters
AnAngularPosition struct containing the values. Units are degrees. angular_command {AngularInfo, FingersPosition}
Returns
  • NO_ERROR_KINOVA if operation is a success
  • ERROR_API_NOT_INITIALIZED if the API has not been initialized. To initialize it, call the InitAPI() function.
void kinova::KinovaComm::getJointAngles ( KinovaAngles angles)

This function returns the angular position of the robotical arm's end effector.

Parameters
anglesA structure that contains the position of each actuator. Unit in degrees.
void kinova::KinovaComm::setJointAngles ( const KinovaAngles angles,
int  timeout = 0,
bool  push = true 
)

Sends a joint angle command to the Kinova arm. This function sends trajectory point(Angular) that will be added in the robotical arm's FIFO. Waits until the arm has stopped moving before releasing control of the API. sendAdvanceTrajectory() is called in api to complete the motion.

Parameters
anglestarget joint angle to set, type float, unit in degree
timeoutdefault value 0.0, not used.
pushdefault true, errase all trajectory before request motion..
void kinova::KinovaComm::getJointVelocities ( KinovaAngles vels)

This function get the velocity of each actuator.

Parameters
velsA kinovaAngles structure contains joint velocity of each actuator. Unit in degrees/second.
void kinova::KinovaComm::setJointVelocities ( const AngularInfo &  joint_vel)

This function controls robot with joint velocity. This function sends trajectory point(ANGULAR_VELOCITY) that will be added in the robotical arm's FIFO. Waits until the arm has stopped moving before releasing control of the API. sendAdvanceTrajectory() is called in api to complete the motion.

Parameters
joint_veljoint velocity in degree/second
void kinova::KinovaComm::getJointAccelerations ( AngularAcceleration &  joint_acc)

This function get the accelerometer values of each actuator. It does not directly refer to the angular acceleration.

Parameters
joint_accAn AngularAcceleration struct containing the values. Units are in G
void kinova::KinovaComm::getJointTorques ( KinovaAngles tqs)

This function returns the torque of each actuator.

Parameters
tqsA structure that contains the torque of each actuator. Unit is Newton * meter
void kinova::KinovaComm::getJointCurrent ( AngularPosition &  anguler_current)

This function returns the current that each actuator consume on the main supply.

Parameters
anguler_currentA structure that contains the current of each actuator and finger. Unit in A.
void kinova::KinovaComm::setJointTorqueMinMax ( AngularInfo &  min,
AngularInfo &  max 
)

This function set the angular torque's maximum and minimum values.

Parameters
minA struct that contains all angular minimum values. (Unit: N * m)
max6 A struct that contains all angular max values. (Unit: N * m)
void kinova::KinovaComm::printAngles ( const KinovaAngles angles)

Dumps the current joint angles onto the screen.

Parameters
anglesA structure contains six joint angles. Unit in degrees
void kinova::KinovaComm::getCartesianCommand ( CartesianPosition &  cartesian_command)

This function get the cartesian command of the end effector. The Cartesian orientation is expressed in Euler-ZYX convention, so that tf::Matrix3x3 EulerYPR = Rz(tz)*Ry(ty)*Rx(tx)

Parameters
cartesian_commandAn CartesianPosition struct containing the values of end-effector and fingers.

Member Unit
Xmeter
Ymeter
Zmeter
Theta XRAD
Theta YRAD
Theta ZRAD
Finger 1No unit
Finger 2No unit
Finger 3No unit

void kinova::KinovaComm::getCartesianPosition ( KinovaPose position)

This function returns the cartesian position of the robotical arm's end effector. In KinovaPose, orientation is expressed in Euler-ZYX convention, so that tf::Matrix3x3 EulerYPR = Rz(tz)*Ry(ty)*Rx(tx)

Parameters
positionpose in [X,Y,Z,ThetaX,ThetaY,ThetaZ] form, Units in meters and radians.
void kinova::KinovaComm::setCartesianPosition ( const KinovaPose pose,
int  timeout = 0,
bool  push = true 
)

Sends a cartesian coordinate trajectory to the Kinova arm. This function sends trajectory point(Cartesian) that will be added in the robotical arm's FIFO. Waits until the arm has stopped moving before releasing control of the API. sendBasicTrajectory() is called in api to complete the motion. In KinovaPose, orientation is expressed in Euler-ZYX convention, so that tf::Matrix3x3 EulerYPR = Rz(tz)*Ry(ty)*Rx(tx)

Parameters
posetarget pose of robot [X,Y,Z, ThetaX, ThetaY, ThetaZ], unit in meter and radians.
timeoutdefault 0.0, not used.
pushdefault true, errase all trajectory before request motion..
void kinova::KinovaComm::setCartesianVelocities ( const CartesianInfo &  velocities)

Linear and angular velocity control in Cartesian space This function sends trajectory point(CARTESIAN_VELOCITY) that will be added in the robotical arm's FIFO. Waits until the arm has stopped moving before releasing control of the API. sendAdvanceTrajectory() is called in api to complete the motion. Definition of angular velocity "Omega" is based on the skew-symmetric matrices "S = R*R^(-1)", where "R" is the rotation matrix. angular velocity vector "Omega = [S(3,2); S(1,3); S(2,1)]".

Parameters
velocitiesunit are meter/second for linear velocity and radians/second for "Omega".
float kinova::KinovaComm::getMaxTranslationVelocity ( void  )

This function returns the max translation(X, Y and Z) velocity of the robot's end effector in ClientConfigurations.

Returns
MaxTranslationVelocity Unit in meter per second
void kinova::KinovaComm::setMaxTranslationVelocity ( const float &  max_trans_vel)

This function set the max translation(X, Y and Z) velocity of the robot's end effector in ClientConfigurations.

Parameters
max_trans_velUnit in meter per second
float kinova::KinovaComm::getMaxOrientationVelocity ( void  )

This function get max orientation(ThetaX, ThetaY and ThetaZ) velocity of the robot's end effector. Definition of angular velocity "Omega" is based on the skew-symmetric matrices "S = R*R^(-1)", where "R" is the rotation matrix. angular velocity vector "Omega = [S(3,2); S(1,3); S(2,1)]".

Returns
Unit in rad/second
void kinova::KinovaComm::setMaxOrientationVelocity ( const float &  max_orient_vel)

This function set max orientation(ThetaX, ThetaY and ThetaZ) velocity of the robot's end effector. Definition of angular velocity "Omega" is based on the skew-symmetric matrices "S = R*R^(-1)", where "R" is the rotation matrix. angular velocity vector "Omega = [S(3,2); S(1,3); S(2,1)]".

Parameters
max_orient_velUnit in rad/second
void kinova::KinovaComm::getCartesianForce ( KinovaPose cart_force)

This function returns the cartesian wrench at the robotical arm's end effector.

Parameters
cart_forceA structure that contains the wrench vector at the end effector. Unit in N and N * m.
void kinova::KinovaComm::setCartesianForceMinMax ( const CartesianInfo &  min,
const CartesianInfo &  max 
)

This function set the Cartesian force's maximum and minimum values.

Parameters
minA struct that contains all Cartesian minimum values. (Translation unit: N Orientation unit: N * m)
maxA struct that contains all Cartesian maximum values. (Translation unit: N Orientation unit: N * m)
void kinova::KinovaComm::setCartesianInertiaDamping ( const CartesianInfo &  inertia,
const CartesianInfo &  damping 
)

This function set the Cartesian inertia and damping value.

Parameters
inertiaA struct that contains all Cartesian inertia values. (Translation unit: Kg, Orientation unit: Kg * m^2)
dampingA struct that contains all Cartesian damping values. (Translation unit: (N * s) / m, Orientation unit: (N * s) / RAD)
void kinova::KinovaComm::printPosition ( const KinovaPose position)

Dumps the current cartesian pose onto the screen. In KinovaPose, orientation is expressed in Euler-ZYX convention, so that tf::Matrix3x3 EulerYPR = Rz(tz)*Ry(ty)*Rx(tx)

Parameters
positionin [X,Y,Z,ThetaX,ThetaY,ThetaZ], where orientation is using Euler-ZYX convention.
void kinova::KinovaComm::getUserCommand ( UserPosition &  user_position)

This function extract the UserPosition from trajectory.

Parameters
user_positioncontains POSITION_TYPE(Angular/Cartesian position/velocity, etc), CartesianInfo and AngularInfo, finger positions etc.
void kinova::KinovaComm::getGlobalTrajectoryInfo ( TrajectoryFIFO &  trajectoryFIFO)

This function returns informations about the trajectories FIFO stored inside the robotical arm. Detail of trajectory point is not stored in trajectoryFIFO.

Parameters
trajectoryFIFOThe structure containing the FIFO's informations: {TrajectoryCount; UsedPercentage; MaxSize}.
int kinova::KinovaComm::numFingers ( ) const

This function get number of fingers. number of fingers determined by robotType. 3 fingers for robotType(0,3,4,6) and 2 fingers for robotType(1,2,5)

Returns
returns number of fingers.
void kinova::KinovaComm::getFingerPositions ( FingerAngles fingers)

This function obtain the joint position of fingers.

Parameters
fingersin degrees, range from 0 to 6800
void kinova::KinovaComm::setFingerPositions ( const FingerAngles fingers,
int  timeout = 0,
bool  push = true 
)

This function sets the finger positions The new finger position, combined with current joint values are constructed as a trajectory point. sendAdvancedTrajectory() is called in api to complete the motion.

Parameters
fingersin degrees from 0 to about 6800
timeouttimeout default 0.0, not used.
pushdefault true, errase all trajectory before request motion.
void kinova::KinovaComm::printFingers ( const FingersPosition &  fingers)

Dumps the current finger agnles onto the screen.

Parameters
fingersUnit in degrees 0 to 6800
void kinova::KinovaComm::homeArm ( void  )

This function move the arm to the "home" position. The code replicates the function of the "home" button on the user controller by "pressing" the home button long enough for the arm to return to the home position.

Warning
The home position is the default home, rather than user defined home.
bool kinova::KinovaComm::isHomed ( void  )

Determines whether the arm has returned to its "Home" state. Checks the current joint angles, then compares them to the known "Home" joint angles.

Returns
true is robot is already in predefined "Home"configuration.
Warning
The home position is the default home, rather than user defined home.
void kinova::KinovaComm::initFingers ( void  )

This function initializes the fingers of the robotical arm. After the initialization, the robotical arm is in angular control mode. If you want to use the cartesian control mode, use the function setCartesianControl(). Move fingers to the full-open position to initialize them for use.

Warning
This routine requires firmware version 5.05.x (or higher).
void kinova::KinovaComm::setEndEffectorOffset ( unsigned int  status,
float  x,
float  y,
float  z 
)

This function Set the end effector offset's parameters. The end effector's offset is a translation offset applied to the end effector of the robotic arm.

Parameters
statusindicates if the offset is applied or not (0 = not applied, 1 = applied)
xUnit in meter
yUnit in meter
zUnit in meter
void kinova::KinovaComm::getEndEffectorOffset ( unsigned int &  status,
float &  x,
float &  y,
float &  z 
)

This function get the end effector offset's parameters. The end effector's offset is a translation offset applied to the end effector of the robotic arm.

Parameters
statusindicates if the offset is applied or not (0 = not applied, 1 = applied)
xUnit in meter
yUnit in meter
zUnit in meter