YARP
Yet Another Robot Platform
 
Loading...
Searching...
No Matches
ITorqueControlTest.h
Go to the documentation of this file.
1/*
2 * SPDX-FileCopyrightText: 2023-2023 Istituto Italiano di Tecnologia (IIT)
3 * SPDX-License-Identifier: BSD-3-Clause
4 */
5
6#ifndef ITORQUECONTROLTEST_H
7#define ITORQUECONTROLTEST_H
8
9#include <vector>
10#include <numeric>
11
14#include <catch2/catch_amalgamated.hpp>
15#include "TestUtils.h"
16
17using namespace yarp::dev;
18using namespace yarp::os;
19
20namespace yarp::dev::tests
21{
23 {
24 REQUIRE(itrq != nullptr);
25 REQUIRE(icmd != nullptr);
26
27 bool b;
28 double val, val1, val2;
29
30 size_t ax = 0;
31 b = itrq->getAxes(ax);
32 CHECK(b);
33
34 for (size_t i = 0; i < ax; i++)
35 {
37 CHECK(b);
38 wait_safe(); // Allow some time for the command to take effect
39
41 b = icmd->getControlMode(i, mode_ret);
42 CHECK(b);
44 }
45
46 auto vals = std::vector<double>(ax);
47 auto refvals = std::vector<double>(ax);
48 auto vals1 = std::vector<double>(ax);
49 auto vals2 = std::vector<double>(ax);
50 auto joints = std::vector<int>(ax);
51 std::iota(joints.begin(), joints.end(), 0);
52
54 //params.bemf = 0.1;
55 //params.bemf_scale = 0.2;
56 //params.ktau = 0.3;
57 //params.ktau_scale = 0.4;
58 params.viscousPos = 0.5;
59 params.viscousNeg = 0.6;
60 params.coulombPos = 0.7;
61 params.coulombNeg = 0.8;
62 params.velocityThres = 0.9;
63 b= itrq->setMotorTorqueParams(0, params);
64 CHECK(b);
65
67 b= itrq->getMotorTorqueParams(0, &res);
68 CHECK(b);
69 //CHECK(res.bemf == 0.1); // interface seems functional
70 //CHECK(res.bemf_scale == 0.2); // interface seems functional
71 //CHECK(res.ktau == 0.3); // interface seems functional
72 //CHECK(res.ktau_scale == 0.4); // interface seems functional
73 CHECK(res.viscousPos == 0.5); // interface seems functional
74 CHECK(res.viscousNeg == 0.6); // interface seems functional
75 CHECK(res.coulombPos == 0.7); // interface seems functional
76 CHECK(res.coulombNeg == 0.8); // interface seems functional
77 CHECK(res.velocityThres == 0.9); // interface seems functional
78
79 b = itrq->getRefTorque(0, &val);
80 CHECK(b);
81
82 b = itrq->getRefTorques(refvals.data());
83 CHECK(b);
84
85 b = itrq->getTorque(0,&val);
86 CHECK(b);
87
88 b = itrq->getTorqueRange(0, &val1, &val2);
89 CHECK(b);
90
91 b = itrq->getTorqueRanges(vals1.data(), vals2.data());
92 CHECK(b);
93
94 b = itrq->getTorques(vals.data());
95 CHECK(b);
96
97 b = itrq->setRefTorque(0,val);
98 CHECK(b);
99
100 b = itrq->setRefTorques(refvals.data());
101 CHECK(b);
102
103 b = itrq->setRefTorques(ax, joints.data(), refvals.data());
104 CHECK(b);
105 }
106
108 {
109 REQUIRE(itrq != nullptr);
110 REQUIRE(icmd != nullptr);
111
112 bool b;
113 size_t ax=0;
114 double val, val1, val2;
116
117 b = itrq->getAxes(ax);
118 CHECK(b);
119 REQUIRE(ax > 0);
120
121 for (size_t i = 0; i < ax; i++)
122 {
124 //CHECK(b); //this will fail
125 }
126
127 auto vals = std::vector<double>(ax);
128 auto refvals = std::vector<double>(ax);
129 auto vals1 = std::vector<double>(ax);
130 auto vals2 = std::vector<double>(ax);
131 auto joints = std::vector<int>(ax);
132 std::iota(joints.begin(), joints.end(), 0);
133
134 b = itrq->setMotorTorqueParams(0, param);
135 CHECK(!b);
136
137 b = itrq->getMotorTorqueParams(0, &param);
138 CHECK(!b);
139
140 b = itrq->getRefTorque(0, &val);
141 CHECK(!b);
142
143 b = itrq->getRefTorques(refvals.data());
144 CHECK(!b);
145
146 b = itrq->getTorque(0,&val);
147 CHECK(!b);
148
149 b = itrq->getTorqueRange(0, &val1, &val2);
150 CHECK(!b);
151
152 b = itrq->getTorqueRanges(vals1.data(), vals2.data());
153 CHECK(!b);
154
155 b = itrq->getTorques(vals.data());
156 CHECK(!b);
157
158 b = itrq->setRefTorque(0,val);
159 CHECK(!b);
160
161 b = itrq->setRefTorques(refvals.data());
162 CHECK(!b);
163
164 b = itrq->setRefTorques(ax, joints.data(), refvals.data());
165 CHECK(!b);
166 }
167}
168
169#endif
Interface for setting control mode in control board.
virtual yarp::dev::ReturnValue getControlMode(int j, yarp::dev::ControlModeEnum &mode)=0
Get the current control mode.
virtual yarp::dev::ReturnValue setControlMode(int j, yarp::dev::SelectableControlModeEnum mode)=0
Set the current control mode.
Interface for control boards implementing torque control.
A mini-server for performing network communication in the background.
void exec_iTorqueControl_test_unimplemented_interface(ITorqueControl *itrq, IControlMode *icmd)
void exec_iTorqueControl_test_1(ITorqueControl *itrq, IControlMode *icmd)
void wait_safe(const double seconds)
Definition TestUtils.cpp:17
For streams capable of holding different kinds of content, check what they actually have.
An interface to the operating system, including Port based communication.