YARP
Yet Another Robot Platform
 
Loading...
Searching...
No Matches
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
12#include <yarp/os/LogStream.h>
13#include <cmath>
14
15using namespace yarp::dev;
16
17namespace {
18YARP_LOG_COMPONENT(IMPLEMENT_LAYER_COMPONENT, "yarp.ImplementImpedanceControl")
19}
20
27
28bool ImplementImpedanceControl::initialize(int size, const int *amap, const double *enc, const double *zos, const double *nw)
29{
30 if (m_helper != nullptr) {
31 return false;
32 }
33
34 m_helper=(void *)(new ControlBoardHelper(size, amap, enc, zos, nw));
35 yAssert (m_helper != nullptr);
36
37 m_buffer_ints.resize (size);
38 m_buffer_doubles.resize(size);
39
40 return true;
41}
42
47
49{
50 if (m_helper!=nullptr)
51 {
52 delete castToMapper(m_helper);
53 m_helper=nullptr;
54 }
55
56 return true;
57}
58
60{
61 return m_iraw->getAxes(axes);
62}
63
64ReturnValue ImplementImpedanceControl::setImpedance(int j, double stiffness, double damping)
65{
66 std::lock_guard lock(m_imp_mutex);
67 JOINTIDCHECK(IMPLEMENT_LAYER_COMPONENT,j)
68
69 int k;
70 double stiff;
71 double damp;
72 castToMapper(m_helper)->impN2S(stiffness,j,stiff,k);
73 castToMapper(m_helper)->impN2S(damping,j,damp,k);
74 return m_iraw->setImpedanceRaw(k, stiff, damp);
75}
76
77ReturnValue ImplementImpedanceControl::getImpedance(int j, double *stiffness, double *damping)
78{
79 std::lock_guard lock(m_imp_mutex);
80 JOINTIDCHECK(IMPLEMENT_LAYER_COMPONENT,j)
81 POINTERCHECK(IMPLEMENT_LAYER_COMPONENT,stiffness)
82 POINTERCHECK(IMPLEMENT_LAYER_COMPONENT,damping)
83
84 int k;
86 ReturnValue ret=m_iraw->getImpedanceRaw(k, stiffness, damping);
87 *stiffness = (castToMapper(m_helper)->impS2N(*stiffness, k));
88 *damping = (castToMapper(m_helper)->impS2N(*damping, k));
89 //prevent negative stiffness
90 *stiffness = fabs (*stiffness);
91 *damping = fabs (*damping);
92 return ret;
93}
94
96{
97 std::lock_guard lock(m_imp_mutex);
98 JOINTIDCHECK(IMPLEMENT_LAYER_COMPONENT,j)
99
100 int k;
101 double off;
102 castToMapper(m_helper)->trqN2S(offset,j,off,k);
103 return m_iraw->setImpedanceOffsetRaw(k, off);
104}
105
107{
108 std::lock_guard lock(m_imp_mutex);
109 JOINTIDCHECK(IMPLEMENT_LAYER_COMPONENT,j)
110 POINTERCHECK(IMPLEMENT_LAYER_COMPONENT,offset)
111
112 int k;
114 ReturnValue ret = m_iraw->getImpedanceOffsetRaw(k, offset);
115 *offset = (castToMapper(m_helper)->trqS2N(*offset,k));
116 return ret;
117}
118
119ReturnValue ImplementImpedanceControl::getCurrentImpedanceLimit(int j, double *min_stiff, double *max_stiff, double *min_damp, double *max_damp)
120{
121 std::lock_guard lock(m_imp_mutex);
122 JOINTIDCHECK(IMPLEMENT_LAYER_COMPONENT,j)
123 POINTERCHECK(IMPLEMENT_LAYER_COMPONENT,min_stiff)
124 POINTERCHECK(IMPLEMENT_LAYER_COMPONENT,max_stiff)
125 POINTERCHECK(IMPLEMENT_LAYER_COMPONENT,min_damp)
126 POINTERCHECK(IMPLEMENT_LAYER_COMPONENT,max_damp)
127
128 int k;
130 return m_iraw->getCurrentImpedanceLimitRaw(k, min_stiff, max_stiff, min_damp, max_damp);
131}
yarp::dev::ControlBoardHelper * castToMapper(void *p)
#define yAssert(x)
Definition Log.h:388
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 yarp::dev::ReturnValue getCurrentImpedanceLimitRaw(int j, double *min_stiff, double *max_stiff, double *min_damp, double *max_damp)=0
Get the current impedance limits for a specific joint.
virtual yarp::dev::ReturnValue getImpedanceOffsetRaw(int j, double *offset)=0
Get current force Offset for a specific joint.
virtual yarp::dev::ReturnValue setImpedanceRaw(int j, double stiffness, double damping)=0
Set current impedance parameters (stiffness,damping) for a specific joint.
virtual yarp::dev::ReturnValue getAxes(size_t &ax)=0
Get the number of controlled axes.
virtual yarp::dev::ReturnValue setImpedanceOffsetRaw(int j, double offset)=0
Set current force Offset for a specific joint.
virtual yarp::dev::ReturnValue getImpedanceRaw(int j, double *stiffness, double *damping)=0
Get current impedance parameters (stiffness,damping,offset) for a specific joint.
bool uninitialize()
Clean up internal data and memory.
yarp::dev::ReturnValue getImpedance(int j, double *stiffness, double *damping) override
Get current impedance gains (stiffness,damping,offset) for a specific joint.
yarp::dev::ReturnValue 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.
ImplementImpedanceControl(yarp::dev::IImpedanceControlRaw *y)
yarp::dev::ReturnValue setImpedanceOffset(int j, double offset) override
Set current force Offset for a specific joint.
yarp::dev::ReturnValue getAxes(size_t &ax) override
Get the number of controlled axes.
yarp::dev::IImpedanceControlRaw * m_iraw
yarp::dev::ReturnValue setImpedance(int j, double stiffness, double damping) override
Set current impedance gains (stiffness,damping) for a specific joint.
yarp::dev::ReturnValue getCurrentImpedanceLimit(int j, double *min_stiff, double *max_stiff, double *min_damp, double *max_damp) override
Get the current impedance limits for a specific joint.
#define YARP_LOG_COMPONENT(name,...)
For streams capable of holding different kinds of content, check what they actually have.