javax.robotics.engine
Class JRoboOp

java.lang.Object
  extended by javax.robotics.engine.JRoboOp
Direct Known Subclasses:
ComputedTorque, DHRobot, GravityCompensation, ImpedanceControl, PDGravityCompensation, ProportionalDerivative, ResolvedRateAcceleration, RobustControl, Spline

public abstract class JRoboOp
extends java.lang.Object

Java Robotics Oriented Package.

A java wrapper for C++ library ROBOOP.

This class contains protected native methods.

To use it install the DLL library JRoboOp.dll (libJRoboOp.so for Linux)

Version:
1.1.2
Author:
Carmine Lia

Field Summary
protected static short ID
           
static java.lang.String Version
           
 
Constructor Summary
protected JRoboOp()
          Constructor of the engine.
 
Method Summary
protected  double[] acceleration(short ID, double[] q, double[] qp, double[] tau)
          Computes the solution of the Forward Dynamics Problem.
protected  double[] acceleration(short ID, double[] q, double[] qp, double[] tauCmd, double[] Fext, double[] Next)
          Computes the solution of the Forward Dynamics Problem with load applied to the last link.
protected  void createComputedTorqueMethod(short ID, short robotID, double[] Kp, double[] Kd)
          Computed Torque Method position controller class constructor.
protected  void createGravityCompensation(short ID, short robotID, double Kvp, double Kpp, double Kvo, double Kpo)
          Gravity Compensation controller class constructor.
protected  void createImpedance(short ID, short robotID, double[] Mp, double[] Dp, double[] Kp, double[] Km, double[] Mo, double[] Do, double[] Ko)
           
protected  void createPDGravityComp(short ID, short robotID, double[] Kp, double[] Kd)
          Proportional Derivative with gravity compensation joints position controller class constructor.
protected  void createProportionalDerivative(short ID, short robotID, double[] Kp, double[] Kd)
          Proportional Derivative joints position controller class constructor.
protected  void createResolveRateAcceleration(short ID, short robotID, double Kvp, double Kpp, double Kvo, double Kpo)
          Resolved Rate Acceleration controller class constructor.
protected  void createRobot(short ID)
          Default Robot constructor.
protected  void createRobot(short ID, double[] initRobot, int rows, int cols)
          Robot constructor with init parameter and motor parameter.
protected  void createRobot(short ID, double[] initRobot, int rowsR, int colsR, double[] initMotor, int rowsM, int colsM)
          Robot constructor with init parameter and motor parameter.
protected  void createRobot(short ID, java.lang.String fileName, java.lang.String robotName)
          Robot constructor with ROBOOP file parameter.
protected  void createRobustControl(short ID, short robotID, double[] Kp, double[] Kd, double[] B, double[] Q, double rho, double eps)
          Robust control position controller class constructor.
protected  void createSplinePath(short ID, int spaceDim, int nPoints, double[] array)
          Spline Path class constructor from ROBOOP file.
protected  void createSplineQuaternion(short ID, int nPoints, double[] array)
          Spline Quaternion class constructor from array.
protected  double[] dTdqi(short ID, int j)
          Computes the partial derivative of homogeneous transform respect j-th joint.
protected  int getAvDof(short ID)
          Gets the available degrees of freedom of robot.
protected  boolean getConverge(short ID)
           
protected  int getDof(short ID)
          Gets the degrees of freedom of robot.
protected  short getError(short ID)
           
protected  double[] getGravity(short ID)
          Gets gravity vector.
protected abstract  short getID()
          Gets the ID.
protected  double[] getPosErrorGCOMP(short ID)
          Gets the position error.
protected  double[] getPosErrorRRA(short ID)
          Gets the position error.
protected  double[] getQ(short ID)
          Gets joints position.
protected  double getQ(short ID, int i)
          Gets the position of i-th joint.
protected  double[] getQMax(short ID)
          Gets joints maximum position.
protected  double[] getQMin(short ID)
          Gets joints minumum position.
protected  double[] getQOffset(short ID)
          Gets joints minumum position.
protected  double[] getQp(short ID)
          Gets joints velocity.
protected  double[] getQpp(short ID)
          Gets joints acceleration.
protected  double[] getQuatErrorGCOMP(short ID)
          Gets the quaternion error.
protected  double[] getQuatErrorRRA(short ID)
          Gets the quaternion error.
protected  short impedanceControl(short ID, short robotID, double[] pdpp, double[] pdp, double[] pd, double[] wdp, double[] wd, double[] qd, double[] f, double[] n, double dt)
           
protected  double[] inertia(short ID, double[] q)
          Computes the inertia matrix.
