LabArm C++ - API

Lab Arm is a multipurpose robotic arm. It is aim to be equiped on drone, UAV and combined with AI to achieved various task as maintenance, fruits picking...

This github provide a simple c++ API to control the arm. The robotic arm use dynamixel servomotor for its 6 joints, the program was tested on a Raspberry PI 3B+, with the Dynamixel XM430 motors connected with U2D2 (Serial-RS485). This API is based on the developed c++ API for the XM430 servomotor available HERE. All the motors are using protocol 2 and a baudrate of 57600. The datasheet references for the motors can be found HERE.
If you are looking for a python version, please go to the following link : (https://github.com/rasheeddo/LabRobotArmOfficial)

Any resquest (new function to add) or issue report are very welcomed

Lastest Update: Switch to TIME_BASED driving mode. This allow to set a deadline on the motor displacement.

Currently implementing

I am currently working on:

• Joystick linear control
• Teach point robot
• Blind Object Detection
• Arm avoiding
  If you want other functions, please feel free to ask :)

Install && Dependencies

The programme depend on the dynamixel_sdk library. Installation information can be found on their github. Please install the proper library depending on your platform.
On Ubuntu, you may need to do a sudo chmod a+rw /dev/ttyUSB0

For Joystick control, we based our function on A minimal C++ object-oriented API onto joystick devices under Linux, but the library is available on this repository.

Once the install is done clone this repository, cd into the make_run directory.
Make the MakeFile, an run the code ./exampleArm
You can add other library to the project by adding: SOURCES += yourcode.cpp in the MakeFile.

Example Program

//LabArm declaration:
LabArm arm;
//LabArm initialisation to mode 3 (Position Control)
arm.MotorsInit(3);
	
//LabArm Torque activation
arm.TorqueON();
arm.GripperON();
			
//Moving the robot with the Awake-Standby-Home functions:
printf("\nArm awaking: \n");
arm.Awake();
	
printf("\nArm go to Standby position: \n");
arm.StandBy();	
printf("\nArm go to Home position\n");
arm.GoHome();
sleep(1);
	
//Moving the robot with XYZ coordonate: create a gripper postion table {X, Y, Z, rot_X, rot_Y, rot_Z} and run GotoXYZ.
float wantedposition[6] = {0, 340, 282, 10, 30, 0};
arm.GotoXYZ(wantedposition);
  
//Closing the gripper.
arm.GripperClose();
sleep(2);
	
//Going back home and disactivate the torque
arm.GoHome();
arm.TorqueOFF();
arm.GripperOFF();
	
//As we are using the motorMX430.h, we can also access motors function as follow:
arm.motor1.PrintOperatingMode();
arm.gripper.PrintOperatingMode();
	
//Joystick control Mode:
arm.JoystickControl();

API Description

Kinematics functions

• void RobotArmFWD(float motorAngle[ ], float positionGripper[ ]);
  RobotArmFWD: calcul the forward kinematics of the arm. Used to get the XYZ and rotation of the gripper,
  Input: motorAngle is an [6] float array which contain the 6 motors current position (degree), positionGripper[6] the float array of the result (passed by reference)

• void armINV(float wantedXYZ[ ], float outputMotorAngle[ ]);
  armINV: calcul the inverse kinematic of the arm. Used for moving the arm based on XYZ command.
  Input: wantedXYZ positon (mm | degree) for the gripper float array [6], outputMotor the [6] array which contained the motors angle (degree)

• void printMatrix(double matrix[][4], int size) && void printMatrix3(double matrix[][3], int size);
  printMatrix*: printf matrix contain in the console. Used for debugging.
  Input: matrix[4][4] or [3][3] depending on the function. (can be optimized into one function)

• void multiplyMatrix(double m1[][4], double m2[][4], double m12[][4], int size); &&void multiplyMatrix3(double m1[][3], double m2[][3], double m12[][3], int size);
  multiplyMatrix*: multiply 2 matrix. Used for INV-Foward kinetics function.
  Input: m1, m2 the 2 matrix to multiply. m12 the matrix result (passed by reference) (can be optimized into one function)

