Skip to content

Commit 8d67473

Browse files
NicogeneNicogene
committed
Second draft of refactor
1 parent 39bf7e3 commit 8d67473

File tree

3 files changed

+33
-258
lines changed

3 files changed

+33
-258
lines changed

‎src/modules/handMk5CouplingHandler/CMakeLists.txt

+1-1
Original file line numberDiff line numberDiff line change
@@ -4,7 +4,7 @@
44
# This software may be modified and distributed under the terms of the
55
# BSD-3-Clause license. See the accompanying LICENSE file for details.
66

7-
find_package(YARP 3.7.2 REQUIRED)
7+
find_package(YARP 3.9.0 REQUIRED)
88

99
yarp_configure_plugins_installation(ergocub-software)
1010

‎src/modules/handMk5CouplingHandler/HandMk5CouplingHandler.cpp

+10-214
Original file line numberDiff line numberDiff line change
@@ -56,6 +56,8 @@ bool HandMk5CouplingHandler::parseFingerParameters(yarp::os::Bottle& hand_params
5656

5757
bool HandMk5CouplingHandler::open(yarp::os::Searchable& config) {
5858
yarp::os::Bottle& coupling_group_bottle = config.findGroup("COUPLING_PARAMS");
59+
60+
// TODO INVOKE ImplementCoupling::initialise()
5961
if(coupling_group_bottle.isNull())
6062
{
6163
yError()<<"HandMk5CouplingHandler: missing coupling parameters, check your configuration file";
@@ -65,217 +67,11 @@ bool HandMk5CouplingHandler::open(yarp::os::Searchable& config) {
6567
return parseFingerParameters(coupling_group_bottle);
6668
}
6769

68-
bool HandMk5CouplingHandler::decouplePos (yarp::sig::Vector& current_pos)
69-
{
70-
if (m_coupledJoints.size()!=m_couplingSize) return false;
71-
72-
const yarp::sig::Vector tmp = current_pos;
73-
74-
/* thumb_add <-- thumb_add */
75-
current_pos[m_coupledJoints[0]] = tmp[0];
76-
/* thumb_oc <-- thumb_prox */
77-
current_pos[m_coupledJoints[1]] = tmp[1];
78-
/* index_add <-- index_add */
79-
current_pos[m_coupledJoints[2]] = tmp[3];
80-
/* index_oc <-- index_prox */
81-
current_pos[m_coupledJoints[3]] = tmp[4];
82-
/* middle_oc <-- middle_prox */
83-
current_pos[m_coupledJoints[4]] = tmp[6];
84-
/**
85-
* ring_pinky_oc <-- pinkie_prox
86-
* as, on the real robot, the coupled group composed of ring_prox, ring_dist, pinkie_prox and pinkie_dist
87-
* is controlled using the encoder on the pinkie_prox as feedback
88-
*/
89-
current_pos[m_coupledJoints[5]] = tmp[10];
90-
91-
return true;
92-
}
93-
94-
bool HandMk5CouplingHandler::decoupleVel (yarp::sig::Vector& current_vel)
95-
{
96-
if (m_coupledJoints.size()!=m_couplingSize) return false;
97-
98-
const yarp::sig::Vector tmp = current_vel;
99-
100-
/* thumb_add <-- thumb_add */
101-
current_vel[m_coupledJoints[0]] = tmp[0];
102-
/* thumb_oc <-- thumb_prox */
103-
current_vel[m_coupledJoints[1]] = tmp[1];
104-
/* index_add <-- index_add */
105-
current_vel[m_coupledJoints[2]] = tmp[3];
106-
/* index_oc <-- index_prox */
107-
current_vel[m_coupledJoints[3]] = tmp[4];
108-
/* middle_oc <-- middle_prox */
109-
current_vel[m_coupledJoints[4]] = tmp[6];
110-
/**
111-
* ring_pinky_oc <-- pinkie_prox
112-
* as, on the real robot, the coupled group composed of ring_prox, ring_dist, pinkie_prox and pinkie_dist
113-
* is controlled using the encoder on the pinkie_prox as feedback
114-
*/
115-
current_vel[m_coupledJoints[5]] = tmp[10];
116-
117-
return true;
118-
}
119-
120-
bool HandMk5CouplingHandler::decoupleAcc (yarp::sig::Vector& current_acc)
121-
{
122-
/**
123-
* Acceleration control not available for fingers on the real robot.
124-
* Note: this method is never called within the controlboard plugin code.
125-
*/
126-
return false;
127-
}
128-
129-
bool HandMk5CouplingHandler::decoupleTrq (yarp::sig::Vector& current_trq)
130-
{
131-
/**
132-
* Torque control not available for fingers on the real robot.
133-
*/
134-
return false;
135-
}
136-
137-
yarp::sig::Vector HandMk5CouplingHandler::decoupleRefPos (yarp::sig::Vector& pos_ref)
138-
{
139-
if (m_coupledJoints.size()!=m_couplingSize) {yCError( HANDMK5COUPLINGHANDLER) << "HandMk5CouplingHandler: Invalid coupling vector"; return pos_ref;}
140-
141-
yarp::sig::Vector out(pos_ref.size());
142-
143-
/* thumb_add <-- thumb_add */
144-
out[0] = pos_ref[m_coupledJoints[0]];
145-
/* thumb_prox <-- thumb_oc */
146-
out[1] = pos_ref[m_coupledJoints[1]];
147-
/* thumb_dist <-- coupling_law(thumb_prox) */
148-
out[2] = evaluateCoupledJoint(out[1], "thumb");
149-
/* index_add <-- index_add */
150-
out[3] = pos_ref[m_coupledJoints[2]];
151-
/* index_prox <-- index_oc */
152-
out[4] = pos_ref[m_coupledJoints[3]];
153-
/* index_dist <-- coupling_law(index_prox) */
154-
out[5] = evaluateCoupledJoint(out[4], "index");
155-
/* middle_prox <-- middle_oc */
156-
out[6] = pos_ref[m_coupledJoints[4]];
157-
/* middle_dist <-- coupling_law(middle_prox) */
158-
out[7] = evaluateCoupledJoint(out[6], "middle");
159-
/* ring_prox <-- ring_pinky_oc */
160-
out[8] = pos_ref[m_coupledJoints[5]];
161-
/* ring_dist <-- coupling_law(ring_prox) */
162-
out[9] = evaluateCoupledJoint(out[8], "ring");
163-
/* pinky_prox <-- ring_pinky_oc */
164-
out[10] = pos_ref[m_coupledJoints[5]];
165-
/* pinky_dist <-- coupling_law(pinky_prox) */
166-
out[11] = evaluateCoupledJoint(out[10], "pinky");
167-
168-
return out;
169-
}
170-
171-
172-
yarp::sig::Vector HandMk5CouplingHandler::decoupleRefVel (yarp::sig::Vector& vel_ref, const yarp::sig::Vector& pos_feedback)
173-
{
174-
if (m_coupledJoints.size()!=m_couplingSize) {yCError( HANDMK5COUPLINGHANDLER) << "HandMk5CouplingHandler: Invalid coupling vector"; return vel_ref;}
175-
176-
/**
177-
* Extract the current position of proximal joints from pos_feedback.
178-
*/
179-
double lastThumbProx = pos_feedback[1];
180-
double lastIndexProx = pos_feedback[4];
181-
double lastMiddleProx = pos_feedback[6];
182-
double lastRingProx = pos_feedback[8];
183-
double lastPinkyProx = pos_feedback[10];
184-
185-
/**
186-
* In the following, we use the fact that:
187-
* /dot{distal_joint} = \partial{distal_joint}{proximal_joint} \dot{proximal_joint}.
188-
*/
189-
190-
yarp::sig::Vector out(vel_ref.size());
191-
192-
/* thumb_add <-- thumb_add */
193-
out[0] = vel_ref[m_coupledJoints[0]];
194-
/* thumb_prox <-- thumb_oc */
195-
out[1] = vel_ref[m_coupledJoints[1]];
196-
/* thumb_dist <-- coupling_law_jacobian(thumb_prox_position) * thumb_prox */
197-
out[2] = evaluateCoupledJointJacobian(lastThumbProx, "thumb") * out[1];
198-
/* index_add <-- index_add */
199-
out[3] = vel_ref[m_coupledJoints[2]];
200-
/* index_prox <-- index_oc */
201-
out[4] = vel_ref[m_coupledJoints[3]];
202-
/* index_dist <-- coupling_law_jacobian(index_prox_position) * index_prox */
203-
out[5] = evaluateCoupledJointJacobian(lastIndexProx, "index") * out[4];
204-
/* middle_prox <-- middle_oc */
205-
out[6] = vel_ref[m_coupledJoints[4]];
206-
/* middle_dist <-- coupling_law_jacobian(middle_prox_position) * middle_prox */
207-
out[7] = evaluateCoupledJointJacobian(lastMiddleProx, "middle") * out[6];
208-
/* ring_prox <-- ring_pinky_oc */
209-
out[8] = vel_ref[m_coupledJoints[5]];
210-
/* ring_dist <-- coupling_law_jacobian(ring_prox_position) * ring_prox */
211-
out[9] = evaluateCoupledJointJacobian(lastRingProx, "ring") * out[8];
212-
/* pinky_prox <-- ring_pinky_oc */
213-
out[10] = vel_ref[m_coupledJoints[5]];
214-
/* pinky_dist <-- coupling_law(pinky_prox) */
215-
out[11] = evaluateCoupledJointJacobian(lastPinkyProx, "pinky") * out[10];
216-
217-
return out;
218-
}
219-
220-
yarp::sig::Vector HandMk5CouplingHandler::decoupleRefTrq (yarp::sig::Vector& trq_ref)
221-
{
222-
/**
223-
* Torque control not available for fingers on the real robot.
224-
*/
225-
return trq_ref;
226-
}
227-
228-
double HandMk5CouplingHandler::evaluateCoupledJoint(const double& q1, const std::string& finger_name)
229-
{
230-
/**
231-
* Coupling law taken from from https://icub-tech-iit.github.io/documentation/hands/hands_mk5_coupling
232-
*/
233-
auto params = mFingerParameters.at(finger_name);
234-
double q1_rad = q1 * M_PI / 180.0;
235-
double q1off_rad = params.q1off * M_PI / 180.0;
236-
double q2bias_rad = params.q2bias * M_PI / 180.0;
237-
238-
double P1x_q1 = params.d * cos(q1_rad + q1off_rad);
239-
double P1y_q1 = params.d * sin(q1_rad + q1off_rad);
240-
241-
double h_sq = std::pow(P1x_q1 - params.L0x, 2) + std::pow(P1y_q1 - params.L0y, 2);
242-
double h = std::sqrt(h_sq);
243-
double l_sq = std::pow(params.l, 2);
244-
double k_sq = std::pow(params.k, 2);
245-
246-
double q2 = atan2(P1y_q1 - params.L0y, P1x_q1 - params.L0x) + \
247-
acos((h_sq + l_sq - k_sq) / (2.0 * params.l * h)) + \
248-
-q2bias_rad - M_PI;
249-
250-
// The value of q1 is subtracted from the result as the formula provides
251-
// the absolute angle of the coupled distal joint with respect to the palm.
252-
return q2 * 180.0 / M_PI - q1;
253-
}
254-
255-
double HandMk5CouplingHandler::evaluateCoupledJointJacobian(const double& q1, const std::string& finger_name)
256-
{
257-
/**
258-
* Coupling law jacobian taken from from https://icub-tech-iit.github.io/documentation/hands/hands_mk5_coupling
259-
*/
260-
auto params = mFingerParameters.at(finger_name);
261-
double q1_rad = q1 * M_PI / 180.0;
262-
double q1off_rad = params.q1off * M_PI / 180.0;
263-
264-
double P1x_q1 = params.d * cos(q1_rad + q1off_rad);
265-
double P1y_q1 = params.d * sin(q1_rad + q1off_rad);
266-
267-
double h_sq = std::pow(P1x_q1 - params.L0x, 2) + std::pow(P1y_q1 - params.L0y, 2);
268-
double h = std::sqrt(h_sq);
269-
double l_sq = std::pow(params.l, 2);
270-
double k_sq = std::pow(params.k, 2);
271-
272-
double dq2_dq1_11 = 1;
273-
double dq2_dq1_21 = 2 - (std::pow(params.d, 2) - std::pow(params.b, 2)) / (std::pow(params.d, 2) - (params.L0x * P1x_q1 + params.L0y * P1y_q1));
274-
double dq2_dq1_12 = (params.L0x * P1y_q1 - params.L0y * P1x_q1) * (l_sq - k_sq - h_sq);
275-
double dq2_dq1_22 = 2 * params.l * h * h_sq * std::sqrt(1 - std::pow((l_sq - k_sq + h_sq) / (2 * params.l * h), 2));
276-
double dq2_dq1 = dq2_dq1_11 / dq2_dq1_21 + dq2_dq1_12 / dq2_dq1_22;
277-
278-
// The value of 1 is subtracted from the result as evaluateCoupledJointJacobian provides
279-
// the jacobian of the absolute angle of the coupled distal joint.
280-
return dq2_dq1 - 1;
281-
}
70+
bool HandMk5CouplingHandler::convertFromPhysicalJointsToActuatedAxesPos(const yarp::sig::Vector& physJointsPos, yarp::sig::Vector& actAxesPos) { return false; }
71+
bool HandMk5CouplingHandler::convertFromPhysicalJointsToActuatedAxesVel(const yarp::sig::Vector& physJointsPos, const yarp::sig::Vector& physJointsVel, yarp::sig::Vector& actAxesVel) { return false; }
72+
bool HandMk5CouplingHandler::convertFromPhysicalJointsToActuatedAxesAcc(const yarp::sig::Vector& physJointsPos, const yarp::sig::Vector& physJointsVel, const yarp::sig::Vector& physJointsAcc, yarp::sig::Vector& actAxesAcc) { return false; }
73+
bool HandMk5CouplingHandler::convertFromPhysicalJointsToActuatedAxesTrq(const yarp::sig::Vector& physJointsPos, const yarp::sig::Vector& physJointsTrq, yarp::sig::Vector& actAxesTrq) { return false; }
74+
bool HandMk5CouplingHandler::convertFromActuatedAxesToPhysicalJointsPos(const yarp::sig::Vector& actAxesPos, yarp::sig::Vector& physJointsPos) { return false; }
75+
bool HandMk5CouplingHandler::convertFromActuatedAxesToPhysicalJointsVel(const yarp::sig::Vector& actAxesPos, const yarp::sig::Vector& actAxesVel, yarp::sig::Vector& physJointsVel) { return false; }
76+
bool HandMk5CouplingHandler::convertFromActuatedAxesToPhysicalJointsAcc(const yarp::sig::Vector& actAxesPos, const yarp::sig::Vector& actAxesVel, const yarp::sig::Vector& actAxesAcc, yarp::sig::Vector& physJointsAcc) { return false; }
77+
bool HandMk5CouplingHandler::convertFromActuatedAxesToPhysicalJointsTrq(const yarp::sig::Vector& actAxesPos, const yarp::sig::Vector& actAxesTrq, yarp::sig::Vector& physJointsTrq) { return false; }

‎src/modules/handMk5CouplingHandler/HandMk5CouplingHandler.h

+22-43
Original file line numberDiff line numberDiff line change
@@ -10,7 +10,7 @@
1010
#define HANDMK5COUPLINGHANDLER_H
1111

1212
#include <yarp/dev/DeviceDriver.h>
13-
#include <yarp/dev/IJointCoupling.h>
13+
#include <yarp/dev/ImplementJointCoupling.h>
1414

1515
#include <unordered_map>
1616
#include <vector>
@@ -19,7 +19,27 @@
1919
/** TBD
2020
*/
2121
class HandMk5CouplingHandler : public yarp::dev::DeviceDriver,
22-
public yarp::dev::IJointCoupling {
22+
public yarp::dev::ImplementJointCoupling {
23+
public:
24+
HandMk5CouplingHandler();
25+
~HandMk5CouplingHandler() override;
26+
bool convertFromPhysicalJointsToActuatedAxesPos(const yarp::sig::Vector& physJointsPos, yarp::sig::Vector& actAxesPos) override;
27+
bool convertFromPhysicalJointsToActuatedAxesVel(const yarp::sig::Vector& physJointsPos, const yarp::sig::Vector& physJointsVel, yarp::sig::Vector& actAxesVel) override;
28+
bool convertFromPhysicalJointsToActuatedAxesAcc(const yarp::sig::Vector& physJointsPos, const yarp::sig::Vector& physJointsVel, const yarp::sig::Vector& physJointsAcc, yarp::sig::Vector& actAxesAcc) override;
29+
bool convertFromPhysicalJointsToActuatedAxesTrq(const yarp::sig::Vector& physJointsPos, const yarp::sig::Vector& physJointsTrq, yarp::sig::Vector& actAxesTrq) override;
30+
bool convertFromActuatedAxesToPhysicalJointsPos(const yarp::sig::Vector& actAxesPos, yarp::sig::Vector& physJointsPos) override;
31+
bool convertFromActuatedAxesToPhysicalJointsVel(const yarp::sig::Vector& actAxesPos, const yarp::sig::Vector& actAxesVel, yarp::sig::Vector& physJointsVel) override;
32+
bool convertFromActuatedAxesToPhysicalJointsAcc(const yarp::sig::Vector& actAxesPos, const yarp::sig::Vector& actAxesVel, const yarp::sig::Vector& actAxesAcc, yarp::sig::Vector& physJointsAcc) override;
33+
bool convertFromActuatedAxesToPhysicalJointsTrq(const yarp::sig::Vector& actAxesPos, const yarp::sig::Vector& actAxesTrq, yarp::sig::Vector& physJointsTrq) override;
34+
35+
//DeviceDriver
36+
bool close() override;
37+
/**
38+
* Configure with a set of options.
39+
* @param config The options to use
40+
* @return true iff the object could be configured.
41+
*/
42+
bool open(yarp::os::Searchable& config) override;
2343
private:
2444

2545
/**
@@ -39,17 +59,6 @@ class HandMk5CouplingHandler : public yarp::dev::DeviceDriver,
3959

4060
std::unordered_map<std::string, FingerParameters> mFingerParameters;
4161

42-
struct Range {
43-
Range() : min(0), max(0){}
44-
double min;
45-
double max;
46-
};
47-
// These could go in a Basic class
48-
yarp::sig::VectorOf<int> m_coupledJoints;
49-
std::vector<std::string> m_coupledJointNames;
50-
std::unordered_map<int, Range> m_coupledJointLimits;
51-
unsigned int m_controllerPeriod;
52-
unsigned int m_couplingSize;
5362

5463
/*
5564
* This method implements the law q2 = q2(q1) from
@@ -71,36 +80,6 @@ class HandMk5CouplingHandler : public yarp::dev::DeviceDriver,
7180
double evaluateCoupledJointJacobian(const double& q1, const std::string& finger_name);
7281

7382
bool parseFingerParameters(yarp::os::Bottle& hand_params);
74-
public:
75-
HandMk5CouplingHandler();
76-
~HandMk5CouplingHandler() override;
77-
78-
79-
virtual bool decouplePos(yarp::sig::Vector& current_pos);
80-
virtual bool decoupleVel(yarp::sig::Vector& current_vel);
81-
virtual bool decoupleAcc(yarp::sig::Vector& current_acc);
82-
virtual bool decoupleTrq(yarp::sig::Vector& current_trq) override;
83-
virtual yarp::sig::Vector decoupleRefPos(yarp::sig::Vector& pos_ref) override;
84-
virtual yarp::sig::Vector decoupleRefVel(yarp::sig::Vector& vel_ref, const yarp::sig::Vector& pos_feedback) override;
85-
virtual yarp::sig::Vector decoupleRefTrq(yarp::sig::Vector& trq_ref) override;
86-
87-
// These could go in a Basic class
88-
virtual yarp::sig::VectorOf<int> getCoupledJoints() override { return m_coupledJoints; };
89-
virtual std::string getCoupledJointName(int joint) override { return m_coupledJointNames[joint]; };
90-
virtual bool checkJointIsCoupled(int joint) override { return m_coupledJoints[joint] != -1; };
91-
92-
93-
virtual void setCoupledJointLimit(int joint, const double& min, const double& max) override { m_coupledJointLimits[joint].min = min; m_coupledJointLimits[joint].max = max; };
94-
virtual void getCoupledJointLimit(int joint, double& min, double& max) override { min = m_coupledJointLimits[joint].min; max = m_coupledJointLimits[joint].max; };
95-
96-
//DeviceDriver
97-
bool close() override;
98-
/**
99-
* Configure with a set of options.
100-
* @param config The options to use
101-
* @return true iff the object could be configured.
102-
*/
103-
bool open(yarp::os::Searchable& config) override;
10483
};
10584

10685
#endif // HANDMK5COUPLINGHANDLER_H

0 commit comments

Comments
 (0)