protected  double[] interpolate(short ID, double t)
          Interpolates the spline at time t to set the position, velocity and acceleration.
protected  double[] interpolatePathQuat(short ID, double t)
          Interpolates the spline at time t to set the position, velocity, acceleration, the quaternion and angular velocity.
protected  double[] interpolateQuat(short ID, double t)
          Interpolates the spline at time t to set the quaternion and angular velocity.
protected  double[] invKine(short ID, double[] Tobj)
          Computes the solution of the Inverse Kinematics Problem from homogeneous transform matrix using a Newton-Raphson technique.
protected  double[] invKine(short ID, double[] Tobj, int mj, int endlink)
          Computes the solution of the Inverse Kinematics Problem from homogeneous transform matrix using a Newton-Raphson or Taylor expansion techniques.
protected  double[] jacobian(short ID)
          Computes the Jacobian matrix expressed in the base frame.
protected  double[] jacobian(short ID, int ref)
          Computes the Jacobian matrix expressed in the ref-th frame.
protected  double[] jacobianDLSinv(short ID, double eps, double lambdaMax)
          Computes the 6x6 inverse Jacobian matrix based on the Damped Least-Squares scheme.
protected  double[] jacobianDot(short ID)
          Computes the Jacobian time derivative matrix expressed in the base frame.
protected  double[] jacobianDot(short ID, int ref)
          Computes the Jacobian time derivative matrix expressed in the ref-th frame.
protected  double[] kine(short ID)
          Computes the solution of the Forward Kinematics Problem.
protected  double[] kine(short ID, int j)
          Computes the solution of the Forward Kinematics Problem for the link j.
protected  void setGravity(short ID, double[] gravity)
          Sets the gravity vector.
protected  short setKdCTM(short ID, short robotID, double[] Kd)
          Sets the position error gain matrix.
protected  short setKdPD(short ID, short robotID, double[] Kd)
          Sets the velocity error gain matrix.
protected  short setKdPDG(short ID, short robotID, double[] Kd)
          Sets the velocity error gain matrix.
protected  short setKdRC(short ID, short robotID, double[] Kd)
          Sets the position error gain matrix.
protected  short setKpCTM(short ID, short robotID, double[] Kp)
          Sets the position error gain matrix.
protected  short setKpPD(short ID, short robotID, double[] Kp)
          Sets the position error gain matrix.
protected  short setKpPDG(short ID, short robotID, double[] Kp)
          Sets the position error gain matrix.
protected  short setKpRC(short ID, short robotID, double[] Kp)
          Sets the position error gain matrix.
protected  void setQ(short ID, double[] q)
          Sets joints position adding the offset.
protected  void setQ(short ID, double q, int i)
          Sets i-th joint position adding the offset.
protected  void setQp(short ID, double[] qp)
          Sets joints velocity.
protected  void setQpp(short ID, double[] qpp)
          Sets joints acceleration.
protected  double[] torque(short ID, double[] q, double[] qp, double[] qpp)
          Computes the solution of the Inverse Dynamics Problem.
protected  double[] torque(short ID, double[] q, double[] qp, double[] qpp, double[] Fext, double[] Next)
          Computes the solution of the Inverse Dynamics Problem with load applied to the last link.
protected  double[] torqueCmdCTM(short ID, short robotID, double[] qd, double[] qpd, double[] qppd)
          Sets the output torque for a desired joint position and velocity.
protected  double[] torqueCmdGCOMP(short ID, short robotID, double[] pd)
          Sets the output torque for a desired end effector position.
protected  double[] torqueCmdGCOMP(short ID, short robotID, double[] pd, double[] qd, short linkPc)
          Sets the output torque for a desired end effector position and angular position.
protected  double[] torqueCmdPD(short ID, short robotID, double[] qd, double[] qpd)
          Sets the output torque for a desired joint position and velocity.
protected  double[] torqueCmdPDG(short ID, short robotID, double[] qd)
          Sets the output torque for a desired joint position and velocity.
protected  double[] torqueCmdRC(short ID, short robotID, double[] qd, double[] qpd, double[] qppd)
          Sets the output torque for a desired joint position and velocity.
protected  double[] torqueCmdRRA(short ID, short robotID, double[] pdpp, double[] pdp, double[] pd)
          Sets the output torque for a desired end effector: acceleration, velocity, position.
protected  double[] torqueCmdRRA(short ID, short robotID, double[] pdpp, double[] pdp, double[] pd, double[] wdp, double[] wd, double[] qd, short linkPc)
          Sets the output torque for a desired end effector: acceleration, velocity, position, angular acceleration, angular velocity, angular position.
protected  double[] torqueCoriolis(short ID, double[] qp)
          Computes the torques from the Coriolis and centrifugal effect.