• void Solve(double A[][2], double B[], double X[]);
  Solve: solve a Ax = b linear system. Used for INV-Forward kinetics function. (direct resultion by Gaussian Elimination)
  Input: the A matrix system [2][2], b [2] array and X [2] array result (passed by reference)

Motion functions

• void MotorsInit(int mode);
  Init: set all the parameteres of the 7 motors depending on the wanted mode. The profile are all set to 0 (step motion).
  Input: wanted mode (Availiable mode: 3 (Position), 5 (current-based position). (othermode are not supported 0 (Current), 1 (Velocity), 4 (Extended position), 16 (PWM))

• void TorqueON();
  TorqueON: activate the torque inside all the 6 arm's motor. Without torque the arm will not move.

• void TorqueOFF();
  TorqueOFF: disactivate the torque inside all the 6 arm's motor. Without torque the arm will not move.

• void ReadArmCurrent( int motorCurrent[ ]);
  ReadArmCurrent: return all motor's current.Used for arm monitoring. The current value will be printed automatically, if unwanted delete the corresponding printf section in motorXM430.cpp
  Input: int array [6] passed by reference to get the current value for each motors.

• void ReadAngle(float outputAngle[ ]);
  ReadAngle: write the current angle of each motors (degree) in the passed array.
  Input: float [6] array to contain the 6 motors angles.

• void RunArm(float inputAngle[ ]);
  RunArm: write the goal position in the 6 motors. This function make the arm move, and wait until all motors stop moving.
  Input: inputAngle array[6] contain the wanted angle for each motors in degree.

• float MAP(uint32_t angle, long in_min, long in_max, long out_min, long out_max);
  MAP: return the mapped angle value into the other unit. Used for converting angle degree to angle motors.]
  Inputs: angle is the value we want to convert. in_min/max: the minimal/maximal angle value in the 1fst unit. out_min/max: the minimal/maximal angle value in the 2nd unit.
  Output: the mapped angle.
  Example: MAP(m_present_position, MIN_MOTOR_ANGLE, MAX_MOTOR_ANGLE, 0.0, 360.0)

• void GoHome();
  GoHome(): make the arm go to the home position (home1 -> home2). The function used the Home1 and Home2[ ] float array constitued of all the MOTOR__HOME of the #define section.

• void Awake();
  Awake(): make the arm go to the awake position (home2 -> home1). The function used the Home1 and Home2[ ] float array constitued of all the MOTOR__HOME of the #define section.

• void StandBy();
  StandBy: make the arm go to the Standby position. For now the standby position is hardcoded {180, 180, 180, 180, 180, 180} (maybe better to create a choice for the user.

• float DeltaPosition(float prePosition, float goalPosition);
  DeltaPosition: calcul the angle difference between the prePosition and the goal position. Used for trajectory calculation. (maybe more optimal to pass an array into that function)
  Input: the prePosition of the motor (usually the current position) and the goal position in degree.
  Output: the differences between the angle (degree)

• int FindMaxDelta(int deltaPosition[ ]);
  FindMaxDelta: search and return the array index of the biggest deltaposition. Used for trajectory calculation.
  Input: the deltaPosition[6] array which containt the differences between the the currentPosition and the goalposition
  Output: the array index corresponding to the bigges deltaposition.

TrajectoryGeneration is outdated, please use TimeProfileGeneration

• void TrajectoryGeneration(float goalPosition[ ], float Vstd, float Astd);
  TrajectoryGeneration: set all motor's profile (velocity/acceleration) based on the current and goal position. Used to generata smooth movement of the arm.
  Input: the goal position array[6] containing the wanted position of the 6 motors, the based velocity-acceleration for each motors

• void TimeProfileGeneration(float goalPosition[ ], uint32_t stdTa, uint32_t stdTf);
  TimeProfileGeneration: set the timeprofile of the arm. Used to time the smooth movement of the arm. Currently constant, but maybe implemented as prop of total angular distance
  Input: the goal position array[6] containing the wanted position of the 6 motors, the based velocity-acceleration for each motors

• void Goto(float goalPosition[ ], int generateTrajectory, uint32_t Vstd, uint32_t Astd);
  Goto: make the arm go to the wanted position. If generateTrajectory is 1, the generated movement will be smooth, if generateTrajectory is 0, all the motors will have the same profile (Vstd, Astd)
  Input: the goal position array[6] containing the wanted position of the 6 motors, the option for smooth movement, the standart profile for the motors.
  Example: Goto(SavedPosition, 1, 40, 20) (a profile of 40, 20 is standard)

• int WorkSpaceLimitation(float X, float Y, float Z);
  WorkSpaceLimitation: check if the wanted X,Y,Z position are in the working space of the arm. The workingspace parameter are fixed in the #define section
  Ouput: return 0: XYZ are in the working space / return 1: out of work range in quadrant 2 or 3 / return 2: Z is lower than the lowest range HMAX / return 3: // Y, Z axe is negatif / return4: X,Y,Z point is not in not in the workspace

• int WorkSpaceHorizontalLimitation(float X, float Y, float Z);
  WorkSpaceHorizontalLimitation: check if the wanted X,Y,Z position are in the horizontal working space of the arm. The workingspace parameter are fixed in the #define section. Used to keep a camera horizontal.
  Ouput: return 0: XYZ are in the working space / return 1: Input out of the XZ plane / return 2: Y out of range / return 3: one of the input if negatif.

• void GetXYZ(float gripperPosition[ ]);
  GetXYZ: calcul the inverve kinematics of the arm and write the result in passed array.
  Input: gripperPosition array[6] result of the calculation. The array is construted as gripperPosition[ * ] = {X, Y, Z, x6_rot, y6_rot, z6_rot}

• void GotoXYZ(float wantedXYZ[ ]);
  GotoXYZ: make the arm go to the wanted XYZ position. The arm moves only if the XYZ are in the working space
  Input the wantedXYZ array contening the wanted gripperPosition {X, Y, Z, x6_rot, y6_rot, z6_rot}.

Gripper functions

• void GripperON();
  GripperON: activate the torque in gripper's motor. Without torque the gripper will not close or open.

• void GripperOFF();
  GripperOFF: disactivate the torque in gripper's motor. Without torque the gripper will not close or open.

• void GripperOpen() && void GripperClose();
  GripperOpen-close: make the gripper close or open. The function check the status of the gripper to see if it is already in the wanted state. The goal position for the close-open can be modified in the #define section.

• int GripperGetCurrent();
  GripperGetCurrent: read the current inside the gripper's motor. Not used yet but can be use for object size and/or object material recognition.

• float GetSize();
  GetSize(): return the size of the gripped object based on experimental calibration
  Output: float size in mm.

• float AverageCurrent(int n);
  AverageCurrent: return the current in the motor5 averaged on N measurment. Used for weight evaluation.
  The float current in Motor's Unit.

• float Weight();
  Weight: move the arm into the weight positon and estimat the weight of the grabbed object. (This function is still underdevelopment)
  Output: the averaged weight of the object over 5 measurment.

• float Thoughness();
  Toughness: try to estimate the thoughness of the grabbed object. (This function is still underdevelopment)
  Output: the averaged estimated thoughness over 5 measurment.

Joystick control

• int FindSelectedMotor(uint8_t buttonstate[ ]);
  FindSelectedMotor: return the id corresponding to a high state of the array buttonstate. Used in JoystickControl to find the join to move.
  Input: the buttonstate array contening updated state of the joystick button

• int JoystickControl();
  JoystickControl: allow you to control the arm with a joystick. The control is done 1 join at the time. (This function code is overcomplicated due to issue on reading event from the joystick (axis event oversampled))
  To move a join you need to keep pressed the angle corresponding to the join (see table) and move the left joystick right or left.