Skip to content

Commit 2d8f9a8

Browse files
PeterBowmanPeterBowman
committed
Merge branch 'bcc-movl-ref'
2 parents eb8b1b3 + 8992f67 commit 2d8f9a8

File tree

7 files changed

+177
-39
lines changed

7 files changed

+177
-39
lines changed

‎libraries/YarpPlugins/BasicCartesianControl/BasicCartesianControl_ParamsParser.cpp‎

Lines changed: 34 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -8,7 +8,7 @@
88
// This is an automatically generated file. Please do not edit it.
99
// It will be re-generated if the cmake flag ALLOW_DEVICE_PARAM_PARSER_GERNERATION is ON.
1010

11-
// Generated on: Wed Apr 16 14:50:07 2025
11+
// Generated on: Mon Sep 15 20:34:59 2025
1212

1313

1414
#include "BasicCartesianControl_ParamsParser.h"
@@ -30,6 +30,8 @@ std::vector<std::string> BasicCartesianControl_ParamsParser::getListOfParams() c
3030
std::vector<std::string> params;
3131
params.push_back("controllerGain");
3232
params.push_back("trajectoryDuration");
33+
params.push_back("trajectoryRefSpeed");
34+
params.push_back("trajectoryRefAccel");
3335
params.push_back("cmcPeriodMs");
3436
params.push_back("waitPeriodMs");
3537
params.push_back("usePosdMovl");
@@ -79,6 +81,34 @@ bool BasicCartesianControl_ParamsParser::parseParams(const yarp::os::Search
7981
prop_check.unput("trajectoryDuration");
8082
}
8183