protected  double[] torqueCoulombFriction(short ID, double[] qp)
          Computes the torques from the Coulomb friction.
protected  double[] torqueGravity(short ID)
          Computes the torques from the gravity effect.
protected  double[] torqueViscousFriction(short ID, double[] qp)
          Computes the torques from the viscous friction.
 
Methods inherited from class java.lang.Object
clone, equals, finalize, getClass, hashCode, notify, notifyAll, toString, wait, wait, wait
 

Field Detail

ID

protected static short ID

Version

public static final java.lang.String Version
See Also:
Constant Field Values
Constructor Detail

JRoboOp

protected JRoboOp()
Constructor of the engine.

Method Detail

getID

protected abstract short getID()
Gets the ID.

Returns:
ID

getError

protected short getError(short ID)
Parameters:
ID -
Returns:
error flag

getConverge

protected boolean getConverge(short ID)
Parameters:
ID -
Returns:
converge flag

createRobot

protected void createRobot(short ID)
Default Robot constructor.

Call C++ default Robot constructor

 Robot()
 

Parameters:
ID - the engine id

createRobot

protected void createRobot(short ID,
                           double[] initRobot,
                           int rows,
                           int cols)
Robot constructor with init parameter and motor parameter.

Call C++ Robot constructor

 Robot(const Matrix &initRobot)
 

Parameters:
ID - engine id
initRobot - vector of robot parameter; dimension: rowsR x colsR
rows - rows of initRobot
cols - columns of initRobot

createRobot

protected void createRobot(short ID,
                           double[] initRobot,
                           int rowsR,
                           int colsR,
                           double[] initMotor,
                           int rowsM,
                           int colsM)
Robot constructor with init parameter and motor parameter.

Call C++ Robot constructor

 Robot(const Matrix &initRobot, const Matrix &initMotor)
 

Parameters:
ID - engine id
initRobot - vector of robot parameter; dimension: rowsR x colsR
rowsR - rows of initRobot
colsR - columns of initRobot
initMotor - vector of motor robot parameter; dimension: rowsM x colsM
rowsM - rows of initMotor
colsM - columns of initMotor

createRobot

protected void createRobot(short ID,
                           java.lang.String fileName,
                           java.lang.String robotName)
Robot constructor with ROBOOP file parameter.

Call C++ Robot constructor

 Robot(const string &fileName, const string &robotName)
 

Parameters:
ID - engine id
fileName - configuration file name of robot parameter
robotName - name of the robot

getDof

protected int getDof(short ID)
Gets the degrees of freedom of robot.

Call C++ Robot method

 get_dof()
 

Parameters:
ID - engine id
Returns:
degrees of freedom of robot manipulator

getAvDof

protected int getAvDof(short ID)
Gets the available degrees of freedom of robot.

Call C++ Robot method

 get_available_dof()
 

Parameters:
ID - engine id
Returns:
the available degrees of freedom of robot manipulator

getQMax

protected double[] getQMax(short ID)
Gets joints maximum position.

Call C++ Robot method

 get_q_max()
 

Parameters:
ID - the engine id
Returns:
the array of joints maximum position

getQMin

protected double[] getQMin(short ID)
Gets joints minumum position.

Call C++ Robot method

 get_q_min()
 

Parameters:
ID - the engine id
Returns:
the array of joints minimum position

getQOffset

protected double[] getQOffset(short ID)
Gets joints minumum position.

Call C++ Robot method

 get_q_offset()
 

Parameters:
ID - the engine id
Returns:
the array of joints offset position

getQ

protected double[] getQ(short ID)
Gets joints position.

Call C++ Robot method

 get_q()
 

Parameters:
ID - the engine id
Returns:
the array of joints position - joints offset

getQ

protected double getQ(short ID,
                      int i)
Gets the position of i-th joint.

Call C++ Robot method

 get_q(int i)
 

Parameters:
ID - the engine id
i - the joint.
Returns:
the joint position - joint offset

getQp

protected double[] getQp(short ID)
Gets joints velocity.

Call C++ Robot method

 get_qp()
 

Parameters:
ID - the engine id
Returns:
the array of joints velocity.

getQpp

protected double[] getQpp(short ID)
Gets joints acceleration.

Call C++ Robot method

 get_qpp()
 

Parameters:
ID - the engine id
Returns:
the array of joints acceleration.

setQ

protected void setQ(short ID,
                    double[] q)
Sets joints position adding the offset. The new position is computed as joint position + joint offset.

Call C++ Robot method

 set_q(const ColumnVector &q)
 

Parameters:
ID - the engine id
q - the array of joints position

setQ

protected void setQ(short ID,
                    double q,
                    int i)
Sets i-th joint position adding the offset. The new position is computed as joint position + joint offset.

Call C++ Robot method

 set_q(const Real q, int i)
 

Parameters:
ID - the engine id
q - joint position
i - joint

setQp

protected void setQp(short ID,
                     double[] qp)
Sets joints velocity.

Call C++ Robot method

 set_qp(const ColumnVector &qp)
 

Parameters:
ID - the engine id
qp - array of joints velocity

setQpp

protected void setQpp(short ID,
                      double[] qpp)
Sets joints acceleration.

Call C++ Robot method

 set_qpp(const ColumnVector &qpp)
 

Parameters:
ID - the engine id
qpp - the array of joints position

invKine

protected double[] invKine(short ID,
                           double[] Tobj)
Computes the solution of the Inverse Kinematics Problem from homogeneous transform matrix using a Newton-Raphson technique.

Call C++ Robot method

 inv_kin(const Matrix &Tobj)
 

Parameters:
ID - the engine id
Tobj - elements of homogeneous transform matrix row major
Returns:
joints position

invKine

protected double[] invKine(short ID,
                           double[] Tobj,
                           int mj,
                           int endlink)
Computes the solution of the Inverse Kinematics Problem from homogeneous transform matrix using a Newton-Raphson or Taylor expansion techniques.

Call C++ Robot method

 inv_kin(const Matrix &Tob, const int mj, bool &converge)
 

Parameters:
ID - the engine id
Tobj - elements homogeneous transform matrix row major.
mj - value 0,1 to choose the technique: Newton-Raphson, Taylor expansion respectively.
endlink - the link to pretend is the end effector.
Returns:
joints position

jacobian

protected double[] jacobian(short ID)
Computes the Jacobian matrix expressed in the base frame.

Call C++ Robot method

 jacobian()
 

Parameters:
ID - the engine id
Returns:
the array of jacobian matrix elements, the representation is row major.

jacobian

protected double[] jacobian(short ID,
                            int ref)
Computes the Jacobian matrix expressed in the ref-th frame.

Call C++ Robot method

 jacobian(const int ref)
 

Parameters:
ID - the engine id
ref - frame reference
Returns:
the array of jacobian matrix elements, the representation is row major.

jacobianDot

protected double[] jacobianDot(short ID)
Computes the Jacobian time derivative matrix expressed in the base frame.

Call C++ Robot method

 jacobian_dot()
 

Parameters:
ID - the engine id
Returns:
the array of time derivative jacobian matrix elements, the representation is row major.

jacobianDot

protected double[] jacobianDot(short ID,
                               int ref)
Computes the Jacobian time derivative matrix expressed in the ref-th frame.

Call C++ Robot method

 jacobian_dot(const int ref)
 

Parameters:
ID - the engine id
ref - frame reference
Returns:
the array of time derivative jacobian matrix elements, the representation is row major.

jacobianDLSinv

protected double[] jacobianDLSinv(short ID,
                                  double eps,
                                  double lambdaMax)
Computes the 6x6 inverse Jacobian matrix based on the Damped Least-Squares scheme.

Call C++ Robot method

 jacobian_DLS_inv(const Real eps, const Real lambda_max)
 

Parameters:
ID - the engine id
eps - epsilon
lambdaMax - damping factor
Returns:
inverse jacobian matrix

kine

protected double[] kine(short ID)
Computes the solution of the Forward Kinematics Problem.

Call C++ Robot method

 kine()
 

Parameters:
ID - the engine id
Returns:
the elements of homogeneous transform matrix in row major representation.

kine

protected double[] kine(short ID,
                        int j)
Computes the solution of the Forward Kinematics Problem for the link j.

Call C++ Robot method

 kine(const int j)
 

Parameters:
ID - the engine id
j - link of forward kinematic
Returns:
the elements of homogeneous transform matrix in row major representation.

dTdqi

protected double[] dTdqi(short ID,
                         int j)
Computes the partial derivative of homogeneous transform respect j-th joint.

Call C++ Robot method

 dTdqi(const int j)
 

Parameters:
ID - the engine id
j - joint
Returns:
homogeneous partial derivative transform in row major representation.

acceleration

protected double[] acceleration(short ID,
                                double[] q,
                                double[] qp,
                                double[] tau)
Computes the solution of the Forward Dynamics Problem.

Call C++ Robot method

 acceleration(const ColumnVector &q, const ColumnVector &qp, const ColumnVector &tau)
 

Parameters:
ID - the engine id
q - joints position
qp - joints velocity
tau - joints torque
Returns:
joints acceleration

acceleration

protected double[] acceleration(short ID,
                                double[] q,
                                double[] qp,
                                double[] tauCmd,
                                double[] Fext,
                                double[] Next)
Computes the solution of the Forward Dynamics Problem with load applied to the last link.

Call C++ Robot method

 acceleration(const ColumnVector &q, const ColumnVector &qp, const ColumnVector &tau_cmd, const ColumnVector &Fext, const ColumnVector &Next)
 

Parameters:
ID - the engine id
q - joints position
qp - joints velocity
tauCmd - joints torque
Fext - load applied at last link
Next - load applied at last link
Returns:
joints acceleration

torque

protected double[] torque(short ID,
                          double[] q,
                          double[] qp,
                          double[] qpp)
Computes the solution of the Inverse Dynamics Problem.

Call C++ Robot method

 torque(const ColumnVector &q, const ColumnVector &qp, const ColumnVector &qpp)
 

Parameters:
ID - the engine id
q - joints position
qp - joints velocity
qpp - joints acceleration
Returns:
joints torque

torque

protected double[] torque(short ID,
                          double[] q,
                          double[] qp,
                          double[] qpp,
                          double[] Fext,
                          double[] Next)
Computes the solution of the Inverse Dynamics Problem with load applied to the last link.

Call C++ Robot method

 torque(const ColumnVector &q, const ColumnVector &qp, const ColumnVector &qpp, const ColumnVector &Fext, const ColumnVector &Next)
 

Parameters:
ID - the engine id
q - joints position
qp - joints velocity
qpp - joints acceleration
Fext - load applied at last link
Next - load applied at last link
Returns:
joints torque

torqueGravity

protected double[] torqueGravity(short ID)
Computes the torques from the gravity effect.

Call C++ Robot method

 G()
 

Parameters:
ID - the engine id
Returns:
joints torque

torqueCoriolis

protected double[] torqueCoriolis(short ID,
                                  double[] qp)
Computes the torques from the Coriolis and centrifugal effect.

Call C++ Robot method

 C(CoulumnVector & qp)
 
Note that the original roboop C() function differs from this one, it includes also the torques due to viscous and coulomb friction.

Parameters:
ID - the engine id
qp - joints velocity
Returns:
joints torque

torqueViscousFriction

protected double[] torqueViscousFriction(short ID,
                                         double[] qp)
Computes the torques from the viscous friction.

Call C++ Robot method

 torqueViscousFriction(CoulumnVector & qp)
 

Parameters:
ID - the engine id
qp - joints velocity
Returns:
joints torque

torqueCoulombFriction

protected double[] torqueCoulombFriction(short ID,
                                         double[] qp)
Computes the torques from the Coulomb friction.

Call C++ Robot method

 torqueCoulombFriction(CoulumnVector & qp)
 

Parameters:
ID - the engine id
qp - joints velocity
Returns:
joints torque

inertia

protected double[] inertia(short ID,
                           double[] q)
Computes the inertia matrix.

Call C++ Robot method

 inertia(const ColumnVector &q)
 

Parameters:
ID - the engine id
q - joints position
Returns:
intertia matrix

getGravity

protected double[] getGravity(short ID)
Gets gravity vector.

return the robot.gravity parameter.

Parameters:
ID - the engine id
Returns:
the gravity vector

setGravity

protected void setGravity(short ID,
                          double[] gravity)
Sets the gravity vector.

sets the robot.gravity parameter.

Parameters:
ID - the engine id
gravity - the new gravity vector.

createProportionalDerivative

protected void createProportionalDerivative(short ID,
                                            short robotID,
                                            double[] Kp,
                                            double[] Kd)
Proportional Derivative joints position controller class constructor.

Call C++ constructor

 Proportional_derivative(const Robot_basic &robot, const DiagonalMatrix &Kp, const DiagonalMatrix &Kd)
 

Parameters:
ID - the engine id
robotID - the robot id
Kp - diagonal matrix joint position error gain matrix.
Kd - diagonal matrix joint velocity error gain matrix.

torqueCmdPD

protected double[] torqueCmdPD(short ID,
                               short robotID,
                               double[] qd,
                               double[] qpd)
Sets the output torque for a desired joint position and velocity.

Call C++ Proportional_derivative method

 torque_cmd(Robot_basic &robot, const ColumnVector &qd)
 

Parameters:
ID - the engine id
robotID - the robot id
qd - array of desired joint position and desired joint velocity.
Returns:
torque.

setKpPD

protected short setKpPD(short ID,
                        short robotID,
                        double[] Kp)
Sets the position error gain matrix.

Call C++ Proportional_derivative method

set_Kp(const DiagonalMatrixr &Kp)

