YARP
Yet Another Robot Platform
 
Loading...
Searching...
No Matches
FakeJointCoupling.cpp
Go to the documentation of this file.
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
9
10#include <yarp/os/Bottle.h>
11#include <yarp/os/Time.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
21using namespace yarp::dev;
22using namespace yarp::os;
23using namespace yarp::os::impl;
24
25
26namespace {
27YARP_LOG_COMPONENT(FAKEJOINTCOUPLING, "yarp.device.fakeJointCoupling")
28}
29
31{
32 if (!this->parseParams(config)) {return false;}
33
36 std::vector<std::string> physical_joint_names{"phys_joint_0", "phys_joint_1", "phys_joint_2", "phys_joint_3"};
37 std::vector<std::string> actuated_axes_names{"act_axes_0", "act_axes_1", "act_axes_2"};
38 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}};
40 return true;
41}
43 return true;
44}
45
47 size_t nrOfPhysicalJoints;
48 size_t nrOfActuatedAxes;
51 if (!ok || physJointsPos.size() != nrOfPhysicalJoints || actAxesPos.size() != nrOfActuatedAxes) {
52 // yCDebug(FAKEJOINTCOUPLING) << ok <<physJointsPos.size()<<nrOfPhysicalJoints<<actAxesPos.size()<<nrOfActuatedAxes;
53 yCError(FAKEJOINTCOUPLING) << "convertFromPhysicalJointsToActuatedAxesPos: input or output vectors have wrong size";
54 return ReturnValue::return_code::return_value_error_input_out_of_bounds;
55 }
56 actAxesPos[0] = physJointsPos[0];
57 actAxesPos[1] = physJointsPos[1];
58 actAxesPos[2] = physJointsPos[2] + physJointsPos[3];
59 return ReturnValue_ok;
60}
62 size_t nrOfPhysicalJoints;
63 size_t nrOfActuatedAxes;
66 if (!ok || physJointsPos.size() != nrOfPhysicalJoints || physJointsVel.size() != nrOfPhysicalJoints || actAxesVel.size() != nrOfActuatedAxes) {
67 yCError(FAKEJOINTCOUPLING) << "convertFromPhysicalJointsToActuatedAxesPos: input or output vectors have wrong size";
68 return ReturnValue::return_code::return_value_error_input_out_of_bounds;
69 }
70 actAxesVel[0] = physJointsVel[0];
71 actAxesVel[1] = physJointsVel[1];
72 actAxesVel[2] = physJointsPos[2] + physJointsPos[3] + physJointsVel[2] + physJointsVel[3];
73 return ReturnValue_ok;
74}
76 const yarp::sig::Vector& physJointsAcc, yarp::sig::Vector& actAxesAcc) {
77 size_t nrOfPhysicalJoints;
78 size_t nrOfActuatedAxes;
81 if(!ok || physJointsPos.size() != nrOfPhysicalJoints || physJointsVel.size() != nrOfPhysicalJoints || physJointsAcc.size() != nrOfPhysicalJoints || actAxesAcc.size() != nrOfActuatedAxes) {
82 yCError(FAKEJOINTCOUPLING) << "convertFromPhysicalJointsToActuatedAxesPos: input or output vectors have wrong size";
83 return ReturnValue::return_code::return_value_error_input_out_of_bounds;
84 }
85 actAxesAcc[0] = physJointsAcc[0];
86 actAxesAcc[1] = physJointsAcc[1];
87 actAxesAcc[2] = physJointsPos[2] + physJointsPos[3] + physJointsVel[2] + physJointsVel[3] + physJointsAcc[2] + physJointsAcc[3];
88 return ReturnValue_ok;
89}
94 size_t nrOfPhysicalJoints;
95 size_t nrOfActuatedAxes;
98 if(!ok || actAxesPos.size() != nrOfActuatedAxes || physJointsPos.size() != nrOfPhysicalJoints) {
99 yCError(FAKEJOINTCOUPLING) << "convertFromActuatedAxesToPhysicalJointsPos: input or output vectors have wrong size";
100 return ReturnValue::return_code::return_value_error_input_out_of_bounds;
101 }
102 physJointsPos[0] = actAxesPos[0];
103 physJointsPos[1] = actAxesPos[1];
104 physJointsPos[2] = actAxesPos[2] / 2.0;
105 physJointsPos[3] = actAxesPos[2] / 2.0;
106 return ReturnValue_ok;
107}
109 size_t nrOfPhysicalJoints;
110 size_t nrOfActuatedAxes;
113 if(!ok || actAxesPos.size() != nrOfActuatedAxes || actAxesVel.size() != nrOfActuatedAxes || physJointsVel.size() != nrOfPhysicalJoints) {
114 yCError(FAKEJOINTCOUPLING) << "convertFromActuatedAxesToPhysicalJointsVel: input or output vectors have wrong size";
115 return ReturnValue::return_code::return_value_error_input_out_of_bounds;
116 }
117 physJointsVel[0] = actAxesVel[0];
118 physJointsVel[1] = actAxesVel[1];
119 physJointsVel[2] = actAxesPos[2] / 2.0 - actAxesVel[2] / 2.0;
120 physJointsVel[3] = actAxesPos[2] / 2.0 + actAxesVel[2] / 2.0;
121 return ReturnValue_ok;
122
123}
125 size_t nrOfPhysicalJoints;
126 size_t nrOfActuatedAxes;
129 if(!ok || actAxesPos.size() != nrOfActuatedAxes || actAxesVel.size() != nrOfActuatedAxes || actAxesAcc.size() != nrOfActuatedAxes || physJointsAcc.size() != nrOfPhysicalJoints) {
130 yCError(FAKEJOINTCOUPLING) << "convertFromActuatedAxesToPhysicalJointsAcc: input or output vectors have wrong size";
131 return ReturnValue::return_code::return_value_error_input_out_of_bounds;
132 }
133 physJointsAcc[0] = actAxesAcc[0];
134 physJointsAcc[1] = actAxesAcc[1];
135 physJointsAcc[2] = actAxesPos[2] / 2.0 - actAxesVel[2] / 2.0 - actAxesAcc[2] / 2.0;
136 physJointsAcc[3] = actAxesPos[2] / 2.0 + actAxesVel[2] / 2.0 + actAxesAcc[2] / 2.0;
137 return ReturnValue_ok;
138}
#define ReturnValue_ok
Definition ReturnValue.h:82
#define YARP_METHOD_NOT_YET_IMPLEMENTED()
bool parseParams(const yarp::os::Searchable &config) override
Parse the DeviceDriver parameters.
yarp::dev::ReturnValue convertFromPhysicalJointsToActuatedAxesAcc(const yarp::sig::Vector &physJointsPos, const yarp::sig::Vector &physJointsVel, const yarp::sig::Vector &physJointsAcc, yarp::sig::Vector &actAxesAcc) override
yarp::dev::ReturnValue convertFromActuatedAxesToPhysicalJointsVel(const yarp::sig::Vector &actAxesPos, const yarp::sig::Vector &actAxesVel, yarp::sig::Vector &physJointsVel) override
bool open(yarp::os::Searchable &par) override
Open the DeviceDriver.
yarp::dev::ReturnValue convertFromPhysicalJointsToActuatedAxesPos(const yarp::sig::Vector &physJointsPos, yarp::sig::Vector &actAxesPos) override
yarp::dev::ReturnValue convertFromActuatedAxesToPhysicalJointsPos(const yarp::sig::Vector &actAxesPos, yarp::sig::Vector &physJointsPos) override
yarp::dev::ReturnValue convertFromPhysicalJointsToActuatedAxesVel(const yarp::sig::Vector &physJointsPos, const yarp::sig::Vector &physJointsVel, yarp::sig::Vector &actAxesVel) override
yarp::dev::ReturnValue convertFromActuatedAxesToPhysicalJointsTrq(const yarp::sig::Vector &actAxesPos, const yarp::sig::Vector &actAxesTrq, yarp::sig::Vector &physJointsTrq) override
yarp::dev::ReturnValue convertFromActuatedAxesToPhysicalJointsAcc(const yarp::sig::Vector &actAxesPos, const yarp::sig::Vector &actAxesVel, const yarp::sig::Vector &actAxesAcc, yarp::sig::Vector &physJointsAcc) override
yarp::dev::ReturnValue convertFromPhysicalJointsToActuatedAxesTrq(const yarp::sig::Vector &physJointsPos, const yarp::sig::Vector &physJointsTrq, yarp::sig::Vector &actAxesTrq) override
bool close() override
Close the DeviceDriver.
yarp::dev::ReturnValue getNrOfActuatedAxes(size_t &nrOfActuatedAxes) override final
yarp::dev::ReturnValue getNrOfPhysicalJoints(size_t &nrOfPhysicalJoints) override final
void initialise(yarp::sig::VectorOf< size_t > coupled_physical_joints, yarp::sig::VectorOf< size_t > coupled_actuated_axes, std::vector< std::string > physical_joint_names, std::vector< std::string > actuated_axes_names, std::vector< std::pair< double, double > > coupled_physical_joint_limits)
A mini-server for performing network communication in the background.
A base class for nested structures that can be searched.
Definition Searchable.h:31
size_t size() const
Definition Vector.h:351
#define yCError(component,...)
#define YARP_LOG_COMPONENT(name,...)
For streams capable of holding different kinds of content, check what they actually have.
The components from which ports and connections are built.
An interface to the operating system, including Port based communication.
Definition OSTestMsg.cpp:13