YARP
Yet Another Robot Platform
 
Loading...
Searching...
No Matches
ImplementTorqueControl.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
10
11#include <cstdio>
12using namespace yarp::dev;
13using namespace yarp::os;
14
15namespace {
16YARP_LOG_COMPONENT(IMPLEMENT_LAYER_COMPONENT, "yarp.ImplementTorqueControl")
17}
18
23
28
29bool ImplementTorqueControl::initialize(int size, const int *amap, const double *enc, const double *zos, const double *nw, const double* amps, const double* dutys, const double* bemfs, const double* ktaus)
30{
31 if (m_helper != nullptr) {
32 return false;
33 }
34
35 m_helper=(void *)(new ControlBoardHelper(size, amap, enc, zos, nw, amps, nullptr, dutys,bemfs,ktaus));
36 yAssert (m_helper != nullptr);
37
38 m_buffer_doubles.resize(size);
39 m_buffer_doubles2.resize(size);
40 m_buffer_ints.resize(size);
41
42 return true;
43}
44
46{
47 if (m_helper!=nullptr)
48 {
49 delete castToMapper(m_helper);
50 m_helper=nullptr;
51 }
52
53 return true;
54}
55
57{
58 std::lock_guard lock(m_imp_mutex);
59
60 return m_iraw->getAxes(axes);
61}
62
64{
65 std::lock_guard lock(m_imp_mutex);
68
69 int k;
70 ReturnValue ret;
71 double torque;
73 ret = m_iraw->getRefTorqueRaw(k, &torque);
74 *r=castToMapper(m_helper)->trqS2N(torque, k);
75 return ret;
76}
77
99
101{
102 std::lock_guard lock(m_imp_mutex);
105
106 int k=castToMapper(m_helper)->toHw(j);
107
110 int tmp_j;
111
112 if (b)
113 {
114 *params = params_raw;
115 castToMapper(m_helper)->bemf_raw2user(params_raw.bemf, k, (*params).bemf, tmp_j);
116 castToMapper(m_helper)->ktau_raw2user(params_raw.ktau, k, (*params).ktau, tmp_j);
117 (*params).bemf_scale = params_raw.bemf_scale;
118 (*params).ktau_scale = params_raw.ktau_scale;
119 castToMapper(m_helper)->viscousPos_raw2user(params_raw.viscousPos, k, (*params).viscousPos, tmp_j);
120 castToMapper(m_helper)->viscousNeg_raw2user(params_raw.viscousNeg, k, (*params).viscousNeg, tmp_j);
121 castToMapper(m_helper)->coulombPos_raw2user(params_raw.coulombPos, k, (*params).coulombPos, tmp_j);
122 castToMapper(m_helper)->coulombNeg_raw2user(params_raw.coulombNeg, k, (*params).coulombNeg, tmp_j);
123 castToMapper(m_helper)->velocityThres_raw2user(params_raw.velocityThres, k, (*params).velocityThres, tmp_j);
124 }
125 return b;
126}
127
129{
130 std::lock_guard lock(m_imp_mutex);
132
135
136 return ret;
137}
138
140{
141 std::lock_guard lock(m_imp_mutex);
143
146
147 return ret;
148}
149
151{
152 std::lock_guard lock(m_imp_mutex);
154
155 int k;
156 double sens;
158 return m_iraw->setRefTorqueRaw(k, sens);
159}
160
162{
163 std::lock_guard lock(m_imp_mutex);
165
168
169 return ret;
170}
171
172ReturnValue ImplementTorqueControl::setRefTorques(const int n_joints, const int *joints, const double *t)
173{
174 std::lock_guard lock(m_imp_mutex);
178
179 for(int idx=0; idx<n_joints; idx++)
180 {
183 }
184 ReturnValue ret = m_iraw->setRefTorquesRaw(n_joints, m_buffer_ints.data(), m_buffer_doubles.data());
185
186 return ret;
187}
188
190{
191 std::lock_guard lock(m_imp_mutex);
194
195 int k;
197 return m_iraw->getTorqueRaw(k, t);
198}
199
212
214{
215 std::lock_guard lock(m_imp_mutex);
219
220 int k;
222 return m_iraw->getTorqueRangeRaw(k, min, max);
223}
yarp::dev::ControlBoardHelper * castToMapper(void *p)
#define yAssert(x)
Definition Log.h:388
void viscousNeg_raw2user(double viscousNeg_raw, int k_raw, double &viscousNeg_user, int &j_user)
double viscousNeg_user2raw(double viscousNeg_user, int j)
void ktau_raw2user(double ktau_raw, int k_raw, double &ktau_user, int &j_user)
void trqN2S(double newtons, int j, double &sens, int &k)
void bemf_raw2user(double bemf_raw, int k_raw, double &bemf_user, int &j_user)
void coulombPos_raw2user(double coulombPos_raw, int k_raw, double &coulombPos_user, int &j_user)
double ktau_user2raw(double ktau_user, int j)
double coulombPos_user2raw(double coulombPos_user, int j)
double coulombNeg_user2raw(double coulombNeg_user, int j)
double velocityThres_user2raw(double velocityThres_user, int j)
void velocityThres_raw2user(double threshold_raw, int k_raw, double &velocityThres_user, int &j_user)
double viscousPos_user2raw(double viscousPos_user, int j)
void coulombNeg_raw2user(double coulombNeg_raw, int k_raw, double &coulombNeg_user, int &j_user)
void trqS2N(const double *sens, double *newtons)
double bemf_user2raw(double bemf_user, int j)
void viscousPos_raw2user(double viscousPos_raw, int k_raw, double &viscousPos_user, int &j_user)
Interface for control boards implementing torque control.
virtual yarp::dev::ReturnValue getTorqueRangesRaw(double *min, double *max)=0
Get the full scale of the torque sensors of all joints.
virtual yarp::dev::ReturnValue getTorqueRangeRaw(int j, double *min, double *max)=0
Get the full scale of the torque sensor of a given joint.
virtual yarp::dev::ReturnValue setMotorTorqueParamsRaw(int j, const yarp::dev::MotorTorqueParameters params)=0
Set the motor parameters.
virtual yarp::dev::ReturnValue getRefTorqueRaw(int j, double *t)=0
Set the reference value of the torque for a given joint.
virtual yarp::dev::ReturnValue getMotorTorqueParamsRaw(int j, yarp::dev::MotorTorqueParameters *params)=0
Get the motor parameters.
virtual yarp::dev::ReturnValue getAxes(size_t &ax)=0
Get the number of controlled axes.
virtual yarp::dev::ReturnValue getTorqueRaw(int j, double *t)=0
Get the value of the torque on a given joint (this is the feedback if you have a torque sensor).
virtual yarp::dev::ReturnValue getTorquesRaw(double *t)=0
Get the value of the torque for all joints (this is the feedback if you have torque sensors).
virtual yarp::dev::ReturnValue setRefTorquesRaw(const double *t)=0
Set the reference value of the torque for all joints.
virtual yarp::dev::ReturnValue getRefTorquesRaw(double *t)=0
Get the reference value of the torque for all joints.
virtual yarp::dev::ReturnValue setRefTorqueRaw(int j, double t)=0
Set the reference value of the torque for a given joint.
yarp::dev::ReturnValue setRefTorque(int j, double t) override
Set the reference value of the torque for a given joint.
yarp::dev::ReturnValue setRefTorques(const double *t) override
Set the reference value of the torque for all joints.
yarp::dev::ReturnValue getMotorTorqueParams(int j, yarp::dev::MotorTorqueParameters *params) override
Get a subset of motor parameters (bemf, ktau etc) useful for torque control.
yarp::dev::ReturnValue setMotorTorqueParams(int j, const yarp::dev::MotorTorqueParameters params) override
Set a subset of motor parameters (bemf, ktau etc) useful for torque control.
yarp::dev::ReturnValue getTorqueRanges(double *min, double *max) override
Get the full scale of the torque sensors of all joints.
yarp::dev::ReturnValue getRefTorques(double *t) override
Get the reference value of the torque for all joints.
yarp::dev::ReturnValue getTorqueRange(int j, double *min, double *max) override
Get the full scale of the torque sensor of a given joint.
bool initialize(int size, const int *amap, const double *enc, const double *zos, const double *nw, const double *amps, const double *dutys, const double *bemfs, const double *ktaus)
Initialize the internal data and alloc memory.
yarp::dev::ITorqueControlRaw * m_iraw
yarp::dev::ReturnValue getTorque(int j, double *t) override
Get the value of the torque on a given joint (this is the feedback if you have a torque sensor).
ImplementTorqueControl(yarp::dev::ITorqueControlRaw *y)
bool uninitialize()
Clean up internal data and memory.
yarp::dev::ReturnValue getAxes(size_t &ax) override
Get the number of controlled axes.
yarp::dev::ReturnValue getRefTorque(int j, double *) override
Get the reference value of the torque for a given joint.
yarp::dev::ReturnValue getTorques(double *t) override
Get the value of the torque for all joints (this is the feedback if you have torque sensors).
A mini-server for performing network communication in the background.
#define YARP_LOG_COMPONENT(name,...)
For streams capable of holding different kinds of content, check what they actually have.
An interface to the operating system, including Port based communication.
Definition OSTestMsg.cpp:13