YARP
Yet Another Robot Platform
 
Loading...
Searching...
No Matches
robotDriver.cpp
Go to the documentation of this file.
1/*
2 * SPDX-FileCopyrightText: 2024 Istituto Italiano di Tecnologia (IIT)
3 * SPDX-License-Identifier: BSD-3-Clause
4 */
5
6#include <cmath>
7#include <iomanip>
8
9#include <yarp/os/Network.h>
10#include <yarp/os/Time.h>
11#include <yarp/os/Log.h>
12#include <yarp/os/LogStream.h>
14#include <yarp/sig/Vector.h>
15
17#include <yarp/dev/PolyDriver.h>
19#include <yarp/os/Thread.h>
20
21using namespace std;
22using namespace yarp::os;
23using namespace yarp::sig;
24using namespace yarp::dev;
25
26#include "robotDriver.h"
27
28// ******************** ROBOT DRIVER CLASS
32
34{
35 drv_ll = pd;
36 return true;
37}
38
40{
41 for (size_t i=0; i<n_joints; i++)
42 m_trajectoryTime[i] = t;
43 return true;
44}
45
46bool robotDriver::setTrajectoryTime(int j, double t)
47{
48 m_trajectoryTime[j] = t;
49 return true;
50}
51
53{
54 if (!drv_ll)
55 {
56 yError() << "Invalid pointer to Polydriver!";
57 return false;
58 }
59
60 bool ok = true;
61 if (drv_ll->isValid())
62 {
63 ok = drv_ll->view(ipos_ll) && drv_ll->view(iposdir_ll) && drv_ll->view(ienc_ll) &&
64 drv_ll->view(ipid_ll) && drv_ll->view(imotenc_ll) && drv_ll->view(icmd_ll);
65 }
66 else
67 {
68 ok = false;
69 }
70
71 if (!ok)
72 {
73 yError() << "Unable to view some interface!";
74 return false;
75 }
76
77 //get the number of the joints
78 ok &= ienc_ll->getAxes(n_joints);
79
80 //set the trajectory duration for the first movement
81 m_trajectoryTime.resize(n_joints,4.0);
82
83 //set the initial reference speeds to 20
84 for (size_t i = 0; i < n_joints; i++)
85 {
86 m_stored_speed.push_back(20.0);
87 }
88
89 ok &= loadRefVelocities();
90
91 return ok;
92}
93
97
99{
100 if (!icmd_ll) return false;
101 return icmd_ll->getControlMode(j, mode);
102}
103
105{
106 if (!icmd_ll) return false;
107 return icmd_ll->setControlMode(j, mode);
108}
109
110bool robotDriver::setPosition(int j, double ref)
111{
112 if (!iposdir_ll) return false;
113 return iposdir_ll->setPosition(j, ref);
114}
115
116bool robotDriver::getEncoder(int j, double *v)
117{
118 if (!ienc_ll) return false;
119 return ienc_ll->getEncoder(j, v);
120}
121
122bool robotDriver::positionMove(int j, double v)
123{
124 if (!ipos_ll) return false;
125 bool ret = true;
126
127 // set ref speed to each joint so that the motion is completed in
128 // the requested amount of time
129 if (1)
130 {
131 double enc = 0;
132 ret &= ienc_ll->getEncoder(j, &enc);
133 double diff = fabs(v - enc);
134 double speed = diff/m_trajectoryTime[j];
135 ipos_ll->setTrajSpeed(j, speed);
136 }
137
138 ret &= ipos_ll->positionMove(j, v);
139
140 return ret;
141}
142
144{
145 return n_joints;
146}
147
148std::string vectorToString(const std::vector<double>& vec)
149{
150 std::ostringstream oss;
151 oss << std::fixed << std::setprecision(2);
152 oss << "(";
153 for (size_t i = 0; i < vec.size(); ++i) {
154 oss << vec[i];
155 if (i + 1 != vec.size()) oss << " ";
156 }
157 oss << ")";
158 return oss.str();
159}
160
162{
163 bool ok;
164 ok = ipos_ll->getTrajSpeeds(m_stored_speed.data());
165 yDebug() << "Storing speeds" << vectorToString(m_stored_speed);
166 return ok;
167}
168
170{
171 bool ok;
172 yDebug() << "Loading speeds" << vectorToString(m_stored_speed);
173 ok = ipos_ll->setTrajSpeeds(m_stored_speed.data());
174 return ok;
175}
define control board standard interfaces
#define yError(...)
Definition Log.h:361
#define yDebug(...)
Definition Log.h:275
contains the definition of a Vector type
bool setTrajectoryTime(double t)
bool loadRefVelocities()
bool setPosition(int j, double ref)
bool configure(yarp::dev::PolyDriver *pd)
bool setControlMode(const int j, const yarp::dev::SelectableControlModeEnum mode)
size_t getNJoints()
bool getEncoder(int j, double *v)
bool storeRefVelocities()
bool getControlMode(const int j, yarp::dev::ControlModeEnum &mode)
bool positionMove(int j, double ref)
bool view(T *&x)
Get an interface to the device driver.
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.
virtual yarp::dev::ReturnValue getEncoder(int j, double *v)=0
Read the value of an encoder.
virtual yarp::dev::ReturnValue getAxes(size_t &ax)=0
Get the number of controlled axes.
virtual yarp::dev::ReturnValue setTrajSpeed(int j, double sp)=0
Set reference speed for a joint, this is the speed used during the interpolation of the trajectory.
virtual yarp::dev::ReturnValue getTrajSpeeds(double *spds)=0
Get reference speed of all joints.
virtual yarp::dev::ReturnValue setTrajSpeeds(const double *spds)=0
Set reference speed on all joints.
virtual yarp::dev::ReturnValue positionMove(int j, double ref)=0
Set new reference point for a single axis.
virtual yarp::dev::ReturnValue setPosition(int j, double ref)=0
Set new position for a single axis.
A container for a device driver.
Definition PolyDriver.h:23
bool isValid() const
Check if device is valid.
A mini-server for performing network communication in the background.
STL namespace.
For streams capable of holding different kinds of content, check what they actually have.
SelectableControlModeEnum
An interface to the operating system, including Port based communication.
Definition OSTestMsg.cpp:13
std::string vectorToString(const std::vector< double > &vec)
int diff(std::string contextName, folderType fType, bool verbose)