YARP
Yet Another Robot Platform
ImplementImpedanceControl.cpp
Go to the documentation of this file.
1/*
2 * SPDX-FileCopyrightText: 2006-2021 Istituto Italiano di Tecnologia (IIT)
3 * SPDX-FileCopyrightText: 2006-2010 RobotCub Consortium
4 * SPDX-License-Identifier: BSD-3-Clause
5 */
6
7#include <cstdio>
8
11#include <yarp/os/LogStream.h>
12#include <cmath>
13
14using namespace yarp::dev;
15#define JOINTIDCHECK if (j >= castToMapper(helper)->axes()){yError("joint id out of bound"); return false;}
16
18ImplementImpedanceControl::ImplementImpedanceControl(IImpedanceControlRaw *r)
19{
21 helper=nullptr;
22}
23
24bool ImplementImpedanceControl::initialize(int size, const int *amap, const double *enc, const double *zos, const double *nw)
25{
26 if (helper != nullptr) {
27 return false;
28 }
29
30 helper=(void *)(new ControlBoardHelper(size, amap, enc, zos, nw));
31 yAssert (helper != nullptr);
32
33 return true;
34}
35
37{
39}
40
42{
43 if (helper!=nullptr)
44 {
45 delete castToMapper(helper);
46 helper=nullptr;
47 }
48
49 return true;
50}
51
53{
54 return iImpedanceRaw->getAxes(axes);
55}
56
57bool ImplementImpedanceControl::setImpedance(int j, double stiffness, double damping)
58{
60 int k;
61 double stiff;
62 double damp;
63 castToMapper(helper)->impN2S(stiffness,j,stiff,k);
64 castToMapper(helper)->impN2S(damping,j,damp,k);
65 return iImpedanceRaw->setImpedanceRaw(k, stiff, damp);
66}
67
68bool ImplementImpedanceControl::getImpedance(int j, double *stiffness, double *damping)
69{
71 int k;
73 bool ret=iImpedanceRaw->getImpedanceRaw(k, stiffness, damping);
74 *stiffness = (castToMapper(helper)->impS2N(*stiffness, k));
75 *damping = (castToMapper(helper)->impS2N(*damping, k));
76 //prevent negative stiffness
77 *stiffness = fabs (*stiffness);
78 *damping = fabs (*damping);
79 return ret;
80}
81
83{
85 int k;
86 double off;
87 castToMapper(helper)->trqN2S(offset,j,off,k);
89}
90
92{
94 int k;
96 bool ret = iImpedanceRaw->getImpedanceOffsetRaw(k, offset);
97 *offset = (castToMapper(helper)->trqS2N(*offset,k));
98 return ret;
99}
100
101bool ImplementImpedanceControl::getCurrentImpedanceLimit(int j, double *min_stiff, double *max_stiff, double *min_damp, double *max_damp)
102{
104 int k;
105 k=castToMapper(helper)->toHw(j);
106 return iImpedanceRaw->getCurrentImpedanceLimitRaw(k, min_stiff, max_stiff, min_damp, max_damp);
107}
yarp::dev::ControlBoardHelper * castToMapper(void *p)
bool ret
#define JOINTIDCHECK
#define yAssert(x)
Definition: Log.h:383
void trqN2S(double newtons, int j, double &sens, int &k)
void impN2S(double newtons, int j, double &sens, int &k)
void trqS2N(const double *sens, double *newtons)
void impS2N(const double *sens, double *newtons)
Interface for control boards implementing impedance control.
virtual bool getAxes(int *ax)=0
Get the number of controlled axes.
virtual bool getImpedanceOffsetRaw(int j, double *offset)=0
Get current force Offset for a specific joint.
virtual bool setImpedanceOffsetRaw(int j, double offset)=0
Set current force Offset for a specific joint.
virtual bool getImpedanceRaw(int j, double *stiffness, double *damping)=0
Get current impedance parameters (stiffness,damping,offset) for a specific joint.
virtual bool getCurrentImpedanceLimitRaw(int j, double *min_stiff, double *max_stiff, double *min_damp, double *max_damp)=0
Get the current impedandance limits for a specific joint.
virtual bool setImpedanceRaw(int j, double stiffness, double damping)=0
Set current impedance parameters (stiffness,damping) for a specific joint.
bool uninitialize()
Clean up internal data and memory.
bool getImpedance(int j, double *stiffness, double *damping) override
Get current impedance gains (stiffness,damping,offset) for a specific joint.
bool getImpedanceOffset(int j, double *offset) override
Get current force Offset for a specific joint.
bool initialize(int size, const int *amap, const double *enc, const double *zos, const double *nw)
Initialize the internal data and alloc memory.
yarp::dev::IImpedanceControlRaw * iImpedanceRaw
bool getCurrentImpedanceLimit(int j, double *min_stiff, double *max_stiff, double *min_damp, double *max_damp) override
Get the current impedandance limits for a specific joint.
bool getAxes(int *ax) override
Get the number of controlled axes.
bool setImpedanceOffset(int j, double offset) override
Set current force Offset for a specific joint.
bool setImpedance(int j, double stiffness, double damping) override
Set current impedance gains (stiffness,damping) for a specific joint.
For streams capable of holding different kinds of content, check what they actually have.