Parameters:
ID - the engine id
robotID - the robot id
Kp - position error gain diagonal matrix
Returns:
0 or WRONG_SIZE

setKdPD

protected short setKdPD(short ID,
                        short robotID,
                        double[] Kd)
Sets the velocity error gain matrix.

Call C++ Proportional_derivative method

set_Kd(const DiagonalMatrixr &Kd)

Parameters:
ID - the engine id
robotID - the robot id
Kd - velocity error gain diagonal matrix
Returns:
0 or WRONG_SIZE

createPDGravityComp

protected void createPDGravityComp(short ID,
                                   short robotID,
                                   double[] Kp,
                                   double[] Kd)
Proportional Derivative with gravity compensation joints position controller class constructor.

Call C++ constructor

 PDGravityComp(const Robot_basic &robot, const DiagonalMatrix &Kp, const DiagonalMatrix &Kd)
 

Parameters:
ID - the engine id
robotID - the robot id
Kp - diagonal matrix joint position error gain matrix.
Kd - diagonal matrix joint velocity error gain matrix.

torqueCmdPDG

protected double[] torqueCmdPDG(short ID,
                                short robotID,
                                double[] qd)
Sets the output torque for a desired joint position and velocity.

Call C++ PDGravityComp method

 torque_cmd(Robot_basic &robot, const ColumnVector &qd)
 

Parameters:
ID - the engine id
robotID - the robot id
qd - array of desired joint position and desired joint velocity.
Returns:
torque.

setKpPDG

protected short setKpPDG(short ID,
                         short robotID,
                         double[] Kp)
Sets the position error gain matrix.

Call C++ PDGravityComp method

set_Kp(const DiagonalMatrixr &Kp)

Parameters:
ID - the engine id
robotID - the robot id
Kp - position error gain diagonal matrix
Returns:
0 or WRONG_SIZE

setKdPDG

protected short setKdPDG(short ID,
                         short robotID,
                         double[] Kd)
Sets the velocity error gain matrix.

Call C++ PDGravityComp method

set_Kd(const DiagonalMatrixr &Kd)

Parameters:
ID - the engine id
robotID - the robot id
Kd - velocity error gain diagonal matrix
Returns:
0 or WRONG_SIZE

createComputedTorqueMethod

protected void createComputedTorqueMethod(short ID,
                                          short robotID,
                                          double[] Kp,
                                          double[] Kd)
Computed Torque Method position controller class constructor.

Call C++ constructor

 Computed_torque_method(const Robot_basic &robot, const DiagonalMatrix &Kp, const DiagonalMatrix &Kd)
 

Parameters:
ID - the engine id
robotID - the robot id
Kp - diagonal matrix joint position error gain matrix.
Kd - diagonal matrix joint velocity error gain matrix.

torqueCmdCTM

protected double[] torqueCmdCTM(short ID,
                                short robotID,
                                double[] qd,
                                double[] qpd,
                                double[] qppd)
Sets the output torque for a desired joint position and velocity.

Call C++ Computed_torque_method method

  torque_cmd(Robot_basic &robot, const ColumnVector &qd, const ColumnVector &qpd, const ColumnVector &qppd)
 

Parameters:
qd - desired joints position
qpd - desired joints velocity
qppd - desired joints acceleration
Returns:
the output torque.

setKpCTM

protected short setKpCTM(short ID,
                         short robotID,
                         double[] Kp)
Sets the position error gain matrix.

Call C++ Computed_torque_method method

set_Kp(const DiagonalMatrixr &Kp)

Parameters:
ID - the engine id
robotID - the robot id
Kp - position error gain diagonal matrix
Returns:
0 or WRONG_SIZE

setKdCTM

protected short setKdCTM(short ID,
                         short robotID,
                         double[] Kd)
Sets the position error gain matrix.

Call C++ Computed_torque_method method

set_Kd(const DiagonalMatrixr &Kd)

Parameters:
ID - the engine id
robotID - the robot id
Kd - position error gain diagonal matrix
Returns:
0 or WRONG_SIZE

createRobustControl

protected void createRobustControl(short ID,
                                   short robotID,
                                   double[] Kp,
                                   double[] Kd,
                                   double[] B,
                                   double[] Q,
                                   double rho,
                                   double eps)
Robust control position controller class constructor.

Call C++ constructor

  Robust_control(const Robot_basic &robot, const DiagonalMatrix &Kp, const DiagonalMatrix &Kd, const Matrix &B, const Matrix &Q, double rho, double eps)
 

Parameters:
ID - the engine id
robotID - the robot id
Kp - diagonal matrix joint position error gain matrix.
Kd - diagonal matrix joint velocity error gain matrix.
B - constant inertia matrix of robot.
Q - the solution of Lyapunov equation.
rho - the robustness gain.
eps - the thickness of robustness region to compensate the chattering.

torqueCmdRC

protected double[] torqueCmdRC(short ID,
                               short robotID,
                               double[] qd,
                               double[] qpd,
                               double[] qppd)
Sets the output torque for a desired joint position and velocity.

Call C++ Robust_control method

  torque_cmd(Robot_basic &robot, const ColumnVector &qd, const ColumnVector &qpd, const ColumnVector &qppd)
 

Parameters:
qd - desired joints position
qpd - desired joints velocity
qppd - desired joints acceleration
Returns:
the output torque.

setKpRC

protected short setKpRC(short ID,
                        short robotID,
                        double[] Kp)
Sets the position error gain matrix.

Call C++ Robust_control method

set_Kp(const DiagonalMatrixr &Kp)

Parameters:
ID - the engine id
robotID - the robot id
Kp - position error gain diagonal matrix
Returns:
0 or WRONG_SIZE

setKdRC

protected short setKdRC(short ID,
                        short robotID,
                        double[] Kd)
Sets the position error gain matrix.

Call C++ Robust_control method

set_Kd(const DiagonalMatrixr &Kd)

Parameters:
ID - the engine id
robotID - the robot id
Kd - position error gain diagonal matrix
Returns:
0 or WRONG_SIZE

createResolveRateAcceleration

protected void createResolveRateAcceleration(short ID,
                                             short robotID,
                                             double Kvp,
                                             double Kpp,
                                             double Kvo,
                                             double Kpo)
Resolved Rate Acceleration controller class constructor.

Call C++ constructor

Resolved_acc(const Robot_basic &robot, 
                               double Kvp, double  Kpp,
                               double Kvo, double Kpo)

Parameters:
ID - the engine id
robotID - the robot id
Kvp - end effector velocity error gain
Kpp - end effector position error gain
Kvo - end effector orientation angular rate gain
Kpo - end effector orientation error gain

getPosErrorRRA

protected double[] getPosErrorRRA(short ID)
Gets the position error.

Call C++ Resolved_acc method

get_pos_error()

Parameters:
ID - the engine id
Returns:
the pos error array.

getQuatErrorRRA

protected double[] getQuatErrorRRA(short ID)
Gets the quaternion error.

Call C++ Resolved_acc method

get_quat_error()

Parameters:
ID - the engine id
Returns:
the quaternion error array.

torqueCmdRRA

protected double[] torqueCmdRRA(short ID,
                                short robotID,
                                double[] pdpp,
                                double[] pdp,
                                double[] pd,
                                double[] wdp,
                                double[] wd,
                                double[] qd,
                                short linkPc)
Sets the output torque for a desired end effector: acceleration, velocity, position, angular acceleration, angular velocity, angular position.

Call C++ Resolved_acc method

torque_cmd(Robot_basic &robot, 
                                     const ColumnVector &pdpp const ColumnVector &pdp,
                                     const ColumnVector &pd, const ColumnVector &wdp,
                                     const ColumnVector &wd, const Quaternion &qd,
                                     const short link_pc, const Real dt)

Parameters:
ID - the engine id
robotID - the robot id
pdpp - desired acceleration
pdp - desired velocity
pd - desired position
wdp - desired angular acceleration
wd - desired angular velocity
qd - desired vector angular position: quaternion (s,v1,v2,v3)
linkPc - link of which control position
Returns:
the output torque

torqueCmdRRA

protected double[] torqueCmdRRA(short ID,
                                short robotID,
                                double[] pdpp,
                                double[] pdp,
                                double[] pd)
Sets the output torque for a desired end effector: acceleration, velocity, position.

Call C++ Resolved_acc method

torque_cmd(Robot_basic &robot, 
                                     const ColumnVector &pdpp const ColumnVector &pdp,
                                     const ColumnVector &pd)

Parameters:
ID - the engine id
robotID - the robot id
pdpp - desired acceleration
pdp - desired velocity
pd - desired position
Returns:
the output torque

createGravityCompensation

protected void createGravityCompensation(short ID,
                                         short robotID,
                                         double Kvp,
                                         double Kpp,
                                         double Kvo,
                                         double Kpo)
Gravity Compensation controller class constructor.

Call C++ constructor

Gravity_comp(const Robot_basic &robot, 
                               double Kvp, double  Kpp,
                               double Kvo, double Kpo)

Parameters:
ID - the engine id
robotID - the robot id
Kvp - end effector velocity gain
Kpp - end effector position error gain
Kvo - end effector orientation angular rate gain
Kpo - end effector orientation error gain

