Skip to content

Commit

Permalink
Merge pull request #3026 from Nicogene/addIJointCoupling
Browse files Browse the repository at this point in the history
IJointCoupling: an interface for handling coupled joints
  • Loading branch information
randaz81 authored Nov 15, 2023
2 parents 5c9a5f1 + 5d9e3c3 commit 45b225c
Show file tree
Hide file tree
Showing 10 changed files with 775 additions and 0 deletions.
1 change: 1 addition & 0 deletions src/devices/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -26,6 +26,7 @@ yarp_begin_plugin_library(yarpmod
add_subdirectory(fakeAnalogSensor)
add_subdirectory(fakeBattery)
add_subdirectory(fakeIMU)
add_subdirectory(fakeJointCoupling)
add_subdirectory(fakeJoypad)
add_subdirectory(fakeLLMDevice)
add_subdirectory(fakeOdometry2D)
Expand Down
52 changes: 52 additions & 0 deletions src/devices/fakeJointCoupling/CMakeLists.txt
Original file line number Diff line number Diff line change
@@ -0,0 +1,52 @@
# SPDX-FileCopyrightText: 2006-2023 Istituto Italiano di Tecnologia (IIT)
# SPDX-License-Identifier: BSD-3-Clause

if (YARP_COMPILE_ALL_FAKE_DEVICES)
set(ENABLE_yarpmod_fakeJointCoupling ON CACHE BOOL "" FORCE)
endif()

yarp_prepare_plugin(fakeJointCoupling
CATEGORY device
TYPE FakeJointCoupling
INCLUDE fakeJointCoupling.h
)

if(NOT SKIP_fakeJointCoupling)
yarp_add_plugin(yarp_fakeJointCoupling)

target_sources(yarp_fakeJointCoupling
PRIVATE
fakeJointCoupling.cpp
fakeJointCoupling.h)

target_link_libraries(yarp_fakeJointCoupling
PRIVATE
YARP::YARP_os
YARP::YARP_sig
YARP::YARP_dev
)
list(APPEND YARP_${YARP_PLUGIN_MASTER}_PRIVATE_DEPS
YARP_os
YARP_sig
YARP_dev
YARP_math
)

yarp_install(
TARGETS yarp_fakeJointCoupling
EXPORT YARP_${YARP_PLUGIN_MASTER}
COMPONENT ${YARP_PLUGIN_MASTER}
LIBRARY DESTINATION ${YARP_DYNAMIC_PLUGINS_INSTALL_DIR}
ARCHIVE DESTINATION ${YARP_STATIC_PLUGINS_INSTALL_DIR}
YARP_INI DESTINATION ${YARP_PLUGIN_MANIFESTS_INSTALL_DIR}
)

set(YARP_${YARP_PLUGIN_MASTER}_PRIVATE_DEPS ${YARP_${YARP_PLUGIN_MASTER}_PRIVATE_DEPS} PARENT_SCOPE)

set_property(TARGET yarp_fakeJointCoupling PROPERTY FOLDER "Plugins/Device/Fake")

if(YARP_COMPILE_TESTS)
add_subdirectory(tests)
endif()

endif()
140 changes: 140 additions & 0 deletions src/devices/fakeJointCoupling/fakeJointCoupling.cpp
Original file line number Diff line number Diff line change
@@ -0,0 +1,140 @@
/*
* SPDX-FileCopyrightText: 2006-2023 Istituto Italiano di Tecnologia (IIT)
* SPDX-License-Identifier: BSD-3-Clause
*/

#include "fakeJointCoupling.h"

#include <yarp/conf/environment.h>

#include <yarp/os/Bottle.h>
#include <yarp/os/Time.h>
#include <yarp/os/LogComponent.h>
#include <yarp/os/LogStream.h>
#include <yarp/os/NetType.h>
#include <yarp/dev/Drivers.h>

#include <sstream>
#include <cstring>
#include <algorithm>

using namespace yarp::dev;
using namespace yarp::os;
using namespace yarp::os::impl;


namespace {
YARP_LOG_COMPONENT(FAKEJOINTCOUPLING, "yarp.device.fakeJointCoupling")
}

bool FakeJointCoupling::open(yarp::os::Searchable &par) {
yarp::sig::VectorOf<size_t> coupled_physical_joints {2,3};
yarp::sig::VectorOf<size_t> coupled_actuated_axes {2};
std::vector<std::string> physical_joint_names{"phys_joint_0", "phys_joint_1", "phys_joint_2", "phys_joint_3"};
std::vector<std::string> actuated_axes_names{"act_axes_0", "act_axes_1", "act_axes_2"};
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}};
initialise(coupled_physical_joints, coupled_actuated_axes, physical_joint_names, actuated_axes_names, physical_joint_limits);
return true;
}
bool FakeJointCoupling::close() {
return true;
}

bool FakeJointCoupling::convertFromPhysicalJointsToActuatedAxesPos(const yarp::sig::Vector& physJointsPos, yarp::sig::Vector& actAxesPos) {
size_t nrOfPhysicalJoints;
size_t nrOfActuatedAxes;
auto ok = getNrOfPhysicalJoints(nrOfPhysicalJoints);
ok = ok && getNrOfActuatedAxes(nrOfActuatedAxes);
if (!ok || physJointsPos.size() != nrOfPhysicalJoints || actAxesPos.size() != nrOfActuatedAxes) {
// yCDebug(FAKEJOINTCOUPLING) << ok <<physJointsPos.size()<<nrOfPhysicalJoints<<actAxesPos.size()<<nrOfActuatedAxes;
yCError(FAKEJOINTCOUPLING) << "convertFromPhysicalJointsToActuatedAxesPos: input or output vectors have wrong size";
return false;
}
actAxesPos[0] = physJointsPos[0];
actAxesPos[1] = physJointsPos[1];
actAxesPos[2] = physJointsPos[2] + physJointsPos[3];
return true;
}
bool FakeJointCoupling::convertFromPhysicalJointsToActuatedAxesVel(const yarp::sig::Vector& physJointsPos, const yarp::sig::Vector& physJointsVel, yarp::sig::Vector& actAxesVel) {
size_t nrOfPhysicalJoints;
size_t nrOfActuatedAxes;
auto ok = getNrOfPhysicalJoints(nrOfPhysicalJoints);
ok = ok && getNrOfActuatedAxes(nrOfActuatedAxes);
if (!ok || physJointsPos.size() != nrOfPhysicalJoints || physJointsVel.size() != nrOfPhysicalJoints || actAxesVel.size() != nrOfActuatedAxes) {
yCError(FAKEJOINTCOUPLING) << "convertFromPhysicalJointsToActuatedAxesPos: input or output vectors have wrong size";
return false;
}
actAxesVel[0] = physJointsVel[0];
actAxesVel[1] = physJointsVel[1];
actAxesVel[2] = physJointsPos[2] + physJointsPos[3] + physJointsVel[2] + physJointsVel[3];
return true;
}
bool FakeJointCoupling::convertFromPhysicalJointsToActuatedAxesAcc(const yarp::sig::Vector& physJointsPos, const yarp::sig::Vector& physJointsVel,
const yarp::sig::Vector& physJointsAcc, yarp::sig::Vector& actAxesAcc) {
size_t nrOfPhysicalJoints;
size_t nrOfActuatedAxes;
auto ok = getNrOfPhysicalJoints(nrOfPhysicalJoints);
ok = ok && getNrOfActuatedAxes(nrOfActuatedAxes);
if(!ok || physJointsPos.size() != nrOfPhysicalJoints || physJointsVel.size() != nrOfPhysicalJoints || physJointsAcc.size() != nrOfPhysicalJoints || actAxesAcc.size() != nrOfActuatedAxes) {
yCError(FAKEJOINTCOUPLING) << "convertFromPhysicalJointsToActuatedAxesPos: input or output vectors have wrong size";
return false;
}
actAxesAcc[0] = physJointsAcc[0];
actAxesAcc[1] = physJointsAcc[1];
actAxesAcc[2] = physJointsPos[2] + physJointsPos[3] + physJointsVel[2] + physJointsVel[3] + physJointsAcc[2] + physJointsAcc[3];
return true;
}
bool FakeJointCoupling::convertFromPhysicalJointsToActuatedAxesTrq(const yarp::sig::Vector& physJointsPos, const yarp::sig::Vector& physJointsTrq, yarp::sig::Vector& actAxesTrq) {
yCError(FAKEJOINTCOUPLING) << "convertFromPhysicalJointsToActuatedAxesTrq: not implemented yet";
return false;
}
bool FakeJointCoupling::convertFromActuatedAxesToPhysicalJointsPos(const yarp::sig::Vector& actAxesPos, yarp::sig::Vector& physJointsPos) {
size_t nrOfPhysicalJoints;
size_t nrOfActuatedAxes;
auto ok = getNrOfPhysicalJoints(nrOfPhysicalJoints);
ok = ok && getNrOfActuatedAxes(nrOfActuatedAxes);
if(!ok || actAxesPos.size() != nrOfActuatedAxes || physJointsPos.size() != nrOfPhysicalJoints) {
yCError(FAKEJOINTCOUPLING) << "convertFromActuatedAxesToPhysicalJointsPos: input or output vectors have wrong size";
return false;
}
physJointsPos[0] = actAxesPos[0];
physJointsPos[1] = actAxesPos[1];
physJointsPos[2] = actAxesPos[2] / 2.0;
physJointsPos[3] = actAxesPos[2] / 2.0;
return true;
}
bool FakeJointCoupling::convertFromActuatedAxesToPhysicalJointsVel(const yarp::sig::Vector& actAxesPos, const yarp::sig::Vector& actAxesVel, yarp::sig::Vector& physJointsVel) {
size_t nrOfPhysicalJoints;
size_t nrOfActuatedAxes;
auto ok = getNrOfPhysicalJoints(nrOfPhysicalJoints);
ok = ok && getNrOfActuatedAxes(nrOfActuatedAxes);
if(!ok || actAxesPos.size() != nrOfActuatedAxes || actAxesVel.size() != nrOfActuatedAxes || physJointsVel.size() != nrOfPhysicalJoints) {
yCError(FAKEJOINTCOUPLING) << "convertFromActuatedAxesToPhysicalJointsVel: input or output vectors have wrong size";
return false;
}
physJointsVel[0] = actAxesVel[0];
physJointsVel[1] = actAxesVel[1];
physJointsVel[2] = actAxesPos[2] / 2.0 - actAxesVel[2] / 2.0;
physJointsVel[3] = actAxesPos[2] / 2.0 + actAxesVel[2] / 2.0;
return true;

}
bool FakeJointCoupling::convertFromActuatedAxesToPhysicalJointsAcc(const yarp::sig::Vector& actAxesPos, const yarp::sig::Vector& actAxesVel, const yarp::sig::Vector& actAxesAcc, yarp::sig::Vector& physJointsAcc) {
size_t nrOfPhysicalJoints;
size_t nrOfActuatedAxes;
auto ok = getNrOfPhysicalJoints(nrOfPhysicalJoints);
ok = ok && getNrOfActuatedAxes(nrOfActuatedAxes);
if(!ok || actAxesPos.size() != nrOfActuatedAxes || actAxesVel.size() != nrOfActuatedAxes || actAxesAcc.size() != nrOfActuatedAxes || physJointsAcc.size() != nrOfPhysicalJoints) {
yCError(FAKEJOINTCOUPLING) << "convertFromActuatedAxesToPhysicalJointsAcc: input or output vectors have wrong size";
return false;
}
physJointsAcc[0] = actAxesAcc[0];
physJointsAcc[1] = actAxesAcc[1];
physJointsAcc[2] = actAxesPos[2] / 2.0 - actAxesVel[2] / 2.0 - actAxesAcc[2] / 2.0;
physJointsAcc[3] = actAxesPos[2] / 2.0 + actAxesVel[2] / 2.0 + actAxesAcc[2] / 2.0;
return true;
}
bool FakeJointCoupling::convertFromActuatedAxesToPhysicalJointsTrq(const yarp::sig::Vector& actAxesPos, const yarp::sig::Vector& actAxesTrq, yarp::sig::Vector& physJointsTrq) {
yCWarning(FAKEJOINTCOUPLING) << "convertFromActuatedAxesToPhysicalJointsTrq: not implemented yet";
return false;
}
54 changes: 54 additions & 0 deletions src/devices/fakeJointCoupling/fakeJointCoupling.h
Original file line number Diff line number Diff line change
@@ -0,0 +1,54 @@
/*
* SPDX-FileCopyrightText: 2006-2023 Istituto Italiano di Tecnologia (IIT)
* SPDX-License-Identifier: BSD-3-Clause
*/

#ifndef YARP_DEVICE_FAKE_JOINTCOUPLING
#define YARP_DEVICE_FAKE_JOINTCOUPLING

#include <yarp/os/Time.h>
#include <yarp/os/Bottle.h>
#include <yarp/sig/Vector.h>
#include <yarp/dev/DeviceDriver.h>
#include <yarp/dev/IJointCoupling.h>
#include <yarp/dev/ImplementJointCoupling.h>

#include <mutex>


/**
* @ingroup dev_impl_fake dev_impl_motor
*
* \brief `fakeJointCoupling`: Documentation to be added
*
* The aim of this device is to mimic the expected behavior of a
* joint coupling device to help testing the high level software.
*
* WIP - it is very basic now, not all interfaces are implemented yet.
*/
class FakeJointCoupling :
public yarp::dev::DeviceDriver,
public yarp::dev::ImplementJointCoupling
{
private:

public:

FakeJointCoupling() = default;
virtual ~FakeJointCoupling() = default;
// Device Driver
bool open(yarp::os::Searchable &par) override;
bool close() override;
// IJointCoupling
bool convertFromPhysicalJointsToActuatedAxesPos(const yarp::sig::Vector& physJointsPos, yarp::sig::Vector& actAxesPos) override;
bool convertFromPhysicalJointsToActuatedAxesVel(const yarp::sig::Vector& physJointsPos, const yarp::sig::Vector& physJointsVel, yarp::sig::Vector& actAxesVel) override;
bool convertFromPhysicalJointsToActuatedAxesAcc(const yarp::sig::Vector& physJointsPos, const yarp::sig::Vector& physJointsVel, const yarp::sig::Vector& physJointsAcc, yarp::sig::Vector& actAxesAcc) override;
bool convertFromPhysicalJointsToActuatedAxesTrq(const yarp::sig::Vector& physJointsPos, const yarp::sig::Vector& physJointsTrq, yarp::sig::Vector& actAxesTrq) override;
bool convertFromActuatedAxesToPhysicalJointsPos(const yarp::sig::Vector& actAxesPos, yarp::sig::Vector& physJointsPos) override;
bool convertFromActuatedAxesToPhysicalJointsVel(const yarp::sig::Vector& actAxesPos, const yarp::sig::Vector& actAxesVel, yarp::sig::Vector& physJointsVel) override;
bool convertFromActuatedAxesToPhysicalJointsAcc(const yarp::sig::Vector& actAxesPos, const yarp::sig::Vector& actAxesVel, const yarp::sig::Vector& actAxesAcc, yarp::sig::Vector& physJointsAcc) override;
bool convertFromActuatedAxesToPhysicalJointsTrq(const yarp::sig::Vector& actAxesPos, const yarp::sig::Vector& actAxesTrq, yarp::sig::Vector& physJointsTrq) override;

};

#endif // YARP_DEVICE_FAKE_JOINTCOUPLING
4 changes: 4 additions & 0 deletions src/devices/fakeJointCoupling/tests/CMakeLists.txt
Original file line number Diff line number Diff line change
@@ -0,0 +1,4 @@
# SPDX-FileCopyrightText: 2006-2021 Istituto Italiano di Tecnologia (IIT)
# SPDX-License-Identifier: BSD-3-Clause

create_device_test(fakeJointCoupling)
Loading

0 comments on commit 45b225c

Please sign in to comment.