Commit 00223c42 authored by Luca's avatar Luca
Browse files

London Commit

parent 8ca3dbbb
......@@ -14,10 +14,9 @@
</component>
<component name="ChangeListManager">
<list default="true" id="f49e54e8-1c2f-4aa2-b4ea-b05666a56758" name="Changes" comment="">
<change beforePath="$PROJECT_DIR$/CMakeLists.txt" beforeDir="false" afterPath="$PROJECT_DIR$/CMakeLists.txt" afterDir="false" />
<change beforePath="$PROJECT_DIR$/velocityControllerModule.cpp" beforeDir="false" />
<change beforePath="$PROJECT_DIR$/velocityControllerThread.cpp" beforeDir="false" />
<change beforePath="$PROJECT_DIR$/velocityControllerThread.h" beforeDir="false" />
<change beforePath="$PROJECT_DIR$/.idea/workspace.xml" beforeDir="false" afterPath="$PROJECT_DIR$/.idea/workspace.xml" afterDir="false" />
<change beforePath="$PROJECT_DIR$/app/scripts/velocityController.xml" beforeDir="false" afterPath="$PROJECT_DIR$/app/scripts/velocityController.xml" afterDir="false" />
<change beforePath="$PROJECT_DIR$/module/velocityControllerThread.cpp" beforeDir="false" afterPath="$PROJECT_DIR$/module/velocityControllerThread.cpp" afterDir="false" />
</list>
<option name="SHOW_DIALOG" value="false" />
<option name="HIGHLIGHT_CONFLICTS" value="true" />
......@@ -56,6 +55,11 @@
<property name="settings.editor.selected.configurable" value="CMakeSettings" />
</component>
<component name="RunManager" selected="CMake Application.Unnamed">
<configuration default="true" type="CLionExternalRunConfiguration" factoryName="Application" REDIRECT_INPUT="false" ELEVATE="false" USE_EXTERNAL_CONSOLE="false" PASS_PARENT_ENVS_2="true">
<method v="2">
<option name="CLION.EXTERNAL.BUILD" enabled="true" />
</method>
</configuration>
<configuration name="Unnamed" type="CLionNativeAppRunConfigurationType" nameIsGenerated="true" REDIRECT_INPUT="false" ELEVATE="false" USE_EXTERNAL_CONSOLE="false" PASS_PARENT_ENVS_2="true" version="1">
<method v="2">
<option name="CLION.COMPOUND.BUILD" enabled="true" />
......@@ -82,11 +86,11 @@
</method>
</configuration>
<list>
<item itemvalue="Makefile Application.Unnamed" />
<item itemvalue="CMake Application.Unnamed" />
<item itemvalue="CMake Application.copy_lua_in_build" />
<item itemvalue="CMake Application.uninstall" />
<item itemvalue="CMake Application.velocityController" />
<item itemvalue="Makefile Application.Unnamed" />
</list>
</component>
<component name="SpellCheckerSettings" RuntimeDictionaries="0" Folders="0" CustomDictionaries="0" DefaultDictionary="application-level" UseSingleDictionary="true" transferred="true" />
......@@ -100,6 +104,7 @@
<workItem from="1643205581629" duration="7000" />
<workItem from="1643208757006" duration="10000" />
<workItem from="1643232050805" duration="13023000" />
<workItem from="1647876034194" duration="119000" />
</task>
<servers />
</component>
......
......@@ -25,17 +25,17 @@
</module>
<module>
<name>iKinCartesianSolver</name>
<parameters>--context simCartesianControl --part right_arm</parameters>
<parameters>--context simCartesianControl --part left_arm</parameters>
<dependencies>
<port timeout="20">/icubSim/torso/state:o</port>
<port timeout="20">/icubSim/right_arm/state:o</port>
<port timeout="20">/icubSim/left_arm/state:o</port>
</dependencies>
<environment>YARP_FORWARD_LOG_ENABLE=1</environment>
<node>localhost</node>
</module>
<module>
<name>yarpdatadumper</name>
<parameters> --name /data/rightArm --rxTime</parameters>
<parameters> --name /data/left_arm --rxTime</parameters>
<node>localhost</node>
<stdio></stdio>
<tag>yarpdatadumper</tag>
......
......@@ -24,7 +24,7 @@
//#include <yarp/dev/GazeControl.h>
//#include <yarp/dev/PolyDriver.h>
#define TASK_PERIOD 0.01 // [s] delta_time between two successive trajectory steps
#define TASK_PERIOD 0.15 // [s] delta_time between two successive trajectory steps
#define CTRL_THREAD_PER 0.005 // [s]
#define MAX_TORSO_PITCH 30.0 // [deg]
......@@ -50,6 +50,7 @@ bool CtrlThread::threadInit() {
partName = rf.find("part").asString();
enable_torso = rf.find("torso").asBool();
enable_gaze = rf.find("gaze").asBool();
std::string plane = rf.find("plane").asString();
std::string file_path = rf.find("file_path").asString();
std::cout << "Module settings:\n";
......@@ -57,6 +58,7 @@ bool CtrlThread::threadInit() {
std::cout << "part: " << partName.c_str() << '\n';
std::cout << "enable torso: " << enable_torso << '\n';
std::cout << "enable gaze: " << enable_gaze << '\n';
std::cout << "plane: " << plane << '\n';
std::cout << "file path: " << file_path.c_str() << '\n';
//read .txt from file_path
......@@ -143,75 +145,31 @@ bool CtrlThread::threadInit() {
od.resize(4);
x_dot.resize(3);
o_dot.resize(4);
//
A_fro.resize(3);
B_fro.resize(3);
od_fro.resize(4);
A_sag.resize(3);
B_sag.resize(3);
od_sag.resize(4);
A_obl.resize(3);
B_obl.resize(3);
od_obl.resize(4);
A_mat.resize(3,3);
B_mat.resize(3,3);
od_mat.resize(3,4);
//
//od[0] = 0.0;
//od[1] = 1.0;
//od[2] = 0.0;
//od[3] = M_PI;
// sagittale
// od[0] = 0.22;
// od[1] = 0.314;
// od[2] = -0.923;
// od[3] = M_PI;
// cerchio (obliquo)
// od[0] = -0.276;
// od[1] = -0.614;
// od[2] = 0.739;
// od[3] = 2.098;
// od[0] = 0.005366;
// od[1] = 0.424294;
// od[2] = -0.905508;
// od[3] = 2.982768;
// palmo verticale verso centro 0 0 1 M_PI
//palmo orizzontale verso basso 0 1 0 M_PI
// palmo icub startup 0 1 0 M_PI
// Frontale
B_fro = { -0.25, 0.0, 0.1 };
A_fro = { -0.25, 0.3, 0.1 };
od_fro = {0 , 0 , 1 , M_PI };
A = { -0.25, 0.0, 0.10 };
B = { -0.25, 0.0, 0.10 };
od = {0 , 1 , 0 , M_PI };
A_fro = { -0.16, 0.01, 0.00 }; //new berry
B_fro = { -0.33, 0.05, 0.21 };
od_fro = {0 , 0 , 1 , M_PI };
// Sagittale
A_sag = { -0.265, 0.124, 0.249 }; // questo OKKKK
B_sag = { -0.265, 0.150, 0.150 };
od_sag = {0 , 1 , 0 , M_PI };
A_sag = { -0.104, 0.373, 0.336 }; //new berry
B_sag = { -0.360, 0.092, 0.220 };
od_sag = {0 , 1 , 0 , M_PI };
// Obliquo
A_obl = { -0.218, 0.396, 0.143 };
B_obl = { -0.313, 0.152, -0.005 };
od_obl = {0.005366 , 0.424294 , -0.905508 , 2.982768 };
//
A = A_sag;
B = B_sag;
od = od_sag;
if (plane == "fro") {
A = { -0.30, 0.25, 0.07 };
B = { -0.30, 0.05, 0.07 };
od = {-0.144, -0.790, 0.59, 3.067};
od = {0.134085924737796402528, 0.746570540360291245996, -0.651654350904950829815, 2.5227762707246648155};
}
if (plane == "obl") {
A = { -0.239, 0.360, 0.114 };
B = { -0.340, 0.133, 0.031 };
od = {0 , 1 , 0 , M_PI };
}
if (plane == "sag") {
A = { -0.23, 0.131, 0.05 };
B = { -0.36, 0.11, 0.14 };
od = {-0.02 , 0.91 , -0.42 , 3.06 };
}
distance = sqrt(pow(B[0] - A[0], 2) + pow(B[1] - A[1], 2) + pow(B[2] - A[2], 2));
xd = A;
AB.resize(3);
......@@ -268,15 +226,105 @@ void CtrlThread::afterStart(bool s) {
void CtrlThread::run() {
//icart->setPosePriority("position");
//wait for new commands on the port
printf("Going to start position\n\n");
icart->goToPose(A, od,1.5);
Time::delay(4);
planTrajectory();
playTrajectory();
Time::delay(2);
for (int i = 0; i < 10; i++) {
printf("Going to start position\n\n");
printf("%i/10 \n", i+1);
icart->goToPose(A, od,1.0);
Time::delay(5);
planAndPlayTrajectory();
// planTrajectory();
// playTrajectory();
Time::delay(2);
}
Time::delay(20000);
}
////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////
void CtrlThread::planAndPlayTrajectory() {
// we keep the orientation of the left arm constant:
// we want the middle finger to point forward (end-effector x-axis)
// with the palm turned down (end-effector y-axis points leftward);
// to achieve that it is enough to rotate the root frame of pi around z-axis
distance = sqrt(pow(B[0] - A[0], 2) + pow(B[1] - A[1], 2) + pow(B[2] - A[2], 2));
//to get the direction of the segment A->B we compute the simple element-wise B-A
AB[0] = B[0] - A[0];
AB[1] = B[1] - A[1];
AB[2] = B[2] - A[2];
//sum the values in the vector
double total_time = 0;
for (int i = 0; i < time.size(); i++) {
total_time += time[i];
}
size_t n_steps = time.length();
Matrix velocityReport(n_steps, 3);
Vector xd_old = xd;
Vector od_old = od;
Network::connect("/" + robotName + "/cartesianController/" + partName + "/state:o","/data/" + partName);
double t_start = Time::now();
xd = A;
for (int i = 0; i < n_steps; i++) {
//update time variable
t = Time::now();
//moltiplico il vettore AB per il valore [0,1] della traiettoria realizzata
float next_distance = time[i];
xd[0] = A[0] + AB[0] * (next_distance / distance);
xd[1] = A[1] + AB[1] * (next_distance / distance);
xd[2] = A[2] + AB[2] * (next_distance / distance);
//icart->setTaskVelocities((xd-xd_old)/TASK_PERIOD, (od-od_old)/TASK_PERIOD);
//icart->goToPose(xd,od,TASK_PERIOD+0.15); //per 0.15 era 0.15
icart->goToPose(xd,od,TASK_PERIOD*3);
xd_old = xd;
od_old = od;
if (enable_gaze) {
igaze->lookAtFixationPointSync(xd);
}
// wait until the next step
while (Time::now() - t < TASK_PERIOD) {
Time::delay(0.001);
}
printf("%.2f%% going to xd | %f %f %f | in %f sec. \n", (next_distance / distance)*100, xd[0], xd[1], xd[2], Time::now() - t);
// icart->getTaskVelocities(x_dot, o_dot);
// velocityReport.setRow(i, x_dot);
}
Time::delay(1);
Network::disconnect("/" + robotName + "/cartesianController/" + partName + "/state:o","/data/" + partName);
//print time elapsed
printf("Action performed in %zu steps, for a total of %f sec. , it was planned to last %f sec. \n",
n_steps,
Time::now() - t_start,
n_steps * TASK_PERIOD);
//write velocity report to file
cout << "writing report after " << n_steps << "steps" << endl;
std::ofstream myfile;
myfile.open("velocityReport-" + to_string(trial_n) + ".csv", std::ofstream::out);
myfile << velocityReport.toString();
myfile.close();
trial_n++;
}
void CtrlThread::threadRelease() {
// we require an immediate stop
......@@ -293,13 +341,6 @@ void CtrlThread::threadRelease() {
// ask the solver to compute the joint angles that allow to reach that position
void CtrlThread::planTrajectory(){
//to get the direction of the segment A->B we compute the simple element-wise B-A
AB[0] = B[0] - A[0];
AB[1] = B[1] - A[1];
AB[2] = B[2] - A[2];
trajectory.resize(time.length(),njoints+3);//+3 for the torso
Vector q_start;
Vector q_final;
q_start.resize(njoints);
......@@ -307,8 +348,19 @@ void CtrlThread::planTrajectory(){
Vector xd_final = xd;
Vector od_final = od;
trajectory.resize(time.length(),njoints+3);//+3 for the torso
icart->getPose(A,od_final);
encs->getEncoders(q_start.data());
//to get the direction of the segment A->B we compute the simple element-wise B-A
AB[0] = B[0] - A[0];
AB[1] = B[1] - A[1];
AB[2] = B[2] - A[2];
distance = sqrt(pow(B[0] - A[0], 2) + pow(B[1] - A[1], 2) + pow(B[2] - A[2], 2));
printf("PLANNING: \n");
for (int i = 0; i < time.length(); i++) {
xd[0] = A[0] + AB[0] * (time[i] / distance);
......@@ -323,15 +375,23 @@ void CtrlThread::planTrajectory(){
q_start = q_final;
if ( (int((float(i+1)/time.length())*1000)) % 200 == 0 ){
yInfo("progress: %i ", int((float(i+1)/time.length())*100) );
yInfo("progress: %i%% ", int((float(i+1)/time.length())*100) );
}
}
}
void CtrlThread::playTrajectory(){
// for (int i = 0; i < 7; i++) {
// ictrl->setControlMode(i, VOCAB_CM_POSITION);
// pos->positionMove(i,trajectory.getRow(0).subVector(3,9)[i]);
// }
// Time::delay(4);
Matrix velocityReport(trajectory.rows(), 3);
Vector command,prev_command;
Vector command,prev_command,x_tmp,o_tmp;
x_tmp.resize(3);
o_tmp.resize(4);
command.resize(trajectory.cols());
prev_command.resize(trajectory.cols());
......@@ -347,7 +407,7 @@ void CtrlThread::playTrajectory(){
command = trajectory.getRow(i); //10 joints
for (int j = 0; j < trajectory.cols(); j++) {
//fix the single encoder value for each row by setting the previous value
if (abs(command[j]-prev_command[j]) > 5.0) {command[j] = prev_command[j];}
if (abs(command[j]-prev_command[j]) > 4.0) {command[j] = prev_command[j];}
}
//update
trajectory.setRow(i, command);
......@@ -373,19 +433,27 @@ void CtrlThread::playTrajectory(){
//ho 10 colonne di traiettoria, 3 torso + 4 braccio + 3 polso
command = trajectory.getRow(i).subVector(3,9);
// wee keep constanst the wrist orientation
command.setSubvector(4, trajectory.getRow(0).subVector(7,9));
// command.setSubvector(4, trajectory.getRow(0).subVector(7,9));
//pos->positionMove(command.data());
idir->setPositions(command.data());
// wait until next time step
while (Time::now() - t < TASK_PERIOD) {
Time::delay(0.001);
icart->getPose(x_tmp,o_tmp);
// if target pose is not reached
if( abs(x_tmp[0]-B[0]) > 0.02 || abs(x_tmp[1]-B[1]) > 0.02 || abs(x_tmp[2]-B[2]) > 0.02 ){
idir->setPositions(command.data());
// wait until next time step
while (Time::now() - t < TASK_PERIOD) {
Time::delay(0.001);
}
//print elapsed time
if (i%4 != 0)
yInfo("%s", x_tmp.toString().c_str());
yInfo("reached | %s | in [%f sec]", command.subVector(0,3).toString().c_str(), Time::now() - t);
}
//print elapsed time
yInfo("reached | %s | in [%f sec]", command.subVector(0,3).toString().c_str(), Time::now() - t);
}
......@@ -403,87 +471,6 @@ void CtrlThread::playTrajectory(){
trial_n++;
}
void CtrlThread::planAndPlayTrajectory() {
// we keep the orientation of the left arm constant:
// we want the middle finger to point forward (end-effector x-axis)
// with the palm turned down (end-effector y-axis points leftward);
// to achieve that it is enough to rotate the root frame of pi around z-axis
//to get the direction of the segment A->B we compute the simple element-wise B-A
AB[0] = B[0] - A[0];
AB[1] = B[1] - A[1];
AB[2] = B[2] - A[2];
//sum the values in the vector
double total_time = 0;
for (int i = 0; i < time.size(); i++) {
total_time += time[i];
}
size_t n_steps = time.length();
Matrix velocityReport(n_steps, 3);
Vector xd_old = xd;
Vector od_old = od;
Network::connect("/icub/cartesianController/right_arm/state:o","/data/rightArm");
double t_start = Time::now();
xd = A;
for (int i = 0; i < n_steps; i++) {
//update time variable
t = Time::now();
//moltiplico il vettore AB per il valore [0,1] della traiettoria realizzata
float next_distance = time[i];
xd[0] = A[0] + AB[0] * (next_distance / distance);
xd[1] = A[1] + AB[1] * (next_distance / distance);
xd[2] = A[2] + AB[2] * (next_distance / distance);
//icart->setTaskVelocities((xd-xd_old)/TASK_PERIOD, (od-od_old)/TASK_PERIOD);
//icart->goToPose(xd,od,TASK_PERIOD+0.15); //per 0.15 era 0.15
icart->goToPose(xd,od,TASK_PERIOD*2.5); //per 0.15 era 0.15
xd_old = xd;
od_old = od;
if (enable_gaze) {
igaze->lookAtFixationPointSync(xd);
}
// wait until the next step
while (Time::now() - t < TASK_PERIOD) {
Time::delay(0.001);
}
printf("%.2f%% going to xd | %f %f %f | in %f sec. \n", (next_distance / distance)*100, xd[0], xd[1], xd[2], Time::now() - t);
icart->getTaskVelocities(x_dot, o_dot);
velocityReport.setRow(i, x_dot);
}
Network::disconnect("/icub/cartesianController/right_arm/state:o","/data/rightArm");
//print time elapsed
printf("Action performed in %zu steps, for a total of %f sec. , it was planned to last %f sec. \n",
n_steps,
Time::now() - t_start,
n_steps * TASK_PERIOD);
//write velocity report to file
cout << "writing report after " << n_steps << "steps" << endl;
std::ofstream myfile;
myfile.open("velocityReport-" + to_string(trial_n) + ".csv", std::ofstream::out);
myfile << velocityReport.toString();
myfile.close();
trial_n++;
}
void CtrlThread::limitTorsoPitch() {
int axis=0; // pitch joint
double min, max;
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment