From b22b6a958d5a28fb2f4630a179907b8cb406376f Mon Sep 17 00:00:00 2001 From: Martin Karlsson <cont-mkr@ulund.org> Date: Fri, 20 Sep 2019 14:31:20 +0200 Subject: [PATCH] comment update --- cpp/main.cc | 12 +++++++----- 1 file changed, 7 insertions(+), 5 deletions(-) diff --git a/cpp/main.cc b/cpp/main.cc index eaef22f..9013cbd 100644 --- a/cpp/main.cc +++ b/cpp/main.cc @@ -87,34 +87,36 @@ void* handler_thread(void *arg) { //------------------------------------------------------------------ /* Is called when trajectory is received from orca/robot. */ void handle_trajectory(coupling_yaFromRobot* v, void* context) { - + // This cuntion is called once per sample. + // Actual configuration. mat ya(1,dof); for (int i = 0; i < dof; ++i) { ya(i) = v->a[i]; } + + // Time derivative of actual configuration. mat yad(1,dof); for (int i = dof; i < 2*dof; ++i) { yad(i-dof) = v->a[i]; } - - - + // Save trajectory (depending on mode) mtx.lock(); record_ya(ya, dmpVec, dt, sparse_dt, takeEvery, RECORD_DMP, PLAY_DMP); mtx.unlock(); + // Execute DMP. Yields reference acceleration given the state. mtx.lock(); state2yardd(ya,yad,yardd, PLAY_DMP, DMP_NBR, dmpVec, yclog, ycddlog, ya_minus_yc, yad_minus_ycd, e_norm_log, xlog, yc_minus_g, zlog); mtx.unlock(); + // Just for logging purposes. mtx.lock(); write_norms_to_file(ya_minus_yc, yad_minus_ycd, e_norm_log, xlog, yc_minus_g, zlog, PLAY_DMP); mtx.unlock(); - } //------------------------------------------------------------- -- GitLab