The Robot and mRobot classes are composed of the following data elements:
and the member functions providing the different algorithms implementation (see tables 2.2–2.17).
The Link class (see table 2.1) encapsulates all the data and functionality required to characterize a single “link” as it is defined by Denavit and Hartenberg (standard notation [3], or modified notation [4]). It is initialized by providing the joint type (int joint_type: revolute=0, prismatic=1) and the parameters θ, d, a, α (Real theta, d, a, alpha) and a boolean value Bool DH (true=standard false=modified) It also contains the inertial parameters data: mass m (Real m), center of mass position vector r (ColumnVector r) and inertia tensor matrix Ic (Matrix I). In this case, r is given with respect to the link coordinate frame and Ic is with respect to a coordinate frame parallel to the link coordinate frame and located at the center of mass of m. The dynamic model takes into account the motors inertia, gear ratio and frictions. The values Im and Gr representing respectively the motors rotor inertia Im and gear ratio Gr; B and Cf representing respectively the motors viscous B and Coulomb friction Cf coefficients:
Kinematic | Inertial | Motor | |||
int | joint_type | Real | m | Real | Im |
Real | theta, d, a, alpha | ColumnVector | r | Real | Gr |
Real | joint_offset | Matrix | I | Real | B |
ColumnVector | p | Real | Cf | ||
Matrix | R, | ||||
Bool | DH | ||||
Real | theta_min, theta_max | ||||
Real | joint_offset | ||||
On initialization, the constructor sets up the matrices R and p such that
If the link corresponds to a revolute (prismatic) joint, then only θ (d) can be changed after the link definition. This is done through the member function transform which sets the new value of q (θ or d) and updates the matrices R and p which compose the link homogeneous transform:
The Robot and mRobot classes provide a default constructor that creates a 1 dof robot. A ndof × 19 matrix containing the kinematic and inertial parameters (as for the Robot class) can be supplied upon initialization. A ndof × 19 matrix containing the kinematic and inertial parameters (as for the Robot class) can be supplied along with a ndof × 4 matrix providing the motors inertia, gear ratio and friction coefficients. A ndof × 23 matrix (kinematic, inertial and motor parameters) can also be used. The structure of the initialization matrix is:
Column | Variable | Description |
1 | σ | joint type (revolute=0, prismatic=1) |
2 | θ | Denavit-Hartenberg parameter |
3 | d | Denavit-Hartenberg parameter |
4 | a | Denavit-Hartenberg parameter |
5 | α | Denavit-Hartenberg parameter |
6 | θmin | minimum value of joint variable |
7 | θmax | maximum value of joint variable |
8 | θoff | joint offset |
9 | m | mass of the link |
10 | c x | center of mass along axis x |
11 | cy | center of mass along axis y |
12 | cz | center of mass along axis z |
13 | Ixx | element xx of the inertia tensor matrix |
14 | Ixy | element xy of the inertia tensor matrix |
15 | Ixz | element xz of the inertia tensor matrix |
16 | Iyy | element yy of the inertia tensor matrix |
17 | Iyz | element yz of the inertia tensor matrix |
18 | Izz | element zz of the inertia tensor matrix |
19 | Im | motor rotor inertia |
20 | Gr | motor gear ratio |
21 | B | motor viscous friction coefficient |
22 | C f | motor Coulomb friction coefficient |
23 | immobile | flag for the kinematics and inverse kinematics |
(if true joint is locked, if false joint is free) |
Standard notation:
Modified notation:
Robot and mRobot object constructors, copy constructor and equal operator.
None
These functions return a column vector containing the joint variables (get_q), velocities (get_qp) or accelerations (get_qpp) when called with no argument. It returns the scalar value for the ith joint variable when called with an integer argument.
ColumnVector or Real
These functions set the joint variables (velocities or accelerations) or the ith joint variable (velocity or acceleration) to q (qp or qpp).
None
The forward kinematic model defines the relation:
The inverse kinematic model is computed using a Newton-Raphson technique. If mj == 0, it is based on the following [6]:
Given the desired position represented by the homogeneous transform Tobj, this function return the column vector of joint variables that is corresponding to this position. On return, the value converge is true if the procedure has converge to values that give the correct position and false otherwise.
Note: mj == 0 is faster (≈ 1.8×) than mj == 1. Also, mj == 1 might converge when mj == 0 does not.
ColumnVector
This function performs the Rhino robot inverse kinematics.
ColumnVector
This function performs the Puma robot inverse kinematics.
ColumnVector
The manipulator Jacobian defines the relation between the velocities in joint space and in the Cartesian space expressed in frame i:
This function returns iJ(q) (i = 0 when not specified) for the endlink (last link when not specified).
Matrix
The manipulator Jacobian time derivative can be used to compute the end effector acceleration due to joints velocities [9]:
where zi and i-1pn are expressed in the base frame and × is the vector cross product. Expressed in the ith frame, the Jacobian time derivative is given by
This function returns i(q,)(i=0 when not specified).
Matrix
This function returns the inverse Jacobian Matrix for 6 dof manipulator based on the Damped Least-Squares scheme [10]. Using the singular value decomposition, the Jacobian matrix is
Matrix
The forward kinematic model is provided by implementing the following recursion:
The overloaded function kine can return the orientation and position or the equivalent homogeneous transform for the last (if not supplied) or the ith link. For example:
are valid calls to the function kine.
Matrix or None (in this case Rot and pos are modified on output)
The forward kinematic model is provided by implementing the following recursion (similar to kine):
Matrix or None (in this case Rot, pos pos_dot are modified on output)
This function computes the partial derivatives:
Matrix or None (in this case dRot and dp are modified on output)
The robotics manipulator dynamic model is given by (see appendix A or [4])
This function computes from q, and τ which is the forward dynamics problem. Walker and Orin [11] presented methods to compute the inverse dynamics. A simplified RNE version computing
ColumnVector
This function computes the robot inertia matrix D(q). A simplified RNE version computing
Matrix
This function computes τ from q, and which is the inverse dynamics problem. The recursive Newton-Euler (RNE) formulation is one of the most computationally efficient algorithm [12, 13] used to solve this problem (see appendix A). The second form allows the inclusion the contribution of a load applied at the last link.
ColumnVector
This function computes τ from q and when = 0 and gravity is set to zero.
ColumnVector
The function G() computes τ from the gravity effect, while C() computes τ from the Coriolis and centrifugal effects.
ColumnVector for G and C
Murray and Neuman [13] have developed an efficient recursive linearized Newton-Euler formulation that can be used to compute (see appendix A)
This function computes
None (torque and dtorque are modified on output)
This function computes
None (torque and dtorque are modified on output)
This function computes
None (torque and dtorque are modified on output)
This function computes
Matrix
This function computes
Matrix
This function, which is not a member of any class, modifies randomly the robot parameters. The parameter variation in percentage is described by f.
None