getPosErrorGCOMP

protected double[] getPosErrorGCOMP(short ID)
Gets the position error.

Call C++ Gravity_comp method

get_pos_error()

Parameters:
ID - the engine id
Returns:
the pos error array.

getQuatErrorGCOMP

protected double[] getQuatErrorGCOMP(short ID)
Gets the quaternion error.

Call C++ Gravity_comp method

get_quat_error()

Parameters:
ID - the engine id
Returns:
the quaternion error array.

torqueCmdGCOMP

protected double[] torqueCmdGCOMP(short ID,
                                  short robotID,
                                  double[] pd,
                                  double[] qd,
                                  short linkPc)
Sets the output torque for a desired end effector position and angular position.

Call C++ Gravity_comp method

torque_cmd(Robot_basic &robot,
                                     const ColumnVector &pd, const Quaternion &qd,
                                     const short link_pc, const Real dt)

Parameters:
ID - the engine id
robotID - the robot id
pd - desired position
qd - desired vector angular position: quaternion (s,v1,v2,v3)
linkPc - link of which control position
Returns:
the output torque

torqueCmdGCOMP

protected double[] torqueCmdGCOMP(short ID,
                                  short robotID,
                                  double[] pd)
Sets the output torque for a desired end effector position.

Call C++ Gravity_comp method

torque_cmd(Robot_basic &robot,
                                     const ColumnVector &pd)

Parameters:
ID - the engine id
robotID - the robot id
pd - desired position
Returns:
the output torque

createImpedance

protected void createImpedance(short ID,
                               short robotID,
                               double[] Mp,
                               double[] Dp,
                               double[] Kp,
                               double[] Km,
                               double[] Mo,
                               double[] Do,
                               double[] Ko)
Parameters:
ID -
robotID -
Mp -
Dp -
Kp -
Km -
Mo -
Do -
Ko -

impedanceControl

protected short impedanceControl(short ID,
                                 short robotID,
                                 double[] pdpp,
                                 double[] pdp,
                                 double[] pd,
                                 double[] wdp,
                                 double[] wd,
                                 double[] qd,
                                 double[] f,
                                 double[] n,
                                 double dt)
Parameters:
ID -
robotID -
pdpp -
pdp -
pd -
wdp -
wd -
qd -
f -
n -
dt -
Returns:
null

createSplinePath

protected void createSplinePath(short ID,
                                int spaceDim,
                                int nPoints,
                                double[] array)
Spline Path class constructor from ROBOOP file.

Parametric cubic splines interpolation of points in cartesian space or joints space, needs at least four points.

Call C++ constructor Spl_path(const Matrix &x)

Parameters:
ID - the engine id.
spaceDim - the dimension of cartesian space or th joints space.
nPoints - the number of points of trajectory
array - the (spaceDim+1)-by-npoints array: 1st row the times, 2nd and next the points

interpolate

protected double[] interpolate(short ID,
                               double t)
Interpolates the spline at time t to set the position, velocity and acceleration.

Call C++ Spl_path method

p_pdot_pddot(const Real time, ColumnVector &p, ColumnVector &dp, ColumnVector &ddp)

It modifies error flag.

Parameters:
ID - the engine id.
t - time
Returns:
the array p & pp & ppp

createSplineQuaternion

protected void createSplineQuaternion(short ID,
                                      int nPoints,
                                      double[] array)
Spline Quaternion class constructor from array.

Parametric cubic splines interpolation of quaternions.

Call C++ constructor
Spl_Quaternion(const quat_map &quat)

Parameters:
ID - the engine id.
nPoints - the number of trajectory points.
array - the 5-by-nPoints array: [1st point (time s vx vy vz)] & [2nd point (time s vx vy vz)] & ...

interpolateQuat

protected double[] interpolateQuat(short ID,
                                   double t)
Interpolates the spline at time t to set the quaternion and angular velocity.

Call C++ Spl_Quaternion method

  quat_w(const Real time, Quaternion & q, ColumnVector &wd)
 

It modifies error flag.

Parameters:
ID - the engine id.
t - time
Returns:
the array q & wd

interpolatePathQuat

protected double[] interpolatePathQuat(short ID,
                                       double t)
Interpolates the spline at time t to set the position, velocity, acceleration, the quaternion and angular velocity.

Call C++ Spl_path and Spl_Quaternion methods:

 p_pdot_pddot(const Real time, ColumnVector &p, ColumnVector &dp, ColumnVector &ddp)
 quat_w(const Real time, Quaternion & q, ColumnVector &wd)
 

It modifies error flag.

Parameters:
ID - the engine id.
t - the time
Returns:
the array of p & pp & ppp & q & wd