YARP
Yet Another Robot Platform
 
Loading...
Searching...
No Matches
IPositionDirectTest.h
Go to the documentation of this file.
1/*
2 * SPDX-FileCopyrightText: 2025-2025 Istituto Italiano di Tecnologia (IIT)
3 * SPDX-License-Identifier: BSD-3-Clause
4 */
5
6#ifndef IPOSITIONDIRECTTEST_H
7#define IPOSITIONDIRECTTEST_H
8
9#include <memory>
10#include <numeric>
11#include <vector>
12
13#include <yarp/os/Time.h>
16#include <catch2/catch_amalgamated.hpp>
17#include "TestUtils.h"
18
19using namespace yarp::dev;
20using namespace yarp::os;
21
22namespace yarp::dev::tests
23{
25 {
26 REQUIRE(ipdir != nullptr);
27 REQUIRE(icmd != nullptr);
28
29 bool b=false;
30
31 size_t ax=0;
32 b = ipdir->getAxes(ax);
33 CHECK(b);
34 REQUIRE(ax != 0);
35
36 for (size_t i = 0; i < ax; i++)
37 {
39 CHECK(b);
40 wait_safe(); // Allow some time for the command to take effect
41
43 b = icmd->getControlMode(i, mode_ret);
44 CHECK(b);
46 }
47
48#if 0
49 {
50 double ref = 12;
51 double ref_check = 12;
52 auto refs = std::vector<double>(ax);
53 auto refs_check = std::vector<double>(ax);
54 auto joints = std::vector<int>(ax);
55 set_vector_crescent(joints.begin(), joints.end(), 0);
56
57 b = ipdir->setPosition(0, ref);
58 CHECK(b);
59 wait_safe(); // Allow some time for the command to take effect
60
61 b = ipdir->getRefPosition(0, ref);
62 CHECK(b);
63 CHECK(ref == ref_check);
64
65 set_vector_crescent(refs, 1.0);
67
68 b = ipdir->setPositions(refs);
69 CHECK(b);
70 wait_safe(); // Allow some time for the command to take effect
71
72 b = ipdir->getRefPositions(refs);
73 CHECK(b);
75
76 set_vector_crescent(refs, 10.0);
78
79 b = ipdir->setPositions(joints, refs);
80 CHECK(b);
81 wait_safe(); // Allow some time for the command to take effect
82
83 b = ipdir->getRefPositions(joints, refs);
84 CHECK(b);
86 }
87#else
88 {
89 double ref = 12;
90 double ref_check = 12;
91 auto refs = std::vector<double>(ax);
92 auto refs_check = std::vector<double>(ax);
93 auto joints = std::vector<int>(ax);
94 set_vector_crescent(ax, joints.data(), 0);
95
96 b = ipdir->setPosition(0, ref);
97 CHECK(b);
98 wait_safe(); // Allow some time for the command to take effect
99
100 b = ipdir->getRefPosition(0, &ref);
101 CHECK(b);
102 CHECK(ref == ref_check);
103
104 set_vector_crescent(ax,refs.data(), 1.0);
105 set_vector_crescent(ax,refs_check.data(), 1.0);
106
107 b = ipdir->setPositions(refs.data());
108 CHECK(b);
109 wait_safe(); // Allow some time for the command to take effect
110
111 b = ipdir->getRefPositions(refs.data());
112 CHECK(b);
113 CHECK(vectors_equal(ax, refs.data(), refs_check.data()));
114
115 set_vector_crescent(ax, refs.data(), 10.0);
116 set_vector_crescent(ax, refs_check.data(), 10.0);
117
118 b = ipdir->setPositions(ax, joints.data(), refs.data());
119 CHECK(b);
120 wait_safe(); // Allow some time for the command to take effect
121
122 b = ipdir->getRefPositions(ax, joints.data(), refs.data());
123 CHECK(b);
124 CHECK(vectors_equal(ax, refs.data(), refs_check.data()));
125 }
126 }
127}
128#endif
129
130#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 a generic control board device implementing position control.
A mini-server for performing network communication in the background.
bool vectors_equal(const std::vector< double > &a, const std::vector< double > &b)
Definition TestUtils.cpp:23
void exec_iPositionDirect_test_1(IPositionDirect *ipdir, IControlMode *icmd)
void set_vector_crescent(std::vector< double > &a, double val)
Definition TestUtils.cpp:65
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.