Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

MotorEconders-consistency - retrieve the coupling matrix at runtime #60

Merged
merged 12 commits into from
Feb 21, 2022
Merged

MotorEconders-consistency - retrieve the coupling matrix at runtime #60

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
12 commits
Select commit Hold shift + click to select a range
44eced0
fixing opt enc gearratio
davidetome Feb 9, 2022
5b878fe
wip
davidetome Feb 9, 2022
34806f7
formatting one matrix from n matricies retrieved at runtime
davidetome Feb 15, 2022
711a08e
wip
davidetome Feb 16, 2022
a80e798
matrix correctly reconstructed - to be tested on the robot
davidetome Feb 17, 2022
7189c5a
matrix correctly reconstructed - to be tested on the robot
davidetome Feb 17, 2022
01d591a
coupling matrix are now retrieved at rurntime for MotorEconders-consi…
davidetome Feb 18, 2022
546c1d0
removed ubused variablw
davidetome Feb 18, 2022
6c08bd4
fixed typo
davidetome Feb 18, 2022
96c4df1
restored default encoders-icub.xml
davidetome Feb 18, 2022
6d4f86c
restored left leg conf file for iCub_SIM
davidetome Feb 18, 2022
39ac97a
motorEncoder-consistenct - removed matrices from .ini files
davidetome Feb 21, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
96 changes: 80 additions & 16 deletions src/motorEncoders-consistency/motorEncodersConsistency.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -31,6 +31,7 @@
#include <fstream>
#include "motorEncodersConsistency.h"
#include <iostream>
#include <yarp/dev/IRemoteVariables.h>

using namespace std;

Expand Down Expand Up @@ -89,7 +90,7 @@ bool OpticalEncodersConsistency::setup(yarp::os::Property& property) {
ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(property.check("speed"), "The positionMove reference speed must be given as the test parameter!");
ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(property.check("tolerance"), "The tolerance of the control signal must be given as the test parameter!");
ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(property.check("matrix_size"), "The matrix size must be given!");
ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(property.check("matrix"), "The coupling matrix must be given!");
// ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(property.check("matrix"), "The coupling matrix must be given!");
robotName = property.find("robot").asString();
partName = property.find("part").asString();
if(property.check("plot_enabled"))
Expand Down Expand Up @@ -124,20 +125,24 @@ bool OpticalEncodersConsistency::setup(yarp::os::Property& property) {
{
matrix.resize(matrix_size,matrix_size);
matrix.eye();
Bottle* matrixBottle = property.find("matrix").asList();
if (matrixBottle!= NULL && matrixBottle->size() == (matrix_size*matrix_size) )
{
for (int i=0; i< (matrix_size*matrix_size); i++)
{
matrix.data()[i]=matrixBottle->get(i).asFloat64();
}
}
else
{
char buff [500];
sprintf (buff, "invalid number of elements of parameter matrix %d!=%d", matrixBottle->size() , (matrix_size*matrix_size));
ROBOTTESTINGFRAMEWORK_ASSERT_ERROR(buff);
}

// The couplig matrix is retrived run-time by the IRemoteVariable interface accessing the jinimatic_mj variable
//
// Bottle* matrixBottle = property.find("matrix").asList();

// if (matrixBottle!= NULL && matrixBottle->size() == (matrix_size*matrix_size) )
// {
// for (int i=0; i< (matrix_size*matrix_size); i++)
// {
// matrix.data()[i]=matrixBottle->get(i).asFloat64();
// }
// }
// else
// {
// char buff [500];
// sprintf (buff, "invalid number of elements of parameter matrix %d!=%d", matrixBottle->size() , (matrix_size*matrix_size));
// ROBOTTESTINGFRAMEWORK_ASSERT_ERROR(buff);
// }
}
else
{
Expand All @@ -161,6 +166,8 @@ bool OpticalEncodersConsistency::setup(yarp::os::Property& property) {
ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(dd->view(iimd),"Unable to open interaction mode interface");
ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(dd->view(imotenc),"Unable to open motor encoders interface");
ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(dd->view(imot),"Unable to open motor interface");
ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(dd->view(ivar),"Unable to open remote variables interface");


if (!ienc->getAxes(&n_part_joints))
{
Expand Down Expand Up @@ -202,6 +209,7 @@ bool OpticalEncodersConsistency::setup(yarp::os::Property& property) {
gearbox[i]=t;
}


return true;
}

Expand Down Expand Up @@ -317,6 +325,57 @@ void OpticalEncodersConsistency::run()
int cycle=0;
double start_time = yarp::os::Time::now();

//****************************************************************************************
//Retrieving coupling matrix using IRemoteVariable
//****************************************************************************************

yarp::os::Bottle b;

ivar->getRemoteVariable("kinematic_mj", b);

int matrix_size = matrix.cols();

matrix.eye();

int njoints [4];

for(int i=0 ; i< b.size() ; i++)
{
Bottle bv;
bv.fromString(b.get(i).toString());
njoints[i] = sqrt(bv.size());

int ele = 0;
if(i==0) {
for (int r=0; r < njoints[i]; r++)
{
for (int c=0; c < njoints[i]; c++)
{
matrix(r,c) = bv.get(ele).asFloat64();
ele++;
}
}

}
else{
for (int r=0; r < njoints[i]; r++)
{
for (int c=0; c < njoints[i]; c++)
{
int jntprev = 0;
for (int j=0; j < i; j++) jntprev += njoints[j];
if(!jntprev > matrix_size) matrix(r+jntprev,c+jntprev) = bv.get(ele).asFloat64();
ele++;
}
}
}

}

// yDebug() << "MATRIX J2M : \n" << matrix.toString();

// **************************************************************************************

trasp_matrix = matrix.transposed();
inv_matrix = yarp::math::luinv(matrix);
inv_trasp_matrix = inv_matrix.transposed();
Expand All @@ -341,6 +400,8 @@ void OpticalEncodersConsistency::run()
yarp::sig::Vector tmp_vector;
tmp_vector.resize(n_part_joints);



while (1)
{
double curr_time = yarp::os::Time::now();
Expand Down Expand Up @@ -375,13 +436,16 @@ void OpticalEncodersConsistency::run()
{
off_enc_jnt = enc_jnt;
off_enc_mot2jnt = enc_mot2jnt;

}

enc_jnt2mot = matrix * enc_jnt;
enc_mot2jnt = inv_matrix * (enc_mot - off_enc_mot);
vel_jnt2mot = matrix * vel_jnt;
//acc_jnt2mot = matrix * acc_jnt;
for (unsigned int i = 0; i < jointsList.size(); i++) enc_jnt2mot[i] = enc_jnt2mot[i] * gearbox[i];


for (unsigned int i = 0; i < jointsList.size(); i++) enc_jnt2mot[i] = enc_jnt2mot[i] * gearbox[i];;
for (unsigned int i = 0; i < jointsList.size(); i++) vel_jnt2mot[i] = vel_jnt2mot[i] * gearbox[i];
//for (unsigned int i = 0; i < jointsList.size(); i++) acc_jnt2mot[i] = acc_jnt2mot[i] * gearbox[i];
for (unsigned int i = 0; i < jointsList.size(); i++) enc_mot2jnt[i] = enc_mot2jnt[i] / gearbox[i];
Expand Down
6 changes: 4 additions & 2 deletions src/motorEncoders-consistency/motorEncodersConsistency.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -108,13 +108,15 @@ class OpticalEncodersConsistency : public yarp::robottestingframework::TestCase

int n_part_joints;
int cycles;

yarp::dev::PolyDriver *dd;
yarp::dev::IPositionControl *ipos;
yarp::dev::IControlMode *icmd;
yarp::dev::IPositionControl *ipos;
yarp::dev::IControlMode *icmd;
yarp::dev::IInteractionMode *iimd;
yarp::dev::IEncoders *ienc;
yarp::dev::IMotorEncoders *imotenc;
yarp::dev::IMotor *imot;
yarp::dev::IRemoteVariables *ivar;

yarp::sig::Vector zero_vector;
yarp::sig::Vector enc_jnt;
Expand Down
5 changes: 0 additions & 5 deletions suites/contexts/icub/motorEncoderConsistency_left_arm.ini
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -9,9 +9,4 @@ min (-40 20 0 35)
cycles 10
tolerance 1.0
matrix_size 4
matrix ( 1 0 0 0 \
-1.625 1.625 0 0 \
-1.625 1.625 1.625 0 \
0 0 0 1 )

plot_enabled 0
7 changes: 0 additions & 7 deletions suites/contexts/icub/motorEncoderConsistency_left_leg.ini
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -9,11 +9,4 @@ min (0 10 0 0 0 0)
cycles 10
tolerance 1.0
matrix_size 6
matrix ( 1.5 0 0 0 0 0 \
0 1 0 0 0 0 \
0 0 1 0 0 0 \
0 0 0 1 0 0 \
0 0 0 0 1 0 \
0 0 0 0 0 1 )

plot_enabled 0
5 changes: 0 additions & 5 deletions suites/contexts/icub/motorEncoderConsistency_right_arm.ini
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -9,9 +9,4 @@ min (-40 20 0 35)
cycles 10
tolerance 1.0
matrix_size 4
matrix ( 1 0 0 0 \
-1.625 1.625 0 0 \
-1.625 1.625 1.625 0 \
0 0 0 1 )

plot_enabled 0
9 changes: 1 addition & 8 deletions suites/contexts/icub/motorEncoderConsistency_right_leg.ini
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -9,11 +9,4 @@ min (0 10 0 0 0 0)
cycles 10
tolerance 1.0
matrix_size 6
matrix ( 1.5 0 0 0 0 0 \
0 1 0 0 0 0 \
0 0 1 0 0 0 \
0 0 0 1 0 0 \
0 0 0 0 1 0 \
0 0 0 0 0 1 )

plot_enabled 0
plot_enabled 0
7 changes: 2 additions & 5 deletions suites/contexts/icub/motorEncoderConsistency_torso.ini
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -7,9 +7,6 @@ max ( 10 5 10)
min (-10 -5 -10)
cycles 10
tolerance 1.2
matrix_size 3
matrix ( 1.818181818182 -1 0 \
0 1 -1 \
0 1 1 )

matrix_size 3
plot_enabled 0

1 change: 1 addition & 0 deletions suites/encoders-icub.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -4,6 +4,7 @@
<description> Testing encoders</description>
<environment>--robotname icub</environment>


<test type="dll" param="--from optical_encoders_drift_left_arm.ini"> OpticalEncodersDrift </test>
<test type="dll" param="--from optical_encoders_drift_right_arm.ini"> OpticalEncodersDrift </test>
<test type="dll" param="--from optical_encoders_drift_left_leg.ini"> OpticalEncodersDrift </test>
Expand Down
7 changes: 4 additions & 3 deletions suites/encoders-icubSim.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -5,10 +5,11 @@
<environment>--robotname icubSim</environment>
<fixture param="--fixture icubsim-fixture.xml"> yarpmanager </fixture>

<test type="dll" param="--from optical_encoders_consistency_left_arm.ini"> MotorEncodersConsistency </test>

<test type="dll" param="--from contexts/icubSim/optical_encoders_consistency_left_arm.ini"> MotorEncodersConsistency </test>
<!-- <test type="dll" param="--from optical_encoders_consistency_left_arm.ini"> MotorEncodersConsistency </test>
<test type="dll" param="--from optical_encoders_consistency_right_arm.ini"> MotorEncodersConsistency </test>
<test type="dll" param="--from optical_encoders_consistency_left_leg.ini"> MotorEncodersConsistency </test>
<test type="dll" param="--from optical_encoders_consistency_right_leg.ini"> MotorEncodersConsistency </test>
<test type="dll" param="--from optical_encoders_consistency_torso.ini"> MotorEncodersConsistency </test>
<test type="dll" param="--from optical_encoders_consistency_torso.ini"> MotorEncodersConsistency </test> -->
</suite>