Let us name the links starting from below. The various link lengths and masses are as follows.
L1= 6.7 cm m1=0.4kg
L2= 9cm m2=0.2kg
L3=11.1cm m3=0.5kg
L4=7.5 cm m4=0.4kg
Find out the joint configuration for which the gravity vector would have the maximum effect on each joint motor. Does the gravity vector have any effect on the base motor? You can assume that the gravity vector acts at the center of each link.If the gearing ratio on the motor is 670 find out the resultant torque on each motor due to gravity in this configuration?
Solution:
The configuration in which the maximum load acts on each motor is shown below. The gravity acts as an offset axial load for the base motor and does not create any load torque in the direction of rotation for that motor.
For the other motors the Torque load is given by
T4=(m4*L4/2)*9.81=0.4*9.81*0.075/2= 0.147 Nm
Reflected load on the motor= 0.147/Gearing=0.147/670= 0.000219 Nm
T3= (m4*(L4/2+L3)+m3*(L3/2))*9.81= 0.854 Nm
Reflected load on the motor= 0.147/Gearing=0.854/670= 0.00127 Nm
Open loop trajectory following 4-link open chain manipulator
Project description
This project involves the use of a think-geek toy robot controlled by an Arduino Duemilanove to perform open loop trajectory following. The control algorithm is based on a kinematic model of the robot that was derived using the DH parameters and differential kinematics. Since differential kinematics can be quite memory intensive as it involves substantial matrix algebra and as joint angles need to be updated quickly enough, it was decided to use the arduino-matlab interface which would ensure that all computation was done by the machine on which matlab was running and then data would be written to the arduino.
The robot consists of 5 DC motors with reduction gears, four for the joints and one for clamp action. We used Pololu H-Bridges and a 5V mean-well power supply to drive the motors.
Objectives
Our main objective in this project was to get the robot to do trajectory following. The idea is to parametrize a spatial curve with time using the appropriate boundary conditions. Once the curve has been parametrized, we use concepts from differential kinematics (the Jacobian) to relate joint velocities to the end effector velocities which we obtained by differentiating the path’s trajectory. As our toy robot has no feedback sensors on it, it is not possible to estimate the states of the system. Also, our algorithm is based purely on kinematics. The system dynamics have not been taken into account. As a result, the system is difficult to stabilize in open loop, and some drift was expected in the path following.
The other objective was…to have fun!
Robot Model
Following are the DH parameters for the robot:
Link
Ó¨
d (cm)
a (cm)
α
1
Ó¨1*+Ï€/2
6.7
0
Ï€/2
2
Ó¨2*
0
9
0
3
Ó¨3*
0
11.1
0
4
Ó¨4*
0
7.5
0
Simple model
Kinematic Model
The homogeneous transforms were obtained for each transformed frame. They are as follows:
Control scheme
The end effector velocities are related to the joint velocities of the robot through the Jacobian matrix as follows:
Once the desired velocity vector is obtained for a particular time step, it is multiplied with the Jacobian inverse to yield the desired joint velocities.As our system consists of a non-square Jacobian, we need to perform the pseudo inverse operation to yield the joint velocities. The pseudo inverse, which is also called the J_sword, is given by
The Gain from velocity to voltage conversions are calculated by characterizing the motors as shown in the implementation.
Jacobian
In order to calculate the Jacobian at every time step, we also require knowing the current joint angles. We perform Euler integration on the joint velocities to solve for joint angles, which results in some amount of drift but can be reasonably low if the time step is small. The first time step uses initial conditions of the joint angles to evaluate the Jacobian.
Once the joint velocities are obtained for a particular time step, these need to be converted to equivalent voltages to the motors through the motor characterization. Once scaled, they can be written to the PWM pins of the arduino and the control input is thus given to the motors on the robot.
Implementation
We had to first find a reasonably accurate relation between the voltage input and angular velocity of the motors. The arduino PWM pins can be used such that a number from 0-255 scales the voltage from 0-5V to the motor. We assume here that the load on the motors remains roughly constant and the motors run with constant angular velocity. To obtain an accurate relationship, we calculated the time required for Motor 2 to complete 90 degrees and obtained a value for angular velocity in relation to the voltage input. We did this for several points between 0-255 and obtained the following characteristic curve:
We then fitted a linear relationship which we found to be Vin=9*w where,
Vin=Voltage (from 0-5V)
w=angular velocity (r/s)
Now that we had our motor characteristic curve, we could code our control system. Once the voltages were calculated for a particular time step, they had to be scaled between 0-255 in order to be sent as a signal via the arduino. A saturation condition was incorporated so that the value of voltage can never exceed 5V as this was the power supply voltage. If the voltage turned out to be negative, it was made positive so that it could be assigned as an argument in the Analogwrite function of arduino and the direction pin was assigned a flag variable whose value depended on the sign of the voltage. The voltages were then rounded off as the writing function cannot take non-integer values as arguments.
We decided to pre-calculate for every time step, the Jacobian and desired angular velocities so that the loop for assigning motor voltages ran faster.
One of the most critical parts in successfully implementing this algorithm was that our time steps had to match the loop rate of voltage assignment to the motors so that the correct values of voltages were sent to the motors at the correct time step and for the correct amount of time. Once these were matched, we were able to successfully implement the trajectory following algorithm with little drift because of lack of dynamics and most importantly, lack of feedback.
We also had to add a bias voltage command to offset the initial friction effects, as very low values of voltage command did not result into any motion of the joints.
Angle variations for straight line trajectory (See Video 1)
After doing the straight line, we implemented a simple 3 dimensional path, which was a sum of 3 straight lines in space. The path had sharp points, thus we designed the trajectory such that the end effector comes to rest at these sharp points because we cannot maintain first order continuity at points, which are non-differentiable (See Video 2)
The third trajectory that we implemented was a 3 dimensional piecewise continuous path of semi-circles and straight lines. Here we could design the trajectory such that the end effector velocity remained continuous unlike the 2nd trajectory, which is required to avoid conflicts at the end points of the pieces in the path.
To generate some of the trajectories we used sinusoidal functions instead of polynomials. Generating and modifying trajectories with sinusoidal functions is much easier compared to a polynomial function. The sinusoidal functions are infinitely differentiable unlike polynomial functions, and hence can generate very smooth trajectories.
Velocity profiles of end effector in Trajectory 3 (See video 3)
It can be seen above that the velocity profile of the end effector is continuous throughout the sum of paths.
