![]() |
Kinova_API
v5.02.00
ROS communication with Kinova API
|
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) |
bool kinova::KinovaComm::isStopped | ( | ) |
check if API lost the control of robot.
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.
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,
void kinova::KinovaComm::getQuickStatus | ( | QuickStatus & | quick_status | ) |
This function gets information regarding some status flag of the robotical arm.
quick_status | This 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.
config | This 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.
config | config 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.
config | This 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.
controlType | Cartesian control[0] or joint control[1] |
void kinova::KinovaComm::getGeneralInformations | ( | GeneralInformations & | general_info | ) |
get almost all information of the robotical arm.
general_info | includes: 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)
The | structure containing the sensor's informations |
void kinova::KinovaComm::getForcesInfo | ( | ForcesInfo & | force_Info | ) |
This function gets information regarding all forces.
force_Info | A 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.
gripper_status | A 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.
An | AngularPosition struct containing the values. Units are degrees. angular_command {AngularInfo, FingersPosition} |
void kinova::KinovaComm::getJointAngles | ( | KinovaAngles & | angles | ) |
This function returns the angular position of the robotical arm's end effector.
angles | A 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.
angles | target joint angle to set, type float, unit in degree |
timeout | default value 0.0, not used. |
push | default true, errase all trajectory before request motion.. |
void kinova::KinovaComm::getJointVelocities | ( | KinovaAngles & | vels | ) |
This function get the velocity of each actuator.
vels | A 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.
joint_vel | joint 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.
joint_acc | An AngularAcceleration struct containing the values. Units are in G |
void kinova::KinovaComm::getJointTorques | ( | KinovaAngles & | tqs | ) |
This function returns the torque of each actuator.
tqs | A 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.
anguler_current | A 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.
min | A struct that contains all angular minimum values. (Unit: N * m) |
max | 6 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.
angles | A 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)
cartesian_command | An CartesianPosition struct containing the values of end-effector and fingers. |
Member | Unit |
---|---|
X | meter |
Y | meter |
Z | meter |
Theta X | RAD |
Theta Y | RAD |
Theta Z | RAD |
Finger 1 | No unit |
Finger 2 | No unit |
Finger 3 | No 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)
position | pose 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)
pose | target pose of robot [X,Y,Z, ThetaX, ThetaY, ThetaZ], unit in meter and radians. |
timeout | default 0.0, not used. |
push | default 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)]".
velocities | unit 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.
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.
max_trans_vel | Unit 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)]".
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)]".
max_orient_vel | Unit in rad/second |
void kinova::KinovaComm::getCartesianForce | ( | KinovaPose & | cart_force | ) |
This function returns the cartesian wrench at the robotical arm's end effector.
cart_force | A 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.
min | A struct that contains all Cartesian minimum values. (Translation unit: N Orientation unit: N * m) |
max | A 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.
inertia | A struct that contains all Cartesian inertia values. (Translation unit: Kg, Orientation unit: Kg * m^2) |
damping | A 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)
position | in [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.
user_position | contains 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.
trajectoryFIFO | The 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)
void kinova::KinovaComm::getFingerPositions | ( | FingerAngles & | fingers | ) |
This function obtain the joint position of fingers.
fingers | in 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.
fingers | in degrees from 0 to about 6800 |
timeout | timeout default 0.0, not used. |
push | default true, errase all trajectory before request motion. |
void kinova::KinovaComm::printFingers | ( | const FingersPosition & | fingers | ) |
Dumps the current finger agnles onto the screen.
fingers | Unit 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.
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.
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.
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.
status | indicates if the offset is applied or not (0 = not applied, 1 = applied) |
x | Unit in meter |
y | Unit in meter |
z | Unit 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.
status | indicates if the offset is applied or not (0 = not applied, 1 = applied) |
x | Unit in meter |
y | Unit in meter |
z | Unit in meter |