Merge branch 'develop'
This commit is contained in:
commit
a0c30188f0
151
README.md
151
README.md
|
|
@ -1 +1,150 @@
|
|||
Repository of Introduction to Robotics Lab Robotic arm.
|
||||
# Overview
|
||||
|
||||
Each of the following codes allows a specific directive to be sent to a 2D planar robot:
|
||||
|
||||
**[jointControl](#jointControl)**: Drives the robot using joint values.
|
||||
|
||||
**[cartControl](#cartControl)**: Drives the robot using cartesian values.
|
||||
|
||||
**[testKinematics](#testKinematics)**: Tests each kinematic algorithm and makes sure they are at the expected value.
|
||||
|
||||
**[linearControl](#linearControl)**: Drives the robot using cartesian values in a linear trajectory.
|
||||
|
||||
**[drawImage](#drawImage)**: Draws an input image.
|
||||
|
||||
**[linearControlFurther](#linearControlFurther)**: Improvement of the linear control which checks for the determinant of the Jacobian to be valid.
|
||||
|
||||
# Repository Structure
|
||||
|
||||
### jointControl/
|
||||
|
||||
* @file jointControl.cpp
|
||||
* @author Guillaume Gibert
|
||||
* @version -
|
||||
* @date 2025/04/13
|
||||
* @brief Description : This file was provided by G. Gibert. It allows to test different kinds of kinematics.
|
||||
* @functions :
|
||||
- void initRobot(DynamixelHandler& dxlHandler, std::string portName, float protocol, int baudRate)
|
||||
- void closeRobot(DynamixelHandler& dxlHandler)
|
||||
- int main()
|
||||
|
||||
### cartControl/
|
||||
* @file cartControl.cpp
|
||||
* @author Charles STELANDRE
|
||||
* @version V1
|
||||
* @date 2025/04/13
|
||||
* @brief Description : This file is a variant from jointControl, adapting cartesian Coordinates translation.
|
||||
* @functions :
|
||||
* @ - void initRobot(DynamixelHandler& dxlHandler, std::string portName, float protocol, int baudRate)
|
||||
- void closeRobot(DynamixelHandler& dxlHandler)
|
||||
- int main(int argc, char** argv)
|
||||
* Input:
|
||||
* - L1 : Length of the first link (cm)
|
||||
* - L2 : Length of the second link (cm)
|
||||
* - x : x-component of target position (cm)
|
||||
* - y : y-component of target position (cm)
|
||||
*
|
||||
* Output:
|
||||
* - Displacement of the robot using cartesian coordinates.
|
||||
|
||||
|
||||
### testKinematics/
|
||||
|
||||
* @file testKinematics.cpp
|
||||
* @author Guillaume Gibert
|
||||
* @version -
|
||||
* @date 2025/04/13
|
||||
* @brief Description : This file was provided by G. Gibert. It allows to test different kinds of kinematics.
|
||||
* @functions :
|
||||
- bool testFK(float q1, float q2, float L1, float L2, std::vector<float> expectedEndEffectorPosition, float errorThreshold)
|
||||
- bool testIK(float x, float y, float L1, float L2, std::vector<float> expectedJointValues, float errorThreshold)
|
||||
- int main()
|
||||
|
||||
|
||||
### linearControl/
|
||||
* @file linearControl.cpp
|
||||
* @author Guillaume Gibert
|
||||
* @version -
|
||||
* @date 2025/04/13
|
||||
* @brief Description : This file was provided by G. Gibert. It allows to use a proportionally controlled system to join two points in a linear trajectory.
|
||||
* @functions :
|
||||
* @ - void initRobot(DynamixelHandler& dxlHandler, std::string portName, float protocol, int baudRate)
|
||||
- void closeRobot(DynamixelHandler& dxlHandler)
|
||||
- int main(int argc, char** argv)
|
||||
* Input:
|
||||
* - L1 : Length of the first link (cm)
|
||||
* - L2 : Length of the second link (cm)
|
||||
* - x : x-component of target position (cm)
|
||||
* - y : y-component of target position (cm)
|
||||
*
|
||||
* Output:
|
||||
* - Displacement of the robot using cartesian coordinates.
|
||||
|
||||
### drawImage/
|
||||
|
||||
* @file drawImage.cpp
|
||||
* @author Guillaume Gibert
|
||||
* @version -
|
||||
* @date 2025/04/13
|
||||
* @brief Description : This file was provided by G. Gibert. It allows to use image processing to draw an image after converting it to correct dimensions.
|
||||
* @functions :
|
||||
* @ - void initRobot(DynamixelHandler& dxlHandler, std::string portName, float protocol, int baudRate)
|
||||
- void closeRobot(DynamixelHandler& dxlHandler)
|
||||
- int main(int argc, char** argv)
|
||||
* Input:
|
||||
* - L1 : Length of the first link (cm)
|
||||
* - L2 : Length of the second link (cm)
|
||||
* - image : directory of an image to use for image drawing
|
||||
|
||||
### linearControlFurther/
|
||||
|
||||
* @file linearControl.cpp
|
||||
* @author Guillaume Gibert
|
||||
* @version -
|
||||
* @date 2025/04/13
|
||||
* @brief Description : This file was provided by G. Gibert. It allows to use a proportionally controlled system to join two points in a linear trajectory.
|
||||
* @functions :
|
||||
* - void initRobot(DynamixelHandler& dxlHandler, std::string portName, float protocol, int baudRate)
|
||||
- void closeRobot(DynamixelHandler& dxlHandler)
|
||||
- int main(int argc, char** argv)
|
||||
* Input:
|
||||
* - L1 : Length of the first link (cm)
|
||||
* - L2 : Length of the second link (cm)
|
||||
* - x : x-component of target position (cm)
|
||||
* - y : y-component of target position (cm)
|
||||
|
||||
# README.md
|
||||
|
||||
Documentation for the repository.
|
||||
|
||||
# Requirements
|
||||
|
||||
Hardware: Ergo Poppy Jr.
|
||||
|
||||
Software: C++ Programming
|
||||
|
||||
Libraries: Ensure required libraries (Dynamixel SDK - DynamixelHandler.h)
|
||||
|
||||
# Installation
|
||||
|
||||
Clone this repository:
|
||||
|
||||
git clone https://gitarero.ecam.fr/charles.stelandre/IntroRoboticsLab1.git
|
||||
|
||||
Open a text editor and navigate to the folder containing the .cpp file you want to use.
|
||||
|
||||
Install any necessary libraries via Arduino Library Manager.
|
||||
|
||||
Upload the code to your mBot using a USB connection.
|
||||
|
||||
# Usage
|
||||
|
||||
Navigate to your installation directory.
|
||||
|
||||
Make the file using ``` make ```
|
||||
|
||||
Navigate to the bin folder.
|
||||
|
||||
Execute the desired file in the command line/
|
||||
|
||||
Follow any instructions provided in the comments within the code to execute experiments.
|
||||
|
|
|
|||
BIN
bin/cartControl
BIN
bin/cartControl
Binary file not shown.
BIN
bin/drawImage
BIN
bin/drawImage
Binary file not shown.
BIN
bin/jointControl
BIN
bin/jointControl
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
BIN
lib/Kinematics.o
BIN
lib/Kinematics.o
Binary file not shown.
Binary file not shown.
BIN
lib/drawImage.o
BIN
lib/drawImage.o
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
8
makefile
8
makefile
|
|
@ -1,9 +1,10 @@
|
|||
all: kinematics dynamics joint cartesian image test linear draw
|
||||
all: linearFurther kinematics dynamics joint cartesian image test linear draw
|
||||
g++ -o bin/jointControl lib/jointControl.o lib/Kinematics.o lib/DynamixelHandler.o -L/usr/local/lib/ -ldxl_x64_cpp -lrt -L/usr/lib/x86_64-linux-gnu `pkg-config --libs opencv4`
|
||||
g++ -o bin/cartControl lib/cartControl.o lib/Kinematics.o lib/DynamixelHandler.o lib/ImageProcessing.o -L/usr/local/lib/ -ldxl_x64_cpp -lrt -L/usr/lib/x86_64-linux-gnu `pkg-config --libs opencv4`
|
||||
g++ -o bin/testKinematics lib/testKinematics.o lib/Kinematics.o `pkg-config --libs opencv4`
|
||||
g++ -o bin/linearControl lib/linearControl.o lib/Kinematics.o lib/DynamixelHandler.o lib/ImageProcessing.o -L/usr/local/lib/ -ldxl_x64_cpp -lrt -L/usr/lib/x86_64-linux-gnu `pkg-config --libs opencv4`
|
||||
g++ -o bin/drawImage lib/drawImage.o lib/Kinematics.o lib/DynamixelHandler.o lib/ImageProcessing.o -L/usr/local/lib/ -ldxl_x64_cpp -lrt -L/usr/lib/x86_64-linux-gnu `pkg-config --libs opencv4`
|
||||
g++ -o bin/linearControlFurther lib/linearControlFurther.o lib/Kinematics.o lib/DynamixelHandler.o lib/ImageProcessing.o -L/usr/local/lib/ -ldxl_x64_cpp -lrt -L/usr/lib/x86_64-linux-gnu `pkg-config --libs opencv4`
|
||||
|
||||
kinematics: src/Kinematics.cpp
|
||||
g++ -c src/Kinematics.cpp -o lib/Kinematics.o -I./include -I/usr/include/opencv4
|
||||
|
|
@ -19,6 +20,9 @@ cartesian: src/cartControl.cpp
|
|||
|
||||
linear: src/linearControl.cpp
|
||||
g++ -c src/linearControl.cpp -o lib/linearControl.o -I./include -I/usr/include/opencv4
|
||||
|
||||
linearFurther: src/linearControlFurther.cpp
|
||||
g++ -c src/linearControlFurther.cpp -o lib/linearControlFurther.o -I./include -I/usr/include/opencv4
|
||||
|
||||
image: src/ImageProcessing.cpp
|
||||
g++ -c src/ImageProcessing.cpp -o lib/ImageProcessing.o -I./include -I/usr/include/opencv4
|
||||
|
|
@ -31,4 +35,4 @@ draw: src/drawImage.cpp
|
|||
|
||||
clean:
|
||||
rm lib/*.o
|
||||
rm bin/*
|
||||
rm bin/*
|
||||
|
|
|
|||
Binary file not shown.
|
|
@ -1,6 +1,20 @@
|
|||
/*
|
||||
@file Kinematics.cpp
|
||||
@author Charles STELANDRE
|
||||
@version V1
|
||||
@date 2025/04/13
|
||||
@brief Description : This file contains all kinematics computations for FK, IK and DK.
|
||||
@functions :
|
||||
@ - float deg2rad(float angle)
|
||||
- float rad2deg(float angle)
|
||||
- std::vector<float> computeForwardKinematics(float q1, float q2, float L1, float L2)
|
||||
- std::vector<float> computeInverseKinematics(float x, float y, float L1, float L2)
|
||||
- std::vector<float> computeDifferentialKinematics(float q1, float q2, float L1, float L2)
|
||||
- int computeJacobianMatrixRank(std::vector<float> vJacobianMatrix, float threshold)
|
||||
- cv::Mat computeInverseJacobianMatrix(std::vector<float> vJacobianMatrix)
|
||||
*/
|
||||
#include "Kinematics.h"
|
||||
|
||||
|
||||
float deg2rad(float angle)
|
||||
{
|
||||
//Converts degrees to radians
|
||||
|
|
@ -9,18 +23,18 @@ float deg2rad(float angle)
|
|||
|
||||
float rad2deg(float angle)
|
||||
{
|
||||
//Converts radians to degrees
|
||||
// Converts radians to degrees
|
||||
return angle*180.0/M_PI;
|
||||
}
|
||||
|
||||
std::vector<float> computeForwardKinematics(float q1, float q2, float L1, float L2)
|
||||
{
|
||||
//Computes the x,y coordinates
|
||||
// Computes the x,y coordinates
|
||||
float x = L1 * cos(q1) + L2 * cos(q1+q2);
|
||||
float y = L1 * sin(q1) + L2 * sin(q1+q2);
|
||||
//Printing each joint value to get the target x,y
|
||||
// Printing each joint value to get the target x,y
|
||||
std::cout << "[INFO] Forward Kinematics : (q1, q2)->(x, y) = (" << rad2deg(q1) << ", " << rad2deg(q2) << ")->(" << x << ", " << y << ")" << std::endl;
|
||||
//Declare a vector X containing target x,y
|
||||
// Declare a vector X containing target x,y
|
||||
std::vector<float> X;
|
||||
X.push_back(x);
|
||||
X.push_back(y);
|
||||
|
|
@ -30,30 +44,30 @@ std::vector<float> computeForwardKinematics(float q1, float q2, float L1, float
|
|||
|
||||
std::vector<float> computeInverseKinematics(float x, float y, float L1, float L2)
|
||||
{
|
||||
//declares an empty vector qi
|
||||
// Declares an empty vector qi
|
||||
std::vector<float> qi;
|
||||
|
||||
//From the equations, computes the cosine(q2) to be used to find the number of solutions
|
||||
// From the equations, computes the cosine(q2) to be used to find the number of solutions
|
||||
float cos_q2 = (x*x+y*y-(L1*L1+L2*L2)) / (2.0 * L1 * L2);
|
||||
|
||||
std::cout << "[INFO] cos_q2= " << cos_q2 << std::endl;
|
||||
//Checks if there is no solution (out of reach)
|
||||
// Checks if there is no solution (out of reach)
|
||||
if (cos_q2 >1 | cos_q2 <-1)
|
||||
{
|
||||
//Fills the first value of qi with a value qi[0]=0.0, indicating no solution
|
||||
// Fills the first value of qi with a value qi[0]=0.0, indicating no solution
|
||||
qi.push_back(0.0);
|
||||
std::cout << "[INFO] Inverse Kinematics: No solution!" << std::endl;
|
||||
}
|
||||
//Checks if there is one solution exactly
|
||||
// Checks if there is one solution exactly
|
||||
else if (cos_q2 == 1)
|
||||
{
|
||||
//Fills the first value of qi with a value qi[0]=1.0, indicating a single solution
|
||||
// Fills the first value of qi with a value qi[0]=1.0, indicating a single solution
|
||||
qi.push_back(1.0);
|
||||
float q1 = atan2(y, x);
|
||||
//q2=0 means arm is extended, consistantly with cos_q2==1
|
||||
// q2=0 means arm is extended, consistantly with cos_q2==1
|
||||
float q2 = 0;
|
||||
std::cout << "[INFO] Inverse Kinematics: One solution: (x, y)->(q1, q2) = (" << x << ", " << y << ")->(" << rad2deg(q1) << ", " << rad2deg(q2) << ")" << std::endl;
|
||||
//Adds q1 and q2 to qi
|
||||
// Adds q1 and q2 to qi
|
||||
qi.push_back(q1);
|
||||
qi.push_back(q2);
|
||||
}
|
||||
|
|
@ -61,32 +75,32 @@ std::vector<float> computeInverseKinematics(float x, float y, float L1, float L2
|
|||
{
|
||||
qi.push_back(1.0);
|
||||
float q1 = atan2(y, x);
|
||||
//q2=0 means arm is folded on itself, consistantly with cos_q2==-1
|
||||
// q2=0 means arm is folded on itself, consistantly with cos_q2==-1
|
||||
float q2 = M_PI;
|
||||
std::cout << "[INFO] Inverse Kinematics: One solution: (x, y)->(q1, q2) = (" << x << ", " << y << ")->(" << rad2deg(q1) << ", " << rad2deg(q2) << ")" << std::endl;
|
||||
//Adds q1 and q2 to qi
|
||||
// Adds q1 and q2 to qi
|
||||
qi.push_back(q1);
|
||||
qi.push_back(q2);
|
||||
}
|
||||
else
|
||||
{
|
||||
//Fills the first value of qi with a value qi[0]=2.0, indicating two solutions
|
||||
// Fills the first value of qi with a value qi[0]=2.0, indicating two solutions
|
||||
qi.push_back(2.0);
|
||||
std::cout << "[INFO] Inverse Kinematics: Two solutions: "<< std::endl;
|
||||
//Computes the joint values
|
||||
// Computes the joint values
|
||||
float q2 = acos(cos_q2);
|
||||
float q1 = (float)(atan2(y, x) - atan2(L2*sin(q2), L1+L2*cos_q2));
|
||||
std::cout << "\t(x, y)->(q1, q2) = (" << x << ", " << y << ")->(" << rad2deg(q1) << ", " << rad2deg(q2) << ")" << std::endl;
|
||||
//Adds q1 and q2 to qi
|
||||
// Adds q1 and q2 to qi
|
||||
qi.push_back(q1);
|
||||
qi.push_back(q2);
|
||||
|
||||
//Compute the second solution
|
||||
// Compute the second solution
|
||||
q2 = -acos(cos_q2);
|
||||
q1 = (float)(atan2(y, x) - atan2(L2*sin(q2), L1+L2*cos_q2));
|
||||
|
||||
std::cout << "\t(x, y)->(q1, q2) = (" << x << ", " << y << ")->(" << rad2deg(q1) << ", " << rad2deg(q2) << ")" << std::endl;
|
||||
//Adds the second solution at the end of vector qi.
|
||||
// Adds the second solution at the end of vector qi.
|
||||
qi.push_back(q1);
|
||||
qi.push_back(q2);
|
||||
}
|
||||
|
|
@ -96,14 +110,14 @@ std::vector<float> computeInverseKinematics(float x, float y, float L1, float L2
|
|||
|
||||
std::vector<float> computeDifferentialKinematics(float q1, float q2, float L1, float L2)
|
||||
{
|
||||
//Creates a vector jacobian
|
||||
// Creates a vector jacobian
|
||||
std::vector<float> jacobian;
|
||||
//Computes the jacobian's coefficients
|
||||
// Computes the jacobian's coefficients
|
||||
float j11 = -L2*sin(q1+q2) - L1*sin(q1);
|
||||
float j12 = -L2*sin(q1+q2);
|
||||
float j21 = L2*cos(q1+q2) + L1*cos(q1);
|
||||
float j22 = L2*cos(q1+q2);
|
||||
//Adds each coefficient to the jacobian vector
|
||||
// Adds each coefficient to the jacobian vector
|
||||
jacobian.push_back(j11);
|
||||
jacobian.push_back(j12);
|
||||
jacobian.push_back(j21);
|
||||
|
|
|
|||
|
|
@ -1,3 +1,15 @@
|
|||
/*
|
||||
@file cartControl.cpp
|
||||
@author Charles STELANDRE
|
||||
@version V1
|
||||
@date 2025/04/13
|
||||
@brief Description : This file is a variant from jointControl, adapting cartesian Coordinates translation.
|
||||
@functions :
|
||||
@ - void initRobot(DynamixelHandler& dxlHandler, std::string portName, float protocol, int baudRate)
|
||||
- void closeRobot(DynamixelHandler& dxlHandler)
|
||||
- int main(int argc, char** argv)
|
||||
*/
|
||||
|
||||
#include <chrono>
|
||||
#include <thread>
|
||||
|
||||
|
|
|
|||
|
|
@ -1,3 +1,15 @@
|
|||
/*
|
||||
@file drawImage.cpp
|
||||
@author Guillaume Gibert
|
||||
@version -
|
||||
@date 2025/04/13
|
||||
@brief Description : This file was provided by G. Gibert. It allows to use image processing to draw an image after converting it to correct dimensions.
|
||||
@functions :
|
||||
@ - void initRobot(DynamixelHandler& dxlHandler, std::string portName, float protocol, int baudRate)
|
||||
- void closeRobot(DynamixelHandler& dxlHandler)
|
||||
- int main(int argc, char** argv)
|
||||
*/
|
||||
|
||||
#include <chrono>
|
||||
#include <thread>
|
||||
|
||||
|
|
@ -73,4 +85,4 @@ int main(int argc, char** argv)
|
|||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -1,3 +1,15 @@
|
|||
/*
|
||||
@file jointControl.cpp
|
||||
@author Guillaume Gibert
|
||||
@version -
|
||||
@date 2025/04/13
|
||||
@brief Description : This file was provided by G. Gibert. It allows to test different kinds of kinematics.
|
||||
@functions :
|
||||
- void initRobot(DynamixelHandler& dxlHandler, std::string portName, float protocol, int baudRate)
|
||||
- void closeRobot(DynamixelHandler& dxlHandler)
|
||||
- int main()
|
||||
*/
|
||||
|
||||
#include <chrono>
|
||||
#include <thread>
|
||||
|
||||
|
|
@ -71,4 +83,4 @@ int main(int argc, char** argv)
|
|||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -1,3 +1,15 @@
|
|||
/*
|
||||
@file linearControl.cpp
|
||||
@author Guillaume Gibert
|
||||
@version -
|
||||
@date 2025/04/13
|
||||
@brief Description : This file was provided by G. Gibert. It allows to use a proportionally controlled system to join two points in a linear trajectory.
|
||||
@functions :
|
||||
@ - void initRobot(DynamixelHandler& dxlHandler, std::string portName, float protocol, int baudRate)
|
||||
- void closeRobot(DynamixelHandler& dxlHandler)
|
||||
- int main(int argc, char** argv)
|
||||
*/
|
||||
|
||||
#include <chrono>
|
||||
#include <thread>
|
||||
|
||||
|
|
|
|||
|
|
@ -0,0 +1,163 @@
|
|||
/*
|
||||
@file linearControl.cpp
|
||||
@author Guillaume Gibert, Charles Stelandre
|
||||
@version -
|
||||
@date 2025/04/13
|
||||
@brief Description : This file was provided by G. Gibert, and completed by C.Stelandre. It allows to use a proportionally controlled system to join two points in a linear trajectory, while checking for the validity of a the target point.
|
||||
@functions :
|
||||
@ - void initRobot(DynamixelHandler& dxlHandler, std::string portName, float protocol, int baudRate)
|
||||
- void closeRobot(DynamixelHandler& dxlHandler)
|
||||
- int main(int argc, char** argv)
|
||||
*/
|
||||
|
||||
#include <chrono>
|
||||
#include <thread>
|
||||
|
||||
#include "Kinematics.h"
|
||||
#include "DynamixelHandler.h"
|
||||
|
||||
// TO BE ADJUSTED FOR TESTINGS
|
||||
float _fDistanceThreshold = 0.1f; // in cm
|
||||
float _pController = 0.1f;
|
||||
float _fps = 20.0f; // in Hz
|
||||
float det_threshold = 1e-6;
|
||||
|
||||
// Dynamixel config infos
|
||||
DynamixelHandler _oDxlHandler;
|
||||
std::string _robotDxlPortName = "/dev/ttyUSB0";
|
||||
float _robotDxlProtocol = 2.0;
|
||||
int _robotDxlBaudRate = 1000000;
|
||||
|
||||
|
||||
void initRobot(DynamixelHandler& dxlHandler, std::string portName, float protocol, int baudRate)
|
||||
{
|
||||
std::cout << "===Initialization of the Dynamixel Motor communication====" << std::endl;
|
||||
dxlHandler.setDeviceName(portName);
|
||||
dxlHandler.setProtocolVersion(protocol);
|
||||
dxlHandler.openPort();
|
||||
dxlHandler.setBaudRate(baudRate);
|
||||
dxlHandler.enableTorque(true);
|
||||
std::cout << std::endl;
|
||||
}
|
||||
|
||||
void closeRobot(DynamixelHandler& dxlHandler)
|
||||
{
|
||||
dxlHandler.enableTorque(false);
|
||||
dxlHandler.closePort();
|
||||
}
|
||||
|
||||
int main(int argc, char** argv)
|
||||
{
|
||||
if (argc == 5)
|
||||
{
|
||||
// Retrieves the args and stores them into variables
|
||||
float L1 = atof(argv[1]); // in cm
|
||||
float L2 = atof(argv[2]); // in cm
|
||||
float x = atof(argv[3]); // in cm
|
||||
float y = atof(argv[4]); // in cm
|
||||
float qpen = deg2rad(-90); // in rad
|
||||
|
||||
// Initializes the robot
|
||||
std::cout << "init" ;
|
||||
initRobot(_oDxlHandler, _robotDxlPortName, _robotDxlProtocol, _robotDxlBaudRate);
|
||||
std::cout << "OK" << std::endl;
|
||||
// Changes the control mode
|
||||
//_oDxlHandler.setControlMode(1); //wheel mode
|
||||
// VELOCITY CONTROL qdot
|
||||
|
||||
// Gets the initial joint positions
|
||||
std::cout << "readcurrent" ;
|
||||
std::vector<float> l_vCurrentJointPosition;
|
||||
_oDxlHandler.readCurrentJointPosition(l_vCurrentJointPosition);
|
||||
std::cout << "OK" << std::endl;
|
||||
// Inits joint velocities
|
||||
std::vector<float> l_vCurrentJointVelocity; l_vCurrentJointVelocity.push_back(0.0f); l_vCurrentJointVelocity.push_back(0.0f); l_vCurrentJointVelocity.push_back(0.0f);
|
||||
// VELOCITY CONTROL qdot
|
||||
|
||||
// Inits the loop
|
||||
bool bIsFarFromTarget = true;
|
||||
|
||||
while (bIsFarFromTarget)
|
||||
{
|
||||
// Estimates the position of the end-effector using FK
|
||||
std::vector<float> vEndEffectorPosition = computeForwardKinematics(l_vCurrentJointPosition[0], l_vCurrentJointPosition[2], L1, L2);
|
||||
// Computes deltaX
|
||||
std::vector<float> deltaX{x-vEndEffectorPosition[0], y - vEndEffectorPosition[1]};
|
||||
// Computes Jacobian matrix
|
||||
std::vector<float> vJacobianMatrix = computeDifferentialKinematics(l_vCurrentJointPosition[0], l_vCurrentJointPosition[2], L1, L2);
|
||||
|
||||
// Computes the determinant of the Jacobian
|
||||
float det = abs(vJacobianMatrix[0] * vJacobianMatrix[3] - vJacobianMatrix[1]*vJacobianMatrix[2]);
|
||||
cv::Mat vInverseJacobianMatrix;
|
||||
// Checking if the determinant is within valid range.
|
||||
if (det < det_threshold){
|
||||
// Prints an error if close to a singularity.
|
||||
std::cerr << "[WARNING] Jacobian determinant is close to 0 !(" << det << "(, potential singularity." << std::endl;
|
||||
// If there is a singularity, return identity matrix (do not move)
|
||||
cv::Mat vInverseJacobianMatrix = cv::Mat::eye(2,2,CV_32F);
|
||||
} else {
|
||||
// Computes Inverse of Jacobian matrix
|
||||
cv::Mat vInverseJacobianMatrix = computeInverseJacobianMatrix(vJacobianMatrix);
|
||||
}
|
||||
// Determines deltaX
|
||||
cv::Mat1f oDeltaX(2, 1);
|
||||
oDeltaX.at<float>(0, 0) = _pController * deltaX[0];
|
||||
oDeltaX.at<float>(1, 0) = _pController * deltaX[1];
|
||||
// Computes deltaQ
|
||||
cv::Mat deltaQ = vInverseJacobianMatrix * oDeltaX;
|
||||
// Updates the current joint position
|
||||
l_vCurrentJointPosition[0] += deltaQ.at<float>(0,0);
|
||||
l_vCurrentJointPosition[2] += deltaQ.at<float>(1,0);
|
||||
// Updates the target joint velocity
|
||||
l_vCurrentJointVelocity[0] = deltaQ.at<float>(0,0)*_fps;
|
||||
l_vCurrentJointVelocity[2] = deltaQ.at<float>(1,0)*_fps;
|
||||
// VELOCITY CONTROL qdot
|
||||
// Creates the target joint position vector
|
||||
std::vector<float> vTargetJointPosition;
|
||||
if(l_vCurrentJointPosition.size()>=3){
|
||||
vTargetJointPosition.push_back(l_vCurrentJointPosition[0]);
|
||||
vTargetJointPosition.push_back(qpen);
|
||||
vTargetJointPosition.push_back(l_vCurrentJointPosition[2]);
|
||||
// Sends the target joint position to actuate the robot
|
||||
_oDxlHandler.sendTargetJointPosition(vTargetJointPosition);
|
||||
}
|
||||
// Creates the target joint velocity vector
|
||||
std::vector<float> vTargetJointVelocity;
|
||||
vTargetJointVelocity.push_back(l_vCurrentJointVelocity[0]);
|
||||
vTargetJointVelocity.push_back(0.0f); // pen lift up/down
|
||||
vTargetJointVelocity.push_back(l_vCurrentJointVelocity[2]);
|
||||
|
||||
// Waits 1000/fps ms
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds((long int)(1000.0f/_fps)));
|
||||
|
||||
// Estimates the distance to the target
|
||||
float currentDistanceToTarget = sqrt(pow(y - vEndEffectorPosition[1],2) + pow(x - vEndEffectorPosition[0], 2));
|
||||
std::cout << "====LINEAR CONTROL====" << std::endl;
|
||||
std::cout << "Current Joint Position in deg (q1, q2)= (" << rad2deg(l_vCurrentJointPosition[0]) << ", " << rad2deg(l_vCurrentJointPosition[2]) << ")" << std::endl;
|
||||
std::cout << "Current Joint Velocity in deg/s (q1, q2)= (" << rad2deg(l_vCurrentJointVelocity[0]) << ", " << rad2deg(l_vCurrentJointVelocity[2]) << ")" << std::endl;
|
||||
std::cout << "Current Cartesian Position (x, y)= (" << vEndEffectorPosition[0] << ", " << vEndEffectorPosition[0] << ")" << std::endl;
|
||||
std::cout << "Current Distance To Target= " << currentDistanceToTarget << std::endl;
|
||||
std::cout << "==================" << std::endl;
|
||||
|
||||
// Determines if the loop should continue
|
||||
if (currentDistanceToTarget < _fDistanceThreshold)
|
||||
{
|
||||
bIsFarFromTarget = false;
|
||||
}
|
||||
}
|
||||
|
||||
// Waits 2s
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(2000));
|
||||
|
||||
// Closes robot connection
|
||||
_oDxlHandler.closePort();
|
||||
|
||||
}
|
||||
else
|
||||
{
|
||||
std::cout << "[WARNING] Cartesian control" << std::endl;
|
||||
std::cout << ">> linearControl L1(cm) L2(cm) x(cm) y(cm)"<< std::endl;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
|
@ -1,7 +1,17 @@
|
|||
/*
|
||||
@file testKinematics.cpp
|
||||
@author Guillaume Gibert
|
||||
@version -
|
||||
@date 2025/04/13
|
||||
@brief Description : This file was provided by G. Gibert. It allows to test different kinds of kinematics.
|
||||
@functions :
|
||||
- bool testFK(float q1, float q2, float L1, float L2, std::vector<float> expectedEndEffectorPosition, float errorThreshold)
|
||||
- bool testIK(float x, float y, float L1, float L2, std::vector<float> expectedJointValues, float errorThreshold)
|
||||
- int main()
|
||||
*/
|
||||
|
||||
#include <iostream>
|
||||
|
||||
#include "Kinematics.h"
|
||||
|
||||
#define ERROR_THRESHOLD 0.00001
|
||||
|
||||
std::vector<float> computeForwardKinematics(float q1, float q2, float L1, float L2);
|
||||
|
|
@ -119,4 +129,4 @@ int main()
|
|||
std::cout << "FAILED!" << std::endl;
|
||||
|
||||
return 0;
|
||||
}
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in New Issue