YARP
Yet Another Robot Platform
 
Loading...
Searching...
No Matches
fakeJointCoupling_test.cpp
Go to the documentation of this file.
1/*
2 * SPDX-FileCopyrightText: 2006-2021 Istituto Italiano di Tecnologia (IIT)
3 * SPDX-License-Identifier: BSD-3-Clause
4 */
5
6#include <yarp/os/Network.h>
9
11#include <catch2/catch_amalgamated.hpp>
12#include <harness.h>
13
14using namespace yarp::dev;
15using namespace yarp::os;
16
17TEST_CASE("dev::fakeJointCoupling", "[yarp::dev]")
18{
19 YARP_REQUIRE_PLUGIN("fakeJointCoupling", "device");
20
21 Network::setLocalMode(true);
22
23 SECTION("Checking fakeJointCoupling device")
24 {
26 IJointCoupling* ijc=nullptr;
27 {
29 p_cfg.put("device", "fakeJointCoupling");
32 }
33
34 REQUIRE(ddjc.view(ijc));
35 REQUIRE(ijc!=nullptr);
38 CHECK(ijc->getCoupledPhysicalJoints(coupled_physical_joints));
39 CHECK(coupled_physical_joints.size() == 2);
42
45 CHECK(ijc->getCoupledActuatedAxes(coupled_actuated_axes));
46 CHECK(coupled_actuated_axes.size() == 1);
48
49 size_t nr_of_physical_joints{0};
50 CHECK(ijc->getNrOfPhysicalJoints(nr_of_physical_joints));
52
53 size_t nr_of_actuated_axes{0};
54 CHECK(ijc->getNrOfActuatedAxes(nr_of_actuated_axes));
56
57 std::string physical_joint_name;
58 CHECK(ijc->getPhysicalJointName(0, physical_joint_name));
59 CHECK(physical_joint_name == "phys_joint_0");
60 CHECK(ijc->getPhysicalJointName(1, physical_joint_name));
61 CHECK(physical_joint_name == "phys_joint_1");
62 CHECK(ijc->getPhysicalJointName(2, physical_joint_name));
63 CHECK(physical_joint_name == "phys_joint_2");
64 CHECK(ijc->getPhysicalJointName(3, physical_joint_name));
65 CHECK(physical_joint_name == "phys_joint_3");
66 CHECK_FALSE(ijc->getPhysicalJointName(4, physical_joint_name));
67
68 std::string actuated_axis_name;
69 CHECK(ijc->getActuatedAxisName(0, actuated_axis_name));
70 CHECK(actuated_axis_name == "act_axes_0");
71 CHECK(ijc->getActuatedAxisName(1, actuated_axis_name));
72 CHECK(actuated_axis_name == "act_axes_1");
73 CHECK(ijc->getActuatedAxisName(2, actuated_axis_name));
74 CHECK(actuated_axis_name == "act_axes_2");
75 CHECK_FALSE(ijc->getActuatedAxisName(3, actuated_axis_name));
76
77 yarp::sig::Vector physJointsPos{0.0, 1.0, 2.0, 3.0};
78 yarp::sig::Vector actAxesPos{0.0, 0.0, 0.0};
79 yarp::sig::Vector physJointsVel{0.0, 1.0, 2.0, 3.0};
80 yarp::sig::Vector actAxesVel{0.0, 0.0, 0.0};
81 yarp::sig::Vector physJointsAcc{0.0, 1.0, 2.0, 3.0};
82 yarp::sig::Vector actAxesAcc{0.0, 0.0, 0.0};;
83
84 CHECK(ijc->convertFromPhysicalJointsToActuatedAxesPos(physJointsPos, actAxesPos));
85 CHECK(actAxesPos[0] == 0.0);
86 CHECK(actAxesPos[1] == 1.0);
87 CHECK(actAxesPos[2] == 5.0);
88
89 CHECK(ijc->convertFromPhysicalJointsToActuatedAxesVel(physJointsPos, physJointsVel, actAxesVel));
90 CHECK(actAxesVel[0] == 0.0);
91 CHECK(actAxesVel[1] == 1.0);
92 CHECK(actAxesVel[2] == 10.0);
93
94 CHECK(ijc->convertFromPhysicalJointsToActuatedAxesAcc(physJointsPos, physJointsVel, physJointsAcc, actAxesAcc));
95 CHECK(actAxesAcc[0] == 0.0);
96 CHECK(actAxesAcc[1] == 1.0);
97 CHECK(actAxesAcc[2] == 15.0);
98
99 CHECK_FALSE(ijc->convertFromPhysicalJointsToActuatedAxesTrq(physJointsPos, physJointsAcc, actAxesAcc));
100
101 CHECK(ijc->convertFromActuatedAxesToPhysicalJointsPos(actAxesPos, physJointsPos));
102 CHECK(physJointsPos[0] == 0.0);
103 CHECK(physJointsPos[1] == 1.0);
104 CHECK(physJointsPos[2] == 2.5);
105 CHECK(physJointsPos[3] == 2.5);
106
107 CHECK(ijc->convertFromActuatedAxesToPhysicalJointsVel(actAxesPos, actAxesVel, physJointsVel));
108 CHECK(physJointsVel[0] == 0.0);
109 CHECK(physJointsVel[1] == 1.0);
110 CHECK(physJointsVel[2] == -2.5);
111 CHECK(physJointsVel[3] == 7.5);
112
113 CHECK(ijc->convertFromActuatedAxesToPhysicalJointsAcc(actAxesPos, actAxesVel, actAxesAcc, physJointsAcc));
114 CHECK(physJointsAcc[0] == 0.0);
115 CHECK(physJointsAcc[1] == 1.0);
116 CHECK(physJointsAcc[2] == -10.0);
117 CHECK(physJointsAcc[3] == 15.0);
118
119 CHECK_FALSE(ijc->convertFromActuatedAxesToPhysicalJointsTrq(actAxesPos, actAxesAcc, physJointsAcc));
120
121 double min{0.0}, max{0.0};
122 CHECK(ijc->getPhysicalJointLimits(0, min, max));
123 CHECK(min == -30.0);
124 CHECK(max == 30.0);
125 CHECK(ijc->getPhysicalJointLimits(1, min, max));
126 CHECK(min == -10.0);
127 CHECK(max == 10.0);
128 CHECK(ijc->getPhysicalJointLimits(2, min, max));
129 CHECK(min == -32.0);
130 CHECK(max == 33.0);
131 CHECK(ijc->getPhysicalJointLimits(3, min, max));
132 CHECK(min == 0.0);
133 CHECK(max == 120.0);
134
135
136
137
138 //"Close all polydrivers and check"
139 {
140 CHECK(ddjc.close());
141 }
142 }
143
144 Network::setLocalMode(false);
145}
A container for a device driver.
Definition PolyDriver.h:23
A mini-server for performing network communication in the background.
void close() override
Stop port activity.
bool open(const std::string &name) override
Start port operation, with a specific name, with automatically-chosen network parameters.
A class for storing options and configuration information.
Definition Property.h:32
TEST_CASE("dev::fakeJointCoupling", "[yarp::dev]")
void exec_params_test(yarp::dev::PolyDriver *pdrv)
For streams capable of holding different kinds of content, check what they actually have.
An interface to the operating system, including Port based communication.
typename vector_selector< T >::type VectorOf
Definition Vector.h:174