This library contains the different components of the r3 robot. Usually, there is no need to use this library directly.
Extends from Modelica.Icons.Package
(Icon for standard packages).
Name | Description |
---|---|
AxisControlBus | Data bus for one robot axis |
AxisType1 | Axis model of the r3 joints 1,2,3 |
AxisType2 | Axis model of the r3 joints 4,5,6 |
ControlBus | Data bus for all axes of robot |
Controller | P-PI cascade controller for one axis |
GearType1 | Motor inertia and gearbox model for r3 joints 1,2,3 |
GearType2 | Motor inertia and gearbox model for r3 joints 4,5,6 |
MechanicalStructure | Model of the mechanical part of the r3 robot (without animation) |
Motor | Motor model including current controller of r3 motors |
PathPlanning1 | Generate reference angles for fastest kinematic movement |
PathPlanning6 | Generate reference angles for fastest kinematic movement |
PathToAxisControlBus | Map path planning to one axis control bus |
Signal bus that is used to communicate all signals for one axis. This is an expandable connector which has a "default" set of signals. Note, the input/output causalities of the signals are determined from the connections to this bus.
Extends from Modelica.Icons.SignalSubBus
(Icon for signal sub-bus).
Type | Name | Description |
---|---|---|
Boolean | motion_ref | = true, if reference motion is not in rest |
Angle | angle_ref | Reference angle of axis flange |
Angle | angle | Angle of axis flange |
AngularVelocity | speed_ref | Reference speed of axis flange |
AngularVelocity | speed | Speed of axis flange |
AngularAcceleration | acceleration_ref | Reference acceleration of axis flange |
AngularAcceleration | acceleration | Acceleration of axis flange |
Current | current_ref | Reference current of motor |
Current | current | Current of motor |
Angle | motorAngle | Angle of motor flange |
AngularVelocity | motorSpeed | Speed of motor flange |
Signal bus that is used to communicate all signals of the robot. This is an expandable connector which has a "default" set of signals. Note, the input/output causalities of the signals are determined from the connections to this bus.
Extends from Modelica.Icons.SignalBus
(Icon for signal bus).
Type | Name | Description |
---|---|---|
AxisControlBus | axisControlBus1 | Bus of axis 1 |
AxisControlBus | axisControlBus2 | Bus of axis 2 |
AxisControlBus | axisControlBus3 | Bus of axis 3 |
AxisControlBus | axisControlBus4 | Bus of axis 4 |
AxisControlBus | axisControlBus5 | Bus of axis 5 |
AxisControlBus | axisControlBus6 | Bus of axis 6 |
Given
this component computes the fastest movement under the given constraints. This means, that:
The acceleration, constant velocity and deceleration phase are determined in such a way that the movement starts form the start angles and ends at the end angles. The output of this block are the computed angles, angular velocities and angular acceleration and this information is stored as reference motion on the controlBus of the r3 robot.
Extends from Modelica.Blocks.Icons.Block
(Basic graphical layout of input/output block).
Type | Name | Default | Description |
---|---|---|---|
Real | angleBegDeg | 0 | Start angle |
Real | angleEndDeg | 1 | End angle |
AngularVelocity | speedMax | 3 | Maximum axis speed |
AngularAcceleration | accMax | 2.5 | Maximum axis acceleration |
Time | startTime | 0 | Start time of movement |
Time | swingTime | 0.5 | Additional time after reference motion is in rest before simulation is stopped |
final Angle | angleBeg | Cv.from_deg(angleBegDeg) | Start angles |
final Angle | angleEnd | Cv.from_deg(angleEndDeg) | End angles |
Type | Name | Description |
---|---|---|
ControlBus | controlBus |   |
Given
this component computes the fastest movement under the given constraints. This means, that:
The acceleration, constant velocity and deceleration phase are determined in such a way that the movement starts form the start angles and ends at the end angles. The output of this block are the computed angles, angular velocities and angular acceleration and this information is stored as reference motion on the controlBus of the r3 robot.
Extends from Modelica.Blocks.Icons.Block
(Basic graphical layout of input/output block).
Type | Name | Default | Description |
---|---|---|---|
Integer | naxis | 6 | Number of driven axis |
Real | angleBegDeg[naxis] | zeros(naxis) | Start angles |
Real | angleEndDeg[naxis] | ones(naxis) | End angles |
AngularVelocity | speedMax[naxis] | fill(3, naxis) | Maximum axis speed |
AngularAcceleration | accMax[naxis] | fill(2.5, naxis) | Maximum axis acceleration |
Time | startTime | 0 | Start time of movement |
Time | swingTime | 0.5 | Additional time after reference motion is in rest before simulation is stopped |
final Angle | angleBeg[:] | Cv.from_deg(angleBegDeg) | Start angles |
final Angle | angleEnd[:] | Cv.from_deg(angleEndDeg) | End angles |
Type | Name | Description |
---|---|---|
ControlBus | controlBus |   |
This model stores the 4 reference variables q, qd, qdd, moving from the path planning on the axis control bus.
Extends from Modelica.Blocks.Icons.Block
(Basic graphical layout of input/output block).
Type | Name | Default | Description |
---|---|---|---|
Integer | nAxis | 6 | Number of driven axis |
Integer | axisUsed | 1 | Map path planning of axisUsed to axisControlBus |
Type | Name | Description |
---|---|---|
input RealInput | q[nAxis] |   |
input RealInput | qd[nAxis] |   |
input RealInput | qdd[nAxis] |   |
AxisControlBus | axisControlBus |   |
input BooleanInput | moving[nAxis] |   |
Models the gearbox used in the first three joints with all its effects, like elasticity and friction. Coulomb friction is approximated by a friction element acting at the "motor"-side. In reality, bearing friction should be also incorporated at the driven side of the gearbox. However, this would require considerable more effort for the measurement of the friction parameters. Default values for all parameters are given for joint 1. Model relativeStates is used to define the relative angle and relative angular velocity across the spring (=gear elasticity) as state variables. The reason is, that a default initial value of zero of these states makes always sense. If the absolute angle and the absolute angular velocity of model Jmotor would be used as states, and the load angle (= joint angle of robot) is NOT zero, one has always to ensure that the initial values of the motor angle and of the joint angle are modified correspondingly. Otherwise, the spring has an unrealistic deflection at initial time. Since relative quantities are used as state variables, this simplifies the definition of initial values considerably.
Extends from Modelica.Mechanics.Rotational.Icons.Gearbox
(Icon of a gear box) and Modelica.Mechanics.Rotational.Interfaces.PartialTwoFlanges
(Partial model for a component with two rotational 1-dim. shaft flanges).
Type | Name | Default | Description |
---|---|---|---|
Real | i | -105 | Gear ratio |
Real | c | 43 | Spring constant |
Real | d | 0.005 | Damper constant |
Torque | Rv0 | 0.4 | Viscous friction torque at zero velocity |
Real | Rv1 | 8.125e-4 | Viscous friction coefficient (R=Rv0+Rv1*abs(qd)) |
Real | peak | 1 | Maximum static friction torque is peak*Rv0 (peak >= 1) |
Type | Name | Description |
---|---|---|
Flange_a | flange_a | Flange of left shaft |
Flange_b | flange_b | Flange of right shaft |
The elasticity and damping in the gearboxes of the outermost three joints of the robot is neglected. Default values for all parameters are given for joint 4.
Extends from Modelica.Mechanics.Rotational.Icons.Gearbox
(Icon of a gear box) and Modelica.Mechanics.Rotational.Interfaces.PartialTwoFlanges
(Partial model for a component with two rotational 1-dim. shaft flanges).
Type | Name | Default | Description |
---|---|---|---|
Real | i | -99 | Gear ratio |
Torque | Rv0 | 21.8 | Viscous friction torque at zero velocity |
Real | Rv1 | 9.8 | Viscous friction coefficient in [Nms/rad] (R=Rv0+Rv1*abs(qd)) |
Real | peak | 1.224770642201835 | Maximum static friction torque is peak*Rv0 (peak >= 1) |
Type | Name | Description |
---|---|---|
Flange_a | flange_a | Flange of left shaft |
Flange_b | flange_b | Flange of right shaft |
Default values are given for the motor of joint 1. The input of the motor is the desired current (the actual current is proportional to the torque produced by the motor).
Extends from Modelica.Electrical.Machines.Icons.Machine
(Generic icon of an electric machine).
Type | Name | Default | Description |
---|---|---|---|
Inertia | J | 0.0013 | Moment of inertia of motor |
Real | k | 1.1616 | Gain of motor |
Real | w | 4590 | Time constant of motor |
Real | D | 0.6 | Damping constant of motor |
AngularVelocity | w_max | 315 | Maximum speed of motor |
Current | i_max | 9 | Maximum current of motor |
Type | Name | Description |
---|---|---|
Flange_b | flange_motor |   |
AxisControlBus | axisControlBus |   |
This controller has an inner PI-controller to control the motor speed, and an outer P-controller to control the motor position of one axis. The reference signals are with respect to the gear-output, and the gear ratio is used in the controller to determine the motor reference signals. All signals are communicated via the "axisControlBus".
Extends from Modelica.Blocks.Icons.Block
(Basic graphical layout of input/output block).
Type | Name | Default | Description |
---|---|---|---|
Real | kp | 10 | Gain of position controller |
Real | ks | 1 | Gain of speed controller |
Time | Ts | 0.01 | Time constant of integrator of speed controller |
Real | ratio | 1 | Gear ratio of gearbox |
Type | Name | Description |
---|---|---|
AxisControlBus | axisControlBus |   |
Model of axis 1, 2, 3 of the robot r3. An axis consists of a gearbox with modelled gear elasticity and bearing friction, a model of the electrical motor and a continuous-time cascade controller.
Extends from Modelica.Mechanics.MultiBody.Examples.Systems.RobotR3.Components.AxisType2
(Axis model of the r3 joints 4,5,6).
Type | Name | Default | Description |
---|---|---|---|
Real | kp | 10 | Gain of position controller |
Real | ks | 1 | Gain of speed controller |
Time | Ts | 0.01 | Time constant of integrator of speed controller |
Real | k | 1.1616 | Gain of motor |
Real | w | 4590 | Time constant of motor |
Real | D | 0.6 | Damping constant of motor |
Inertia | J | 0.0013 | Moment of inertia of motor |
Real | ratio | -105 | Gear ratio |
Torque | Rv0 | 0.4 | Viscous friction torque at zero velocity in [Nm] |
Real | Rv1 | 8.125e-4 | Viscous friction coefficient in [Nms/rad] |
Real | peak | 1 | Maximum static friction torque is peak*Rv0 (peak >= 1) |
Real | c | 43 | Spring constant |
Real | cd | 0.005 | Damper constant |
Type | Name | Description |
---|---|---|
Flange_b | flange |   |
AxisControlBus | axisControlBus |   |
The axis model consists of the controller, the motor including current controller and the gearbox including gear elasticity and bearing friction. The only difference to the axis model of joints 4,5,6 (= model axisType2) is that elasticity and damping in the gear boxes are not neglected.
The input signals of this component are the desired angle and desired angular velocity of the joint. The reference signals have to be "smooth" (position has to be differentiable at least 2 times). Otherwise, the gear elasticity leads to significant oscillations.
Default values of the parameters are given for the axis of joint 1.
Type | Name | Default | Description |
---|---|---|---|
Real | kp | 10 | Gain of position controller |
Real | ks | 1 | Gain of speed controller |
Time | Ts | 0.01 | Time constant of integrator of speed controller |
Real | k | 1.1616 | Gain of motor |
Real | w | 4590 | Time constant of motor |
Real | D | 0.6 | Damping constant of motor |
Inertia | J | 0.0013 | Moment of inertia of motor |
Real | ratio | -105 | Gear ratio |
Torque | Rv0 | 0.4 | Viscous friction torque at zero velocity in [Nm] |
Real | Rv1 | 8.125e-4 | Viscous friction coefficient in [Nms/rad] |
Real | peak | 1 | Maximum static friction torque is peak*Rv0 (peak >= 1) |
Type | Name | Description |
---|---|---|
Flange_b | flange |   |
AxisControlBus | axisControlBus |   |
This model contains the mechanical components of the r3 robot (multibody system).
Type | Name | Default | Description |
---|---|---|---|
Boolean | animation | true | = true, if animation shall be enabled |
Mass | mLoad | 15 | Mass of load |
Position | rLoad[3] | {0, 0.25, 0} | Distance from last flange to load mass> |
Acceleration | g | 9.81 | Gravity acceleration |
Type | Name | Description |
---|---|---|
Flange_a | axis1 |   |
Flange_a | axis2 |   |
Flange_a | axis3 |   |
Flange_a | axis4 |   |
Flange_a | axis5 |   |
Flange_a | axis6 |   |
Generated 2018-12-12 12:12:41 EST by MapleSim.