javax.robotics.engine
Class JRoboOp

java.lang.Object
  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