Skip to content

Commit 45b225c

Browse files
randaz81randaz81
authored
Merge pull request #3026 from Nicogene/addIJointCoupling
IJointCoupling: an interface for handling coupled joints
2 parents 5c9a5f1 + 5d9e3c3 commit 45b225c

File tree

10 files changed

+775
-0
lines changed

10 files changed

+775
-0
lines changed

‎src/devices/CMakeLists.txt‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -26,6 +26,7 @@ yarp_begin_plugin_library(yarpmod
2626
add_subdirectory(fakeAnalogSensor)
2727
add_subdirectory(fakeBattery)
2828
add_subdirectory(fakeIMU)
29+
add_subdirectory(fakeJointCoupling)
2930
add_subdirectory(fakeJoypad)
3031
add_subdirectory(fakeLLMDevice)
3132
add_subdirectory(fakeOdometry2D)

‎src/devices/fakeJointCoupling/CMakeLists.txt‎

Lines changed: 52 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,52 @@
1+
# SPDX-FileCopyrightText: 2006-2023 Istituto Italiano di Tecnologia (IIT)
2+
# SPDX-License-Identifier: BSD-3-Clause
3+
4+
if (YARP_COMPILE_ALL_FAKE_DEVICES)
5+
set(ENABLE_yarpmod_fakeJointCoupling ON CACHE BOOL "" FORCE)
6+
endif()
7+
8+
yarp_prepare_plugin(fakeJointCoupling
9+
CATEGORY device
10+
TYPE FakeJointCoupling
11+
INCLUDE fakeJointCoupling.h
12+
)
13+
14+
if(NOT SKIP_fakeJointCoupling)
15+
yarp_add_plugin(yarp_fakeJointCoupling)
16+
17+
target_sources(yarp_fakeJointCoupling
18+
PRIVATE
19+
fakeJointCoupling.cpp
20+
fakeJointCoupling.h)
21+
22+
target_link_libraries(yarp_fakeJointCoupling
23+
PRIVATE
24+
YARP::YARP_os
25+
YARP::YARP_sig
26+
YARP::YARP_dev
27+
)
28+
list(APPEND YARP_${YARP_PLUGIN_MASTER}_PRIVATE_DEPS
29+
YARP_os
30+
YARP_sig
31+
YARP_dev
32+
YARP_math
33+
)
34+
35+
yarp_install(
36+
TARGETS yarp_fakeJointCoupling
37+
EXPORT YARP_${YARP_PLUGIN_MASTER}
38+
COMPONENT ${YARP_PLUGIN_MASTER}
39+
LIBRARY DESTINATION ${YARP_DYNAMIC_PLUGINS_INSTALL_DIR}
40+
ARCHIVE DESTINATION ${YARP_STATIC_PLUGINS_INSTALL_DIR}
41+
YARP_INI DESTINATION ${YARP_PLUGIN_MANIFESTS_INSTALL_DIR}
42+
)
43+
44+
set(YARP_${YARP_PLUGIN_MASTER}_PRIVATE_DEPS ${YARP_${YARP_PLUGIN_MASTER}_PRIVATE_DEPS} PARENT_SCOPE)
45+
46+
set_property(TARGET yarp_fakeJointCoupling PROPERTY FOLDER "Plugins/Device/Fake")
47+
48+
if(YARP_COMPILE_TESTS)
49+
add_subdirectory(tests)
50+
endif()
51+
52+
endif()

‎src/devices/fakeJointCoupling/fakeJointCoupling.cpp‎

Lines changed: 140 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,140 @@
1+
/*
2+
* SPDX-FileCopyrightText: 2006-2023 Istituto Italiano di Tecnologia (IIT)
3+
* SPDX-License-Identifier: BSD-3-Clause
4+
*/
5+
6+
#include "fakeJointCoupling.h"
7+
8+
#include <yarp/conf/environment.h>
9+
10+
#include <yarp/os/Bottle.h>
11+
#include <yarp/os/Time.h>
12+
#include <yarp/os/LogComponent.h>
13+
#include <yarp/os/LogStream.h>
14+
#include <yarp/os/NetType.h>
15+
#include <yarp/dev/Drivers.h>
16+
17+
#include <sstream>
18+
#include <cstring>
19+
#include <algorithm>
20+
21+
using namespace yarp::dev;
22+
using namespace yarp::os;
23+
using namespace yarp::os::impl;
24+
25+
26+
namespace {
27+
YARP_LOG_COMPONENT(FAKEJOINTCOUPLING, "yarp.device.fakeJointCoupling")
28+
}
29+
30+
bool FakeJointCoupling::open(yarp::os::Searchable &par) {
31+
yarp::sig::VectorOf<size_t> coupled_physical_joints {2,3};
32+
yarp::sig::VectorOf<size_t> coupled_actuated_axes {2};
33+
std::vector<std::string> physical_joint_names{"phys_joint_0", "phys_joint_1", "phys_joint_2", "phys_joint_3"};
34+
std::vector<std::string> actuated_axes_names{"act_axes_0", "act_axes_1", "act_axes_2"};
35+
std::vector<std::pair<double, double>> physical_joint_limits{{-30.0, 30.0}, {-10.0, 10.0}, {-32.0, 33.0}, {0.0, 120.0}};
36+
initialise(coupled_physical_joints, coupled_actuated_axes, physical_joint_names, actuated_axes_names, physical_joint_limits);
37+
return true;
38+
}
39+
bool FakeJointCoupling::close() {
40+
return true;
41+
}
42+
43+
bool FakeJointCoupling::convertFromPhysicalJointsToActuatedAxesPos(const yarp::sig::Vector& physJointsPos, yarp::sig::Vector& actAxesPos) {
44+
size_t nrOfPhysicalJoints;
45+
size_t nrOfActuatedAxes;
46+
auto ok = getNrOfPhysicalJoints(nrOfPhysicalJoints);
47+
ok = ok && getNrOfActuatedAxes(nrOfActuatedAxes);
48+
if (!ok || physJointsPos.size() != nrOfPhysicalJoints || actAxesPos.size() != nrOfActuatedAxes) {
49+
// yCDebug(FAKEJOINTCOUPLING) << ok <<physJointsPos.size()<<nrOfPhysicalJoints<<actAxesPos.size()<<nrOfActuatedAxes;
50+
yCError(FAKEJOINTCOUPLING) << "convertFromPhysicalJointsToActuatedAxesPos: input or output vectors have wrong size";
51+
return false;
52+
}
53+
actAxesPos[0] = physJointsPos[0];
54+
actAxesPos[1] = physJointsPos[1];
55+
actAxesPos[2] = physJointsPos[2] + physJointsPos[3];
56+
return true;
57+
}
58+
bool FakeJointCoupling::convertFromPhysicalJointsToActuatedAxesVel(const yarp::sig::Vector& physJointsPos, const yarp::sig::Vector& physJointsVel, yarp::sig::Vector& actAxesVel) {
59+
size_t nrOfPhysicalJoints;
60+
size_t nrOfActuatedAxes;
61+
auto ok = getNrOfPhysicalJoints(nrOfPhysicalJoints);
62+
ok = ok && getNrOfActuatedAxes(nrOfActuatedAxes);
63+
if (!ok || physJointsPos.size() != nrOfPhysicalJoints || physJointsVel.size() != nrOfPhysicalJoints || actAxesVel.size() != nrOfActuatedAxes) {
64+
yCError(FAKEJOINTCOUPLING) << "convertFromPhysicalJointsToActuatedAxesPos: input or output vectors have wrong size";
65+
return false;
66+
}
67+
actAxesVel[0] = physJointsVel[0];
68+
actAxesVel[1] = physJointsVel[1];
69+
actAxesVel[2] = physJointsPos[2] + physJointsPos[3] + physJointsVel[2] + physJointsVel[3];
70+
return true;
71+
}
72+
bool FakeJointCoupling::convertFromPhysicalJointsToActuatedAxesAcc(const yarp::sig::Vector& physJointsPos, const yarp::sig::Vector& physJointsVel,
73+
const yarp::sig::Vector& physJointsAcc, yarp::sig::Vector& actAxesAcc) {
74+
size_t nrOfPhysicalJoints;
75+
size_t nrOfActuatedAxes;
76+
auto ok = getNrOfPhysicalJoints(nrOfPhysicalJoints);
77+
ok = ok && getNrOfActuatedAxes(nrOfActuatedAxes);
78+
if(!ok || physJointsPos.size() != nrOfPhysicalJoints || physJointsVel.size() != nrOfPhysicalJoints || physJointsAcc.size() != nrOfPhysicalJoints || actAxesAcc.size() != nrOfActuatedAxes) {
79+
yCError(FAKEJOINTCOUPLING) << "convertFromPhysicalJointsToActuatedAxesPos: input or output vectors have wrong size";
80+
return false;
81+
}
82+
actAxesAcc[0] = physJointsAcc[0];
83+
actAxesAcc[1] = physJointsAcc[1];
84+
actAxesAcc[2] = physJointsPos[2] + physJointsPos[3] + physJointsVel[2] + physJointsVel[3] + physJointsAcc[2] + physJointsAcc[3];
85+
return true;
86+
}
87+
bool FakeJointCoupling::convertFromPhysicalJointsToActuatedAxesTrq(const yarp::sig::Vector& physJointsPos, const yarp::sig::Vector& physJointsTrq, yarp::sig::Vector& actAxesTrq) {
88+
yCError(FAKEJOINTCOUPLING) << "convertFromPhysicalJointsToActuatedAxesTrq: not implemented yet";
89+
return false;
90+
}
91+
bool FakeJointCoupling::convertFromActuatedAxesToPhysicalJointsPos(const yarp::sig::Vector& actAxesPos, yarp::sig::Vector& physJointsPos) {
92+
size_t nrOfPhysicalJoints;
93+
size_t nrOfActuatedAxes;
94+
auto ok = getNrOfPhysicalJoints(nrOfPhysicalJoints);
95+
ok = ok && getNrOfActuatedAxes(nrOfActuatedAxes);
96+
if(!ok || actAxesPos.size() != nrOfActuatedAxes || physJointsPos.size() != nrOfPhysicalJoints) {
97+
yCError(FAKEJOINTCOUPLING) << "convertFromActuatedAxesToPhysicalJointsPos: input or output vectors have wrong size";
98+
return false;
99+
}
100+
physJointsPos[0] = actAxesPos[0];
101+
physJointsPos[1] = actAxesPos[1];
102+
physJointsPos[2] = actAxesPos[2] / 2.0;
103+
physJointsPos[3] = actAxesPos[2] / 2.0;
104+
return true;
105+
}
106+
bool FakeJointCoupling::convertFromActuatedAxesToPhysicalJointsVel(const yarp::sig::Vector& actAxesPos, const yarp::sig::Vector& actAxesVel, yarp::sig::Vector& physJointsVel) {
107+
size_t nrOfPhysicalJoints;
108+
size_t nrOfActuatedAxes;
109+
auto ok = getNrOfPhysicalJoints(nrOfPhysicalJoints);
110+
ok = ok && getNrOfActuatedAxes(nrOfActuatedAxes);
111+
if(!ok || actAxesPos.size() != nrOfActuatedAxes || actAxesVel.size() != nrOfActuatedAxes || physJointsVel.size() != nrOfPhysicalJoints) {
112+
yCError(FAKEJOINTCOUPLING) << "convertFromActuatedAxesToPhysicalJointsVel: input or output vectors have wrong size";
113+
return false;
114+
}
115+
physJointsVel[0] = actAxesVel[0];
116+
physJointsVel[1] = actAxesVel[1];
117+
physJointsVel[2] = actAxesPos[2] / 2.0 - actAxesVel[2] / 2.0;
118+
physJointsVel[3] = actAxesPos[2] / 2.0 + actAxesVel[2] / 2.0;
119+
return true;
120+
121+
}
122+
bool FakeJointCoupling::convertFromActuatedAxesToPhysicalJointsAcc(const yarp::sig::Vector& actAxesPos, const yarp::sig::Vector& actAxesVel, const yarp::sig::Vector& actAxesAcc, yarp::sig::Vector& physJointsAcc) {
123+
size_t nrOfPhysicalJoints;
124+
size_t nrOfActuatedAxes;
125+
auto ok = getNrOfPhysicalJoints(nrOfPhysicalJoints);
126+
ok = ok && getNrOfActuatedAxes(nrOfActuatedAxes);
127+
if(!ok || actAxesPos.size() != nrOfActuatedAxes || actAxesVel.size() != nrOfActuatedAxes || actAxesAcc.size() != nrOfActuatedAxes || physJointsAcc.size() != nrOfPhysicalJoints) {
128+
yCError(FAKEJOINTCOUPLING) << "convertFromActuatedAxesToPhysicalJointsAcc: input or output vectors have wrong size";
129+
return false;
130+
}
131+
physJointsAcc[0] = actAxesAcc[0];
132+
physJointsAcc[1] = actAxesAcc[1];
133+
physJointsAcc[2] = actAxesPos[2] / 2.0 - actAxesVel[2] / 2.0 - actAxesAcc[2] / 2.0;
134+
physJointsAcc[3] = actAxesPos[2] / 2.0 + actAxesVel[2] / 2.0 + actAxesAcc[2] / 2.0;
135+
return true;
136+
}
137+
bool FakeJointCoupling::convertFromActuatedAxesToPhysicalJointsTrq(const yarp::sig::Vector& actAxesPos, const yarp::sig::Vector& actAxesTrq, yarp::sig::Vector& physJointsTrq) {
138+
yCWarning(FAKEJOINTCOUPLING) << "convertFromActuatedAxesToPhysicalJointsTrq: not implemented yet";
139+
return false;
140+
}

‎src/devices/fakeJointCoupling/fakeJointCoupling.h‎

Lines changed: 54 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,54 @@
1+
/*
2+
* SPDX-FileCopyrightText: 2006-2023 Istituto Italiano di Tecnologia (IIT)
3+
* SPDX-License-Identifier: BSD-3-Clause
4+
*/
5+
6+
#ifndef YARP_DEVICE_FAKE_JOINTCOUPLING
7+
#define YARP_DEVICE_FAKE_JOINTCOUPLING
8+
9+
#include <yarp/os/Time.h>
10+
#include <yarp/os/Bottle.h>
11+
#include <yarp/sig/Vector.h>
12+
#include <yarp/dev/DeviceDriver.h>
13+
#include <yarp/dev/IJointCoupling.h>
14+
#include <yarp/dev/ImplementJointCoupling.h>
15+
16+
#include <mutex>
17+
18+
19+
/**
20+
* @ingroup dev_impl_fake dev_impl_motor
21+
*
22+
* \brief `fakeJointCoupling`: Documentation to be added
23+
*
24+
* The aim of this device is to mimic the expected behavior of a
25+
* joint coupling device to help testing the high level software.
26+
*
27+
* WIP - it is very basic now, not all interfaces are implemented yet.
28+
*/
29+
class FakeJointCoupling :
30+
public yarp::dev::DeviceDriver,
31+
public yarp::dev::ImplementJointCoupling
32+
{
33+
private:
34+
35+
public:
36+
37+
FakeJointCoupling() = default;
38+
virtual ~FakeJointCoupling() = default;
39+
// Device Driver
40+
bool open(yarp::os::Searchable &par) override;
41+
bool close() override;
42+
// IJointCoupling
43+
bool convertFromPhysicalJointsToActuatedAxesPos(const yarp::sig::Vector& physJointsPos, yarp::sig::Vector& actAxesPos) override;
44+
bool convertFromPhysicalJointsToActuatedAxesVel(const yarp::sig::Vector& physJointsPos, const yarp::sig::Vector& physJointsVel, yarp::sig::Vector& actAxesVel) override;
45+
bool convertFromPhysicalJointsToActuatedAxesAcc(const yarp::sig::Vector& physJointsPos, const yarp::sig::Vector& physJointsVel, const yarp::sig::Vector& physJointsAcc, yarp::sig::Vector& actAxesAcc) override;
46+
bool convertFromPhysicalJointsToActuatedAxesTrq(const yarp::sig::Vector& physJointsPos, const yarp::sig::Vector& physJointsTrq, yarp::sig::Vector& actAxesTrq) override;
47+
bool convertFromActuatedAxesToPhysicalJointsPos(const yarp::sig::Vector& actAxesPos, yarp::sig::Vector& physJointsPos) override;
48+
bool convertFromActuatedAxesToPhysicalJointsVel(const yarp::sig::Vector& actAxesPos, const yarp::sig::Vector& actAxesVel, yarp::sig::Vector& physJointsVel) override;
49+
bool convertFromActuatedAxesToPhysicalJointsAcc(const yarp::sig::Vector& actAxesPos, const yarp::sig::Vector& actAxesVel, const yarp::sig::Vector& actAxesAcc, yarp::sig::Vector& physJointsAcc) override;
50+
bool convertFromActuatedAxesToPhysicalJointsTrq(const yarp::sig::Vector& actAxesPos, const yarp::sig::Vector& actAxesTrq, yarp::sig::Vector& physJointsTrq) override;
51+
52+
};
53+
54+
#endif // YARP_DEVICE_FAKE_JOINTCOUPLING

‎src/devices/fakeJointCoupling/tests/CMakeLists.txt‎

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,4 @@
1+
# SPDX-FileCopyrightText: 2006-2021 Istituto Italiano di Tecnologia (IIT)
2+
# SPDX-License-Identifier: BSD-3-Clause
3+
4+
create_device_test(fakeJointCoupling)

0 commit comments

Comments
 (0)