Skip to content

Commit e7ac5b6

Browse files
WoolfreyWoolfrey
authored
Merge pull request #145 from Woolfrey/devel
Torque control for robot arms
2 parents b30daac + 016cbf2 commit e7ac5b6

Some content is hidden

Large Commits have some content hidden by default. Use the searchbox below for content that may be hidden.

54 files changed

+1972
-1298
lines changed

‎.github/ISSUE_TEMPLATE/bug_report.yml‎

Lines changed: 55 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,55 @@
1+
name: 🐞 Bug Report
2+
description: Report a problem.
3+
title: "[BUG] <short description>"
4+
labels: [bug]
5+
body:
6+
7+
- type: markdown
8+
attributes:
9+
value: |
10+
Thanks for reporting a bug!
11+
12+
- type: input
13+
id: os
14+
attributes:
15+
label: What operating system are you using?
16+
placeholder: e.g. Ubuntu 22.04
17+
validations:
18+
required: true
19+
20+
- type: input
21+
id: ros
22+
attributes:
23+
label: What version of Eigen are use using?
24+
placeholder: e.g. v3.4
25+
validations:
26+
required: true
27+
28+
- type: textarea
29+
id: what
30+
attributes:
31+
label: What happened?
32+
description: List the sequence of events that led to the problem.
33+
placeholder: |
34+
1. The Big Bang happened, then
35+
2. I was born.
36+
validations:
37+
required: true
38+
39+
- type: textarea
40+
id: why
41+
attributes:
42+
label: What might be causing the problem?
43+
description: Provide information on why it might be happening.
44+
placeholder: I asked ChatGPT and this is what it said.
45+
validations:
46+
required: false
47+
48+
- type: textarea
49+
id: evidence
50+
attributes:
51+
label: Supporting Evidence
52+
description: Provide screenshots, compilation or runtime errors, etc.
53+
placeholder: (┛ಠДಠ)┛彡┻━┻
54+
validations:
55+
required: true

‎.github/ISSUE_TEMPLATE/feature_request.yml‎

Lines changed: 58 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,58 @@
1+
############################################################
2+
# Required Fields #
3+
############################################################
4+
name: ✨ Feature Request
5+
description: Suggest a new feature or enhancement.
6+
7+
############################################################
8+
# Optional Fields #
9+
############################################################
10+
assignees: []
11+
labels: [enhancement]
12+
projects: []
13+
title: "[REQUEST] <Short description>"
14+
15+
############################################################
16+
# Layout #
17+
############################################################
18+
body:
19+
- type: markdown
20+
attributes:
21+
value: |
22+
Ideas are always weclome!
23+
24+
- type: dropdown
25+
id: priority
26+
attributes:
27+
label: Priority
28+
description: How important is this feature to you?
29+
options:
30+
- Nice to have
31+
- Important
32+
- Blocking my work
33+
validations:
34+
required: true
35+
36+
- type: textarea
37+
id: feature
38+
attributes:
39+
label: What would you like to see?
40+
description: A concise description, with pictures if possible.
41+
validations:
42+
required: true
43+
44+
- type: textarea
45+
id: benefit
46+
attributes:
47+
label: What is the immediate benefit?
48+
description: How this feature will be useful.
49+
validations:
50+
required: true
51+
52+
- type: textarea
53+
id: implication
54+
attributes:
55+
label: What implications will it have?
56+
description: The long-term benefit or impact.
57+
validations:
58+
required: false

‎.github/ISSUE_TEMPLATE/open_task.yml‎

Lines changed: 57 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,57 @@
1+
############################################################
2+
# Required Fields #
3+
############################################################
4+
name: 🙋 Open Task
5+
description: This template is intended for internal use only.
6+
7+
############################################################
8+
# Optional Fields #
9+
############################################################
10+
assignees: []
11+
labels: [enhancement]
12+
projects: []
13+
title: "🙋 [OPEN] This template is intended for internal use only."
14+
15+
############################################################
16+
# Layout #
17+
############################################################
18+
body:
19+
- type: markdown
20+
attributes:
21+
value: |
22+
Open tasks that the public can contribute to.
23+
24+
- type: dropdown
25+
id: priority
26+
attributes:
27+
label: What is the priority?
28+
options:
29+
- Low (Nice to have, but not critical).
30+
- Medium (Useful, but can live without it).
31+
- High (I would do this myself, but don't have time).
32+
validations:
33+
required: true
34+
35+
- type: textarea
36+
id: feature
37+
attributes:
38+
label: What would we like to have?
39+
description: A concise description, with pictures if possible.
40+
validations:
41+
required: true
42+
43+
- type: textarea
44+
id: benefit
45+
attributes:
46+
label: What is the immediate benefit?
47+
description: How this feature will be useful.
48+
validations:
49+
required: true
50+
51+
- type: textarea
52+
id: implication
53+
attributes:
54+
label: What implications will it have?
55+
description: The long-term benefit or impact.
56+
validations:
57+
required: false

‎CITATION.cff‎

Lines changed: 10 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,10 @@
1+
cff-version: 1.2.0
2+
message: "If you use this software, please cite it as below."
3+
authors:
4+
- family-names: "Woolfrey"
5+
given-names: "Jon"
6+
orcid: "https://orcid.org/0000-0001-5926-5669"
7+
title: "Robot Library"
8+
version: 1.0.0
9+
date-released: 2025-04-11
10+
url: "https://github.com/Woolfrey/software_robot_library"

‎Control/CMakeLists.txt‎

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,6 +1,7 @@
11

22
# List the source files for this library
3-
add_library(Control src/SerialKinematicControl.cpp
3+
add_library(Control src/SerialDynamicControl.cpp
4+
src/SerialKinematicControl.cpp
45
src/SerialLinkBase.cpp
56
)
67

‎Control/include/Control/DataStructures.h‎

Lines changed: 63 additions & 29 deletions
Original file line numberDiff line numberDiff line change
@@ -2,17 +2,17 @@
22
* @file DataStructures.h
33
* @author Jon Woolfrey
44
* @email [email protected]
5-
* @date April 2025
6-
* @version 1.0
5+
* @date July 2025
6+
* @version 1.1
77
* @brief Contains custom structs used in Control classes.
8-
*
9-
*
10-
* @copyright Copyright (c) 2025 Jon Woolfrey
11-
*
12-
* @license GNU General Public License V3
13-
*
14-
* @see https://github.com/Woolfrey/software_robot_library for more information.
15-
* @see https://github.com/Woolfrey/software_simple_qp for the optimisation algorithm used in the control.
8+
*
9+
* @copyright (c) 2025 Jon Woolfrey
10+
* @license OSCL - Free for non-commercial open-source use only.
11+
* Commercial use requires a license.
12+
* Contact: [email protected]
13+
*
14+
* @see https://github.com/Woolfrey/software_robot_library
15+
* @see https://github.com/Woolfrey/software_simple_qp
1616
*/
1717

1818
#ifndef CONTROL_DATA_STRUCTS_H
@@ -26,34 +26,68 @@ namespace RobotLibrary { namespace Control {
2626
/**
2727
* @brief A data structure for passing control parameters to the SerialLinkBase class in a single argument.
2828
*/
29-
struct Parameters
29+
struct SerialLinkParameters
3030
{
31-
// NOTE: These are for the control class itself:
32-
Parameters() = default; ///< This enables default options
31+
SerialLinkParameters() = default; ///< This enables default options
3332

34-
double controlFrequency = 100.0; ///< Rate at which control loop operates.
35-
double jointPositionGain = 10.0; ///< Scales the position error feedback
36-
double jointVelocityGain = 1.0; ///< Scales the velocity error feedback
37-
double maxJointAcceleration = 10.0; ///< Limits joint acceleration
33+
double jointPositionGain = 100.0; ///< Scales the position error feedback
34+
double jointVelocityGain = 20.0; ///< Scales the velocity error feedback
35+
double maxJointAcceleration = 5.0; ///< Limits joint acceleration
3836
double minManipulability = 1e-04; ///< Threshold for singularity avoidance
3937

40-
Eigen::Matrix<double,6,6> cartesianStiffness = (Eigen::MatrixXd(6,6) << 10.0, 0.0, 0.0, 0.0, 0.0, 0.0,
41-
0.0, 10.0, 0.0, 0.0, 0.0, 0.0,
42-
0.0, 0.0, 10.0, 0.0, 0.0, 0.0,
43-
0.0, 0.0, 0.0, 2.0, 0.0, 0.0,
44-
0.0, 0.0, 0.0, 0.0, 2.0, 0.0,
45-
0.0, 0.0, 0.0, 0.0, 0.0, 2.0).finished(); ///< Scales pose error feedback
38+
unsigned int controlFrequency = 500; ///< Rate at which control loop operates.
39+
40+
Eigen::Matrix<double,6,6> cartesianPoseGain = (Eigen::MatrixXd(6,6) << 10.0, 0.0, 0.0, 0.0, 0.0, 0.0,
41+
0.0, 10.0, 0.0, 0.0, 0.0, 0.0,
42+
0.0, 0.0, 10.0, 0.0, 0.0, 0.0,
43+
0.0, 0.0, 0.0, 5.0, 0.0, 0.0,
44+
0.0, 0.0, 0.0, 0.0, 5.0, 0.0,
45+
0.0, 0.0, 0.0, 0.0, 0.0, 5.0).finished(); ///< Scales pose error feedback
4646

47-
Eigen::Matrix<double,6,6> cartesianDamping = (Eigen::MatrixXd(6,6) << 1.0, 0.0, 0.0, 0.0, 0.0, 0.0,
48-
0.0, 1.0, 0.0, 0.0, 0.0, 0.0,
49-
0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
50-
0.0, 0.0, 0.0, 0.2, 0.0, 0.0,
51-
0.0, 0.0, 0.0, 0.0, 0.2, 0.0,
52-
0.0, 0.0, 0.0, 0.0, 0.0, 0.2).finished(); ///< Scales twist error feedback
47+
Eigen::Matrix<double,6,6> cartesianVelocityGain = (Eigen::MatrixXd(6,6) << 20.0, 0.0, 0.0, 0.0, 0.0, 0.0,
48+
0.0, 20.0, 0.0, 0.0, 0.0, 0.0,
49+
0.0, 0.0, 20.0, 0.0, 0.0, 0.0,
50+
0.0, 0.0, 0.0, 2.0, 0.0, 0.0,
51+
0.0, 0.0, 0.0, 0.0, 2.0, 0.0,
52+
0.0, 0.0, 0.0, 0.0, 0.0, 2.0).finished(); ///< Scales twist error feedback
5353

5454
SolverOptions<double> qpsolver = SolverOptions<double>(); ///< Parameters for the underlying QP solver
5555
};
5656

57+
/**
58+
* @brief A data structure for parameters in the feedback control class.
59+
*/
60+
struct DifferentialDriveFeedbackParameters
61+
{
62+
DifferentialDriveFeedbackParameters() = default;
63+
64+
double controlFrequency = 100.0; ///< Rate at which control is computed
65+
double minimumSafeDistance = 1.0; ///< Used in collision avoidance
66+
double orientationGain = 10.0; ///< Feedback gain on orientation error
67+
double xPositionGain = 5.0; ///< Feedback gain on x position error
68+
double yPositionGain = 25.0; ///< Feedback gain on y position error
69+
};
70+
71+
/**
72+
* @brief A data structure containing parameters for model predictive control.
73+
*/
74+
struct DifferentialDrivePredictiveParameters
75+
{
76+
DifferentialDrivePredictiveParameters() = default;
77+
78+
double controlFrequency = 100.0; ///< Rate at which control is calculated / implemented
79+
double exponent = -0.1; ///< Scales the pose error weight across the horizon
80+
double maximumControlStepNorm = 1e-10; ///< Threshold for terminating optimisation
81+
double minimumSafeDistance = 1.0; ///< Used in collision avoidance
82+
unsigned int numberOfRecursions = 2; ///< Number of forward & backward passes to optimise control
83+
unsigned int predictionSteps = 10; ///< Length of prediction horizon
84+
85+
Eigen::Matrix3d poseErrorWeight
86+
= (Eigen::MatrixXd(3,3) << 200.0, 0.00, 0.00,
87+
0.0, 200.00, -0.09,
88+
0.0, -0.09, 0.10).finished();
89+
};
90+
5791
} } // namespace
5892

5993
#endif

0 commit comments

Comments
 (0)