84+
//Parser of parameter trajectoryRefSpeed
85+
{
86+
if (config.check("trajectoryRefSpeed"))
87+
{
88+
m_trajectoryRefSpeed = config.find("trajectoryRefSpeed").asFloat64();
89+
yCInfo(BasicCartesianControlParamsCOMPONENT) << "Parameter 'trajectoryRefSpeed' using value:" << m_trajectoryRefSpeed;
90+
}
91+
else
92+
{
93+
yCInfo(BasicCartesianControlParamsCOMPONENT) << "Parameter 'trajectoryRefSpeed' using DEFAULT value:" << m_trajectoryRefSpeed;
94+
}
95+
prop_check.unput("trajectoryRefSpeed");
96+
}
97+
98+
//Parser of parameter trajectoryRefAccel
99+
{
100+
if (config.check("trajectoryRefAccel"))
101+
{
102+
m_trajectoryRefAccel = config.find("trajectoryRefAccel").asFloat64();
103+
yCInfo(BasicCartesianControlParamsCOMPONENT) << "Parameter 'trajectoryRefAccel' using value:" << m_trajectoryRefAccel;
104+
}
105+
else
106+
{
107+
yCInfo(BasicCartesianControlParamsCOMPONENT) << "Parameter 'trajectoryRefAccel' using DEFAULT value:" << m_trajectoryRefAccel;
108+
}
109+
prop_check.unput("trajectoryRefAccel");
110+
}
111+
82112
//Parser of parameter cmcPeriodMs
83113
{
84114
if (config.check("cmcPeriodMs"))
@@ -215,6 +245,8 @@ std::string BasicCartesianControl_ParamsParser::getDocumentationOfDevicePar
215245
doc = doc + std::string("This is the list of the parameters accepted by the device:\n");
216246
doc = doc + std::string("'controllerGain': controller gain\n");
217247
doc = doc + std::string("'trajectoryDuration': trajectory duration\n");
248+
doc = doc + std::string("'trajectoryRefSpeed': trajectory reference linear speed\n");
249+
doc = doc + std::string("'trajectoryRefAccel': trajectory reference linear acceleration\n");
218250
doc = doc + std::string("'cmcPeriodMs': CMC rate\n");
219251
doc = doc + std::string("'waitPeriodMs': wait command period\n");
220252
doc = doc + std::string("'usePosdMovl': execute MOVL commands in POSD mode using IK\n");
@@ -224,7 +256,7 @@ std::string BasicCartesianControl_ParamsParser::getDocumentationOfDevicePar
224256
doc = doc + std::string("'solver': cartesian solver device\n");
225257
doc = doc + std::string("\n");
226258
doc = doc + std::string("Here are some examples of invocation command with yarpdev, with all params:\n");
227-
doc = doc + " yarpdev --device BasicCartesianControl --controllerGain 0.05 --trajectoryDuration 10.0 --cmcPeriodMs 50 --waitPeriodMs 30 --usePosdMovl false --enableFailFast false --referenceFrame base --robot remote_controlboard --solver KdlSolver\n";
259+
doc = doc + " yarpdev --device BasicCartesianControl --controllerGain 0.05 --trajectoryDuration 0.0 --trajectoryRefSpeed 0.05 --trajectoryRefAccel 0.02 --cmcPeriodMs 50 --waitPeriodMs 30 --usePosdMovl false --enableFailFast false --referenceFrame base --robot remote_controlboard --solver KdlSolver\n";
228260
doc = doc + std::string("Using only mandatory params:\n");
229261
doc = doc + " yarpdev --device BasicCartesianControl\n";
230262
doc = doc + std::string("=============================================\n\n"); return doc;

‎libraries/YarpPlugins/BasicCartesianControl/BasicCartesianControl_ParamsParser.h‎

Lines changed: 21 additions & 15 deletions
Original file line numberDiff line numberDiff line change
@@ -8,7 +8,7 @@
88
// This is an automatically generated file. Please do not edit it.
99
// It will be re-generated if the cmake flag ALLOW_DEVICE_PARAM_PARSER_GERNERATION is ON.
1010

11-
// Generated on: Wed Apr 16 14:50:07 2025
11+
// Generated on: Mon Sep 15 20:34:59 2025
1212

1313

1414
#ifndef BASICCARTESIANCONTROL_PARAMSPARSER_H
@@ -23,21 +23,23 @@
2323
* This class is the parameters parser for class BasicCartesianControl.
2424
*
2525
* These are the used parameters:
26-
* | Group name | Parameter name | Type | Units | Default Value | Required | Description | Notes |
27-
* |:----------:|:------------------:|:------:|:-----:|:-------------------:|:--------:|:-------------------------------------------:|:---------:|
28-
* | - | controllerGain | double | - | 0.05 | 0 | controller gain | - |
29-
* | - | trajectoryDuration | double | s | 10.0 | 0 | trajectory duration | - |
30-
* | - | cmcPeriodMs | int | ms | 50 | 0 | CMC rate | - |
31-
* | - | waitPeriodMs | int | ms | 30 | 0 | wait command period | - |
32-
* | - | usePosdMovl | bool | - | false | 0 | execute MOVL commands in POSD mode using IK | - |
33-
* | - | enableFailFast | bool | - | false | 0 | enable fail-fast mode for MOVL commands | - |
34-
* | - | referenceFrame | string | - | base | 0 | reference frame | base, tcp |
35-
* | - | robot | string | - | remote_controlboard | 0 | robot device | - |
36-
* | - | solver | string | - | KdlSolver | 0 | cartesian solver device | - |
26+
* | Group name | Parameter name | Type | Units | Default Value | Required | Description | Notes |
27+
* |:----------:|:------------------:|:------:|:-----:|:-------------------:|:--------:|:-------------------------------------------:|:----------------------------------------:|
28+
* | - | controllerGain | double | - | 0.05 | 0 | controller gain | - |
29+
* | - | trajectoryDuration | double | s | 0.0 | 0 | trajectory duration | 0: use ref speed/acc to compute duration |
30+
* | - | trajectoryRefSpeed | double | m/s | 0.05 | 0 | trajectory reference linear speed | - |
31+
* | - | trajectoryRefAccel | double | m/s^2 | 0.02 | 0 | trajectory reference linear acceleration | - |
32+
* | - | cmcPeriodMs | int | ms | 50 | 0 | CMC rate | - |
33+
* | - | waitPeriodMs | int | ms | 30 | 0 | wait command period | - |
34+
* | - | usePosdMovl | bool | - | false | 0 | execute MOVL commands in POSD mode using IK | - |
35+
* | - | enableFailFast | bool | - | false | 0 | enable fail-fast mode for MOVL commands | - |
36+
* | - | referenceFrame | string | - | base | 0 | reference frame | base, tcp |
37+
* | - | robot | string | - | remote_controlboard | 0 | robot device | - |
38+
* | - | solver | string | - | KdlSolver | 0 | cartesian solver device | - |
3739
*
3840
* The device can be launched by yarpdev using one of the following examples (with and without all optional parameters):
3941
* \code{.unparsed}
40-
* yarpdev --device BasicCartesianControl --controllerGain 0.05 --trajectoryDuration 10.0 --cmcPeriodMs 50 --waitPeriodMs 30 --usePosdMovl false --enableFailFast false --referenceFrame base --robot remote_controlboard --solver KdlSolver
42+
* yarpdev --device BasicCartesianControl --controllerGain 0.05 --trajectoryDuration 0.0 --trajectoryRefSpeed 0.05 --trajectoryRefAccel 0.02 --cmcPeriodMs 50 --waitPeriodMs 30 --usePosdMovl false --enableFailFast false --referenceFrame base --robot remote_controlboard --solver KdlSolver
4143
* \endcode
4244
*
4345
* \code{.unparsed}
@@ -64,7 +66,9 @@ class BasicCartesianControl_ParamsParser : public yarp::dev::IDeviceDriverParams
6466
const parser_version_type m_parser_version = {};
6567

6668
const std::string m_controllerGain_defaultValue = {"0.05"};
67-
const std::string m_trajectoryDuration_defaultValue = {"10.0"};
69+
const std::string m_trajectoryDuration_defaultValue = {"0.0"};
70+
const std::string m_trajectoryRefSpeed_defaultValue = {"0.05"};
71+
const std::string m_trajectoryRefAccel_defaultValue = {"0.02"};
6872
const std::string m_cmcPeriodMs_defaultValue = {"50"};
6973
const std::string m_waitPeriodMs_defaultValue = {"30"};
7074
const std::string m_usePosdMovl_defaultValue = {"false"};
@@ -74,7 +78,9 @@ class BasicCartesianControl_ParamsParser : public yarp::dev::IDeviceDriverParams
7478
const std::string m_solver_defaultValue = {"KdlSolver"};
7579

7680
double m_controllerGain = {0.05};
77-
double m_trajectoryDuration = {10.0};
81+
double m_trajectoryDuration = {0.0};
82+
double m_trajectoryRefSpeed = {0.05};
83+
double m_trajectoryRefAccel = {0.02};
7884
int m_cmcPeriodMs = {50};
7985
int m_waitPeriodMs = {30};
8086
bool m_usePosdMovl = {false};

‎libraries/YarpPlugins/BasicCartesianControl/BasicCartesianControl_params.md‎

Lines changed: 13 additions & 11 deletions
Original file line numberDiff line numberDiff line change
@@ -1,11 +1,13 @@
1-
| Group | Parameter | Type | Units | Default Value | Required | Description | Notes |
2-
|:-----:|:------------------:|:------:|:-----:|:-------------------:|:--------:|:-------------------------------------------:|:---------:|
3-
| | controllerGain | double | | 0.05 | no | controller gain | |
4-
| | trajectoryDuration | double | s | 10.0 | no | trajectory duration | |
5-
| | cmcPeriodMs | int | ms | 50 | no | CMC rate | |
6-
| | waitPeriodMs | int | ms | 30 | no | wait command period | |
7-
| | usePosdMovl | bool | | false | no | execute MOVL commands in POSD mode using IK | |
8-
| | enableFailFast | bool | | false | no | enable fail-fast mode for MOVL commands | |
9-
| | referenceFrame | string | | base | no | reference frame | base, tcp |
10-
| | robot | string | | remote_controlboard | no | robot device | |
11-
| | solver | string | | KdlSolver | no | cartesian solver device | |
1+
| Group | Parameter | Type | Units | Default Value | Required | Description | Notes |
2+
|:-----:|:------------------:|:------:|:-----:|:-------------------:|:--------:|:-------------------------------------------:|:----------------------------------------:|
3+
| | controllerGain | double | | 0.05 | no | controller gain | |
4+
| | trajectoryDuration | double | s | 0.0 | no | trajectory duration | 0: use ref speed/acc to compute duration |
5+
| | trajectoryRefSpeed | double | m/s | 0.05 | no | trajectory reference linear speed | |
6+
| | trajectoryRefAccel | double | m/s^2 | 0.02 | no | trajectory reference linear acceleration | |
7+
| | cmcPeriodMs | int | ms | 50 | no | CMC rate | |
8+
| | waitPeriodMs | int | ms | 30 | no | wait command period | |
9+
| | usePosdMovl | bool | | false | no | execute MOVL commands in POSD mode using IK | |
10+
| | enableFailFast | bool | | false | no | enable fail-fast mode for MOVL commands | |
11+
| | referenceFrame | string | | base | no | reference frame | base, tcp |
12+
| | robot | string | | remote_controlboard | no | robot device | |
13+
| | solver | string | | KdlSolver | no | cartesian solver device | |

‎libraries/YarpPlugins/BasicCartesianControl/DeviceDriverImpl.cpp‎

Lines changed: 44 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -38,6 +38,50 @@ bool BasicCartesianControl::open(yarp::os::Searchable& config)
3838
return false;
3939
}
4040

41+
if (m_controllerGain < 0.0)
42+
{
43+
yCError(BCC) << "Controller gain must be positive";
44+
return false;
45+
}
46+
47+
if (m_trajectoryDuration < 0.0)
48+
{
49+
yCError(BCC) << "Trajectory duration must be positive or zero";
50+
return false;
51+
}
52+
else if (m_trajectoryDuration == 0.0)
53+
{
54+
yCInfo(BCC) << "Duration set to zero, therefore trajectory execution time will depend on reference speed and acceleration";
55+
}
56+
else
57+
{
58+
yCInfo(BCC) << "Trajectory duration forced to" << m_trajectoryDuration << "seconds regardless of velocity profile";
59+
}
60+
61+
if (m_trajectoryRefSpeed <= 0.0)
62+
{
63+
yCError(BCC) << "Trajectory reference speed must be positive";
64+
return false;
65+
}
66+
67+
if (m_trajectoryRefAccel <= 0.0)
68+
{
69+
yCError(BCC) << "Trajectory reference acceleration must be positive";
70+
return false;
71+
}
72+
73+
if (m_cmcPeriodMs <= 0)
74+
{
75+
yCError(BCC) << "CMC period must be positive";
76+
return false;
77+
}
78+
79+
if (m_waitPeriodMs <= 0)
80+
{
81+
yCError(BCC) << "Wait period must be positive";
82+
return false;
83+
}
84+
4185
yarp::os::Property robotOptions;
4286
robotOptions.fromString(config.toString());
4387
robotOptions.put("device", m_robot);

‎libraries/YarpPlugins/BasicCartesianControl/ICartesianControlImpl.cpp‎

Lines changed: 53 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -174,8 +174,6 @@ bool BasicCartesianControl::relj(const std::vector<double> &xd)
174174

175175
bool BasicCartesianControl::movl(const std::vector<double> &xd)
176176
{
177-
yCWarning(BCC) << "MOVL mode still experimental";
178-
179177
std::vector<double> currentQ(numJoints);
180178

181179
if (!iEncoders->getEncoders(currentQ.data()))
@@ -220,9 +218,20 @@ bool BasicCartesianControl::movl(const std::vector<double> &xd)
220218

221219
auto * interpolator = new KDL::RotationalInterpolation_SingleAxis();
222220
auto * path = new KDL::Path_Line(H_base_start, H_base_end, interpolator, 1.0);
223-
auto * profile = new KDL::VelocityProfile_Trap(10.0, 10.0);
221+
auto * profile = new KDL::VelocityProfile_Trap(m_trajectoryRefSpeed, m_trajectoryRefAccel);
222+
223+
if (m_trajectoryDuration != 0.0)
224+
{
225+
// Set duration, let profile compute speed and acceleration
226+
profile->SetProfileDuration(0.0, path->PathLength(), m_trajectoryDuration);
227+
}
228+
else
229+
{
230+
// Set speed and acceleration, let profile compute duration
231+
profile->SetProfile(0.0, path->PathLength());
232+
}
224233

225-
trajectories.emplace_back(new KDL::Trajectory_Segment(path, profile, m_trajectoryDuration));
234+
trajectories.emplace_back(new KDL::Trajectory_Segment(path, profile));
226235
}
227236

228237
if (m_enableFailFast && !doFailFastChecks(currentQ))
@@ -278,13 +287,13 @@ bool BasicCartesianControl::movv(const std::vector<double> &xdotd)
278287

279288
auto * interpolator = new KDL::RotationalInterpolation_SingleAxis();
280289
auto * path = new KDL::Path_Line(H_base_start, twist_in_base, interpolator, 1.0);
281-
auto * profile = new KDL::VelocityProfile_Rectangular(10.0);
282-
profile->SetProfileDuration(0, 10.0, 10.0 / path->PathLength());
290+
auto * profile = new KDL::VelocityProfile_Rectangular(m_trajectoryRefSpeed);
291+
profile->SetProfileDuration(0.0, m_trajectoryRefSpeed, m_trajectoryRefSpeed / path->PathLength());
283292

284293
trajectories.emplace_back(new KDL::Trajectory_Segment(path, profile));
285294
}
286295

287-
//-- Set velocity mode and set state which makes periodic thread implement control.
296+
//-- Set velocity mode and set state which makes periodic thread implement control
288297
if (!setControlModes(VOCAB_CM_VELOCITY))
289298
{
290299
yCError(BCC) << "Unable to set velocity mode";
@@ -586,13 +595,40 @@ bool BasicCartesianControl::setParameter(int vocab, double value)
586595
m_controllerGain = value;
587596
break;
588597
case VOCAB_CC_CONFIG_TRAJ_DURATION:
589-
if (value <= 0.0)
598+
if (value < 0.0)
590599
{
591-
yCError(BCC) << "Trajectory duration cannot be negative nor zero";
600+
yCError(BCC) << "Trajectory duration cannot be negative";
592601
return false;
593602
}
603+
else if ((m_trajectoryDuration == 0.0) ^ (value == 0.0))
604+
{
605+
if (value == 0.0)
606+
{
607+
yCInfo(BCC) << "Duration set to zero, therefore trajectory execution time will depend on reference speed and acceleration";
608+
}
609+
else
610+
{
611+
yCInfo(BCC) << "Trajectory duration forced to" << value << "seconds regardless of velocity profile";
612+
}
613+
}
594614
m_trajectoryDuration = value;
595615
break;
616+
case VOCAB_CC_CONFIG_TRAJ_REF_SPD:
617+
if (value <= 0.0)
618+
{
619+
yCError(BCC) << "Trajectory reference speed cannot be negative nor zero";
620+
return false;
621+
}
622+
m_trajectoryRefSpeed = value;
623+
break;
624+
case VOCAB_CC_CONFIG_TRAJ_REF_ACC:
625+
if (value <= 0.0)
626+
{
627+
yCError(BCC) << "Trajectory reference acceleration cannot be negative nor zero";
628+
return false;
629+
}
630+
m_trajectoryRefAccel = value;
631+
break;
596632
case VOCAB_CC_CONFIG_CMC_PERIOD:
597633
if (!yarp::os::PeriodicThread::setPeriod(value * 0.001))
598634
{
@@ -645,6 +681,12 @@ bool BasicCartesianControl::getParameter(int vocab, double * value)
645681
case VOCAB_CC_CONFIG_TRAJ_DURATION:
646682
*value = m_trajectoryDuration;
647683
break;
684+
case VOCAB_CC_CONFIG_TRAJ_REF_SPD:
685+
*value = m_trajectoryRefSpeed;
686+
break;
687+
case VOCAB_CC_CONFIG_TRAJ_REF_ACC:
688+
*value = m_trajectoryRefAccel;
689+
break;
648690
case VOCAB_CC_CONFIG_CMC_PERIOD:
649691
*value = m_cmcPeriodMs;
650692
break;
@@ -691,6 +733,8 @@ bool BasicCartesianControl::getParameters(std::map<int, double> & params)
691733
{
692734
params.emplace(VOCAB_CC_CONFIG_GAIN, m_controllerGain);
693735
params.emplace(VOCAB_CC_CONFIG_TRAJ_DURATION, m_trajectoryDuration);
736+
params.emplace(VOCAB_CC_CONFIG_TRAJ_REF_SPD, m_trajectoryRefSpeed);
737+
params.emplace(VOCAB_CC_CONFIG_TRAJ_REF_ACC, m_trajectoryRefAccel);
694738
params.emplace(VOCAB_CC_CONFIG_CMC_PERIOD, m_cmcPeriodMs);
695739
params.emplace(VOCAB_CC_CONFIG_WAIT_PERIOD, m_waitPeriodMs);
696740
params.emplace(VOCAB_CC_CONFIG_FRAME, referenceFrame);

0 commit comments

Comments
 (0)