[Command joint angles] Update the target angle of the neck joint.

mmurooka · Sep 10, 2023 · 6832ff7 · 6832ff7
1 parent 554854c
commit 6832ff7
Show file tree

Hide file tree

Showing 2 changed files with 11 additions and 1 deletion.
diff --git a/src/HVACTutorialController.cpp b/src/HVACTutorialController.cpp
@@ -13,6 +13,12 @@ HVACTutorialController::HVACTutorialController(mc_rbdyn::RobotModulePtr rm, doub
 
 bool HVACTutorialController::run()
 {
+  // Update time
+  t += solver().dt();
+
+  // Update the target angle of the neck joint
+  postureTask->target({{"NECK_Y", {mc_rtc::constants::toRad(45) * std::sin(0.5 * t)}}});
+
   return mc_control::MCController::run();
 }
 

diff --git a/src/HVACTutorialController.h b/src/HVACTutorialController.h
@@ -12,4 +12,8 @@ struct HVACTutorialController_DLLAPI HVACTutorialController : public mc_control:
   bool run() override;
 
   void reset(const mc_control::ControllerResetData & reset_data) override;
-};
+
+protected:
+  //! Current time [sec]
+  double t = 0.0;
+};