This package provides a simple Kinematic Control for a 3-DOF (only revolute joints) manipulator with the following DH parameters:
| Link | Twist | Length | Offset | angle |
|---|---|---|---|---|
| 0 | 0 | 0 | - | - |
| 1 | pi/2 | 10 | - | t1 |
| 2 | 0 | 5 | - | t2 |
| 3 | 0 | 5 | - | t3 |
And the following joints' limits:
| Joint | Min | Max |
|---|---|---|
| 1 | -pi | pi |
| 2 | -pi/2 | pi/2 |
| 3 | -pi | pi |
The Robot class provides forward and inverse kinematics for the given manipulator.
There are three implemented solutions for the inverse kinematics problem:
There are two implemented solutions for the forward kinematics problem:
Unittests for all IK and FK are available.
All equations and math for each solution are documented in the source code.
It is important to note that the code is flexible enough to integrate new solutions,
because it explores the function object wrapper concept with lambda functions.
The Robot's settings can also be changed at runtime, by using the custom RemyRobotSettings struct.
A custom class Angles was implemented to deal with angle limits and to properly wrap an angle between [-pi,pi]. Therefore, the robot uses the custom RemyJoints struct, and joints manipulation are transparent. One may see the unittests to fully understand.
The Control has a model of the Robot and it provides Kinematic Control. There are two implemented solutions for the Kinematic Control:
- Proportional + Feedforward with DLS, which is a more generic control but requires some parameter tuning. It also has an extra term to avoid singularities (which is the case). Math is explained in the source file.
- Analytical, a project specific solution which uses the exact inverse kinematics.
As in the Robot file, the concept of function object wrapper was explored for flexibility.
The Control settings can be changed with the custom RemyControlSettings struct. The minimum value (50) for the control's frequency is verified.
The Control's constructor requires a string argument, which is the absolute path to the input (waypoints) file. It has a method to parse that file.
The Control has a shared_ptr to connection. In this particular setup and example, that shared_ptr<connection> is shared (as a weak_ptr) to the RoboticSystem for communication between threads.
Note: In this architecture, I am using threads and sharing resources through the connection class. An inter-process system would be a more real/generic approach here, but it is not in the scope of this task. You don't need to use ROS, however, and one of the faster solutions for a two publisher/subscriber architecture would be to use ZMQ, as already implemented by me in this other project here.
The start()/stop() methods initializes/pauses the communication with the RoboticSystem (sends/receives data through connections). For testing purposes, the start(weak_ptr<connection>) method requires the connection pointer of the RoboticSystem (see below in the RoboticSystem section). The start loop works as following:
- Check if connection is open and the resource is available (weak_ptr)
- Receive the joints values and convert it to float by pointer cast. The unittests of signal conversion are available here
- Compute new control signal
- Convert control signal to vector<char>
- Send control signal
The Controller frequency is 50Hz.
For better understanding, the Control Unittests are available here.
The waypoints are fed in the Controller, which generates a trajectory, a continuous and feasible path (cartesian coordinates for the end-effector) over time. A piecewise-linear interpolation of the points was chosen, then a constant velocity in cartesian space is acquired.
Note: The standard choice (and actually the choice for production) would be a 3DSpline composed of a third/fourth polynomial degree (at least continuous accelerations). It would be possible to evaluate and constraint torques. Since we are dealing with kinematics and no saturation, we kept it simple.
The update(t) method calculates the desired position/velocity for the end-effector. Unitests are available here.
The RoboticSystem mocks the robot (it has a Robot class), encoders, and it communicates with the Controller through connection. The System has it own parameters RemySystemSetting.
To start communication, one must pass the shared communication resource connection. See the test_integration. The main communication loop is available at start, and it works as following:
- Check if connection is open and the resource is available (weak_ptr)
- Move the manipulator with the last control input by the elapsed time
- Get the new manipulator joints from encoder, by mocking the Encoder precision lost
- Convert the joints value to vector<char> with pointer cast
- Send the data through connection
- Receive the control signal
The frequency is 1000 Hz.
The Configuration is a JSON file and it is read in the main file to set Control, Robot and System parameters.
The system uses the header-only file json.hpp for json parser. It also uses GTest for unittests and Eigen (Required) for all math.
To install and run the system, clone this repository and run, in the root folder:
mkdir build
cd build
cmake ..
make
Then, in the build folder, run (the two arguments are required):
./RemyRobotControl <path_to_input> <path_to_config>
The output file "out.csv" is composed of time, the end-effector path (x,y,z), the joints path (theta1, theta2, theta3) and control signal for each joint.
To run unittests, one just need to run them individually, for instance:
./RemyRobotControl_Test
The output, available in the "result" folder represents the following images:
