fix examples

syroco · Jan 22, 2020 · 104127a · 104127a
1 parent 8d0edf2
commit 104127a
Show file tree

Hide file tree

Showing 2 changed files with 24 additions and 23 deletions.
diff --git a/examples/intermediate/01-using_callbacks.cc b/examples/intermediate/01-using_callbacks.cc
@@ -123,23 +123,23 @@ int main(int argc, char const *argv[])
         ,QPSolverImplType::qpOASES
     );
 
-    auto cart_task = std::make_shared<CartesianTask>("CartTask_EE");
-    controller.addTask(cart_task);
-    cart_task->setControlFrame("link_7"); //
-    Eigen::Affine3d cart_pos_ref;
-    cart_pos_ref.translation() = Eigen::Vector3d(1.,0.75,0.5); // x,y,z in meters
-    cart_pos_ref.linear() = Eigen::Quaterniond::Identity().toRotationMatrix();
-    Vector6d cart_vel_ref = Vector6d::Zero();
-    Vector6d cart_acc_ref = Vector6d::Zero();
-
+    auto cart_acc_pid = std::make_shared<CartesianAccelerationPID>("servo_controller");
     Vector6d P;
     P << 1000, 1000, 1000, 10, 10, 10;
-    //cart_task->servoController()->pid()->setProportionalGain(P);
+    cart_acc_pid->pid()->setProportionalGain(P);
     Vector6d D;
     D << 100, 100, 100, 1, 1, 1;
-    //cart_task->servoController()->pid()->setDerivativeGain(D);
+    cart_acc_pid->pid()->setDerivativeGain(D);
+    cart_acc_pid->setControlFrame("link_7");
+    Eigen::Affine3d cart_pos_ref;
+    cart_pos_ref.translation() = Eigen::Vector3d(0.3,-0.5,0.41); // x,y,z in meters
+    cart_pos_ref.linear() = orca::math::quatFromRPY(M_PI,0,0).toRotationMatrix();
+    Vector6d cart_vel_ref = Vector6d::Zero();
+    Vector6d cart_acc_ref = Vector6d::Zero();
+    cart_acc_pid->setDesired(cart_pos_ref.matrix(),cart_vel_ref,cart_acc_ref);
 
-    //cart_task->servoController()->setDesired(cart_pos_ref.matrix(),cart_vel_ref,cart_acc_ref);
+    auto cart_task = controller.addTask<CartesianTask>("CartTask_EE");
+    cart_task->setServoController(cart_acc_pid);
 
     const int ndof = robot_model->getNrOfDegreesOfFreedom();
 

diff --git a/examples/intermediate/02-using_lambda_callbacks.cc b/examples/intermediate/02-using_lambda_callbacks.cc
@@ -122,22 +122,23 @@ int main(int argc, char const *argv[])
         ,QPSolverImplType::qpOASES
     );
 
-    auto cart_task = std::make_shared<CartesianTask>("CartTask_EE");
-    controller.addTask(cart_task);
-    cart_task->setControlFrame("link_7"); //
-    Eigen::Affine3d cart_pos_ref;
-    cart_pos_ref.translation() = Eigen::Vector3d(1.,0.75,0.5); // x,y,z in meters
-    cart_pos_ref.linear() = Eigen::Quaterniond::Identity().toRotationMatrix();
-    Vector6d cart_vel_ref = Vector6d::Zero();
-    Vector6d cart_acc_ref = Vector6d::Zero();
-
+    auto cart_acc_pid = std::make_shared<CartesianAccelerationPID>("servo_controller");
     Vector6d P;
     P << 1000, 1000, 1000, 10, 10, 10;
-    //cart_task->servoController()->pid()->setProportionalGain(P);
+    cart_acc_pid->pid()->setProportionalGain(P);
     Vector6d D;
     D << 100, 100, 100, 1, 1, 1;
-    //cart_task->servoController()->pid()->setDerivativeGain(D);
+    cart_acc_pid->pid()->setDerivativeGain(D);
+    cart_acc_pid->setControlFrame("link_7");
+    Eigen::Affine3d cart_pos_ref;
+    cart_pos_ref.translation() = Eigen::Vector3d(0.3,-0.5,0.41); // x,y,z in meters
+    cart_pos_ref.linear() = orca::math::quatFromRPY(M_PI,0,0).toRotationMatrix();
+    Vector6d cart_vel_ref = Vector6d::Zero();
+    Vector6d cart_acc_ref = Vector6d::Zero();
+    cart_acc_pid->setDesired(cart_pos_ref.matrix(),cart_vel_ref,cart_acc_ref);
 
+    auto cart_task = controller.addTask<CartesianTask>("CartTask_EE");
+    cart_task->setServoController(cart_acc_pid);
 
     const int ndof = robot_model->getNrOfDegreesOfFreedom();