Updated app name (M1DemoMachineROS)

Author: ywen3

doc/1.GettingStarted/GSM1DemoMachine.md

@@ -84,7 +84,7 @@ Use keyboard to explore different controllers:
 - Key A: position, velocity, and torque controller demonstrations
 - Key Q: return to idle state
 
-## Build and run M1ROSDemoMachine (To be tested)
+## Build and run M1DemoMachineROS
 
 Configure the CMakeList.txt, select the M1DemoMachine, and set the flags for using a real robot without ROS support:
 
@@ -94,7 +94,7 @@ Configure the CMakeList.txt, select the M1DemoMachine, and set the flags for usi
 #set (STATE_MACHINE_NAME "M2DemoMachine")
 #set (STATE_MACHINE_NAME "M3DemoMachine")
 #set (STATE_MACHINE_NAME "X2DemoMachine")
-set (STATE_MACHINE_NAME "M1ROSDemoMachine")
+set (STATE_MACHINE_NAME "M1DemoMachineROS")
 #set (STATE_MACHINE_NAME "LoggingDevice")
 # Comment to use actual hardware, uncomment for a nor robot (virtual) app
 set(NO_ROBOT OFF)

doc/1.GettingStarted/GettingStarted.md

@@ -104,4 +104,3 @@ This example enables simple movements with the AnkleMotus M1 System.
 ### [M1DemoMachine - Instructions](GSM1DemoMachine.md)
 See the code example in `src/apps/M1DemoMachine`.
 
-

…c/apps/MultiM1Machine/MultiM1Machine.cpp …ps/M1DemoMachineROS/M1DemoMachineROS.cppsrc/apps/MultiM1Machine/MultiM1Machine.cpp renamed to src/apps/M1DemoMachineROS/M1DemoMachineROS.cpp src/apps/MultiM1Machine/MultiM1Machine.cpp renamed to src/apps/M1DemoMachineROS/M1DemoMachineROS.cpp

@@ -1,9 +1,9 @@
-#include "MultiM1Machine.h"
+#include "M1DemoMachineROS.h"
 
-#define OWNER ((MultiM1Machine *)owner)
+#define OWNER ((M1DemoMachineROS *)owner)
 
-MultiM1Machine::MultiM1Machine(int argc, char *argv[]){
-    spdlog::debug("MultiM1Machine::constructed!");
+M1DemoMachineROS::M1DemoMachineROS(int argc, char *argv[]){
+    spdlog::debug("M1DemoMachineROS::constructed!");
 
     ros::init(argc, argv, "m1", ros::init_options::NoSigintHandler);
     ros::NodeHandle nodeHandle("~");
@@ -16,23 +16,23 @@ MultiM1Machine::MultiM1Machine(int argc, char *argv[]){
     robot_ = new RobotM1(robotName_);
 
     // Create ros object
-    multiM1MachineRos_ = new MultiM1MachineROS(robot_);
+    M1MachineRos_ = new M1MachineROS(robot_);
 
     // Pass nodeHandle to the classes that use ROS features
-    multiM1MachineRos_->setNodeHandle(nodeHandle);
-    multiM1MachineRos_->initialize();
+    M1MachineRos_->setNodeHandle(nodeHandle);
+    M1MachineRos_->initialize();
 
     // Create states with ROS features // This should be created after ros::init()
-    multiControllerState_ = new MultiControllerState(this, robot_, multiM1MachineRos_);
+    multiControllerState_ = new MultiControllerState(this, robot_, M1MachineRos_);
 
     //Initialize the state machine with first state of the designed state machine, using baseclass function.
     StateMachine::initialize(multiControllerState_);
 }
 
-MultiM1Machine::~MultiM1Machine() {
+M1DemoMachineROS::~M1DemoMachineROS() {
     currentState->exit();
     robot_->disable();
-    delete multiM1MachineRos_;
+    delete M1MachineRos_;
     delete robot_;
 }
 
@@ -42,12 +42,12 @@ MultiM1Machine::~MultiM1Machine() {
  *
  */
 
-void MultiM1Machine::init() {
+void M1DemoMachineROS::init() {
 //    ros::init(argc, argv, "m1", ros::init_options::NoSigintHandler);
 //    ros::NodeHandle nodeHandle("~");
 
 //    // Pass nodeHandle to the classes that use ROS features
-//    multiM1MachineRos_->setNodeHandle(nodeHandle);
+//    M1MachineRos_->setNodeHandle(nodeHandle);
 
     if(robot_->initialise()) {
         initialised = true;
@@ -88,27 +88,27 @@ void MultiM1Machine::init() {
     logHelper.add(multiControllerState_->tau_cmd, "CommandTorque");      // motor_torque = command_torque + compensation_torque
     logHelper.add(robot_->tau_motor, "MotorTorque");
 
-    logHelper.add(multiM1MachineRos_->jointTorqueCommand_, "MM1_DesiredJointTorques");
-    logHelper.add(multiM1MachineRos_->jointPositionCommand_, "MM1_DesiredJointPositions");
-    logHelper.add(multiM1MachineRos_->interactionTorqueCommand_, "MM1_DesiredInteractionTorques");
+    logHelper.add(M1MachineRos_->jointTorqueCommand_, "MM1_DesiredJointTorques");
+    logHelper.add(M1MachineRos_->jointPositionCommand_, "MM1_DesiredJointPositions");
+    logHelper.add(M1MachineRos_->interactionTorqueCommand_, "MM1_DesiredInteractionTorques");
 
     logHelper.add(multiControllerState_->digitalInValue_, "digitalIn");
     logHelper.add(multiControllerState_->digitalOutValue_, "digitalOut");
 
     logHelper.startLogger();
 }
 
-void MultiM1Machine::end() {
+void M1DemoMachineROS::end() {
     if(initialised) {
         currentState->exit();
         robot_->stop();
         logHelper.endLog();
-        delete multiM1MachineRos_;
+        delete M1MachineRos_;
         delete robot_;
     }
 }
 
-bool MultiM1Machine::configureMasterPDOs() {
+bool M1DemoMachineROS::configureMasterPDOs() {
     spdlog::debug("M1DemoMachine::configureMasterPDOs()");
     return robot_->configureMasterPDOs();
 }
@@ -118,9 +118,9 @@ bool MultiM1Machine::configureMasterPDOs() {
  * that need to run every program loop update cycle.
  *
  */
-void MultiM1Machine::hwStateUpdate(void) {
+void M1DemoMachineROS::hwStateUpdate(void) {
     robot_->updateRobot();
-    multiM1MachineRos_->update();
+    M1MachineRos_->update();
     time_ = (std::chrono::duration_cast<std::chrono::microseconds>(
             std::chrono::steady_clock::now() - time0_).count()) / 1e6;
     ros::spinOnce();

src/apps/MultiM1Machine/MultiM1Machine.h …apps/M1DemoMachineROS/M1DemoMachineROS.hsrc/apps/MultiM1Machine/MultiM1Machine.h renamed to src/apps/M1DemoMachineROS/M1DemoMachineROS.h src/apps/MultiM1Machine/MultiM1Machine.h renamed to src/apps/M1DemoMachineROS/M1DemoMachineROS.h

@@ -1,11 +1,11 @@
 /**
- * \file MultiM1Machine.h
+ * \file M1DemoMachineROS.h
  * \author Emek Baris Kucuktabak
  * \version 0.1
  * \date 2020-11-22
  * \copyright Copyright (c) 2020
  *
- * /brief The MultiM1Machine class represents an example implementation of a stateMachine in multi-robot control setting
+ * /brief The M1DemoMachineROS class represents an example implementation of a stateMachine in multi-robot control setting
  *
  */
 #ifndef M1_SM_H
@@ -28,22 +28,22 @@
 #include "LogHelper.h"
 #include "logging.h"
 
-#include "MultiM1MachineROS.h"
+#include "M1MachineROS.h"
 
 /**
  * @brief Example implementation of a StateMachine for the M1Robot class. States should implemented M1DemoState
  *
  */
-class MultiM1Machine : public StateMachine {
+class M1DemoMachineROS : public StateMachine {
    public:
     bool running = false;
     /**
      *  \todo Pilot Parameters would be set in constructor here
      *
      */
-//    MultiM1Machine();
-    MultiM1Machine(int argc, char *argv[]);
-    ~MultiM1Machine();
+//    M1DemoMachineROS();
+    M1DemoMachineROS(int argc, char *argv[]);
+    ~M1DemoMachineROS();
 //    void init(int argc, char *argv[]);
     void init();
     void end();
@@ -60,7 +60,7 @@ class MultiM1Machine : public StateMachine {
 
    protected:
     RobotM1 *robot_; /*<!Pointer to the Robot*/
-    MultiM1MachineROS *multiM1MachineRos_; /*<!Pointer to the ROS Class*/
+    M1MachineROS *M1MachineRos_; /*<!Pointer to the ROS Class*/
 
    private:
     std::string robotName_; // robot name(obtained from node name)

…pps/MultiM1Machine/MultiM1MachineROS.cpp …c/apps/M1DemoMachineROS/M1MachineROS.cppsrc/apps/MultiM1Machine/MultiM1MachineROS.cpp renamed to src/apps/M1DemoMachineROS/M1MachineROS.cpp src/apps/MultiM1Machine/MultiM1MachineROS.cpp renamed to src/apps/M1DemoMachineROS/M1MachineROS.cpp

@@ -1,18 +1,18 @@
-#include "MultiM1MachineROS.h"
+#include "M1MachineROS.h"
 
-MultiM1MachineROS::MultiM1MachineROS(RobotM1 *robot) {
+M1MachineROS::M1MachineROS(RobotM1 *robot) {
     robot_ = robot;
 }
 
-MultiM1MachineROS::~MultiM1MachineROS() {
+M1MachineROS::~M1MachineROS() {
     ros::shutdown();
 }
 
-void MultiM1MachineROS::initialize() {
-    spdlog::debug("MultiM1MachineROS::init()");
+void M1MachineROS::initialize() {
+    spdlog::debug("M1MachineROS::init()");
 
-    jointCommandSubscriber_ = nodeHandle_->subscribe("joint_commands", 1, &MultiM1MachineROS::jointCommandCallback, this);
-    interactionTorqueCommandSubscriber_ = nodeHandle_->subscribe("interaction_effort_commands", 1, &MultiM1MachineROS::interactionTorqueCommandCallback, this);
+    jointCommandSubscriber_ = nodeHandle_->subscribe("joint_commands", 1, &M1MachineROS::jointCommandCallback, this);
+    interactionTorqueCommandSubscriber_ = nodeHandle_->subscribe("interaction_effort_commands", 1, &M1MachineROS::interactionTorqueCommandCallback, this);
     jointStatePublisher_ = nodeHandle_->advertise<sensor_msgs::JointState>("joint_states", 10);
     interactionWrenchPublisher_ = nodeHandle_->advertise<geometry_msgs::WrenchStamped>("interaction_wrench", 10);
 
@@ -22,12 +22,12 @@ void MultiM1MachineROS::initialize() {
     interactionTorqueCommand_ = Eigen::VectorXd(M1_NUM_INTERACTION);
 }
 
-void MultiM1MachineROS::update() {
+void M1MachineROS::update() {
     publishJointStates();
     publishInteractionForces();
 }
 
-void MultiM1MachineROS::publishJointStates() {
+void M1MachineROS::publishJointStates() {
     Eigen::VectorXd jointPositions = robot_->getPosition();
     Eigen::VectorXd jointVelocities = robot_->getVelocity();
     Eigen::VectorXd jointTorques = robot_->getTorque();
@@ -45,7 +45,7 @@ void MultiM1MachineROS::publishJointStates() {
     jointStatePublisher_.publish(jointStateMsg_);
 }
 
-void MultiM1MachineROS::publishInteractionForces() {
+void M1MachineROS::publishInteractionForces() {
     Eigen::VectorXd interactionTorque = robot_->getJointTor_s();
     ros::Time time = ros::Time::now();
 
@@ -57,11 +57,11 @@ void MultiM1MachineROS::publishInteractionForces() {
     interactionWrenchPublisher_.publish(interactionWrenchMsg_);
 }
 
-void MultiM1MachineROS::setNodeHandle(ros::NodeHandle &nodeHandle) {
+void M1MachineROS::setNodeHandle(ros::NodeHandle &nodeHandle) {
     nodeHandle_ = &nodeHandle;
 }
 
-void MultiM1MachineROS::jointCommandCallback(const sensor_msgs::JointState &msg) {
+void M1MachineROS::jointCommandCallback(const sensor_msgs::JointState &msg) {
 
     for(int i=0; i<M1_NUM_JOINTS; i++){
         jointPositionCommand_[i] = msg.position[i];
@@ -70,7 +70,7 @@ void MultiM1MachineROS::jointCommandCallback(const sensor_msgs::JointState &msg)
     }
 }
 
-void MultiM1MachineROS::interactionTorqueCommandCallback(const std_msgs::Float64MultiArray &msg) {
+void M1MachineROS::interactionTorqueCommandCallback(const std_msgs::Float64MultiArray &msg) {
 
     for(int i=0; i<M1_NUM_JOINTS; i++){
         interactionTorqueCommand_[i] = msg.data[i];

…/apps/MultiM1Machine/MultiM1MachineROS.h src/apps/M1DemoMachineROS/M1MachineROS.hsrc/apps/MultiM1Machine/MultiM1MachineROS.h renamed to src/apps/M1DemoMachineROS/M1MachineROS.h src/apps/MultiM1Machine/MultiM1MachineROS.h renamed to src/apps/M1DemoMachineROS/M1MachineROS.h

@@ -1,16 +1,16 @@
 /**
- * /file MultiM1MachineROS.h
+ * /file M1MachineROS.h
  * /author Emek Baris Kucuktabak
- * /brief ROS part of the MultiM1Machine
+ * /brief ROS part of the M1Machine
  * /version 0.1
  * /date 2020-11-03
  *
  * @copyright Copyright (c) 2020
  *
  */
 
-#ifndef SRC_MultiM1MachineROS_H
-#define SRC_MultiM1MachineROS_H
+#ifndef SRC_M1MachineROS_H
+#define SRC_M1MachineROS_H
 
 // msg types
 #include <sensor_msgs/JointState.h>
@@ -21,10 +21,10 @@
 #include "RobotM1.h"
 #include "ros/ros.h"  // This state machine requires ROS
 
-class MultiM1MachineROS {
+class M1MachineROS {
 public:
-    MultiM1MachineROS(RobotM1 *robot);
-    ~MultiM1MachineROS();
+    M1MachineROS(RobotM1 *robot);
+    ~M1MachineROS();
 
     void update(void);
     void publishJointStates(void);
@@ -57,4 +57,4 @@ class MultiM1MachineROS {
 
 };
 
-#endif  //SRC_MultiM1MachineROS_H
+#endif  //SRC_M1MachineROS_H

…1Machine/states/MultiControllerState.cpp …chineROS/states/MultiControllerState.cppsrc/apps/MultiM1Machine/states/MultiControllerState.cpp renamed to src/apps/M1DemoMachineROS/states/MultiControllerState.cpp src/apps/MultiM1Machine/states/MultiControllerState.cpp renamed to src/apps/M1DemoMachineROS/states/MultiControllerState.cpp

@@ -99,10 +99,10 @@ void MultiControllerState::during(void) {
         robot_->setJointTor(Eigen::VectorXd::Zero(M1_NUM_JOINTS));
     }
     else if(controller_mode_ == 2){ // follow position commands
-        robot_->setJointPos(multiM1MachineRos_->jointPositionCommand_);
+        robot_->setJointPos(M1MachineRos_->jointPositionCommand_);
     }
     else if(controller_mode_ == 3){ // follow torque commands
-        robot_->setJointTor(multiM1MachineRos_->jointTorqueCommand_);
+        robot_->setJointTor(M1MachineRos_->jointTorqueCommand_);
     }
     else if(controller_mode_ == 4){ // virtual spring - torque mode
         tau = robot_->getJointTor();
@@ -125,7 +125,7 @@ void MultiControllerState::during(void) {
         tau_filtered = tau_s(0);
 
         // get interaction torque from virtual spring
-        spring_tor = -multiM1MachineRos_->interactionTorqueCommand_(0);
+        spring_tor = -M1MachineRos_->interactionTorqueCommand_(0);
 //        spring_tor = spk_*M_PIf64*(45-q(0))/180.0;  //stiffness; q(0) in degree
         robot_->tau_spring[0] = spring_tor;   // for ROS publish only
 

…iM1Machine/states/MultiControllerState.h …MachineROS/states/MultiControllerState.hsrc/apps/MultiM1Machine/states/MultiControllerState.h renamed to src/apps/M1DemoMachineROS/states/MultiControllerState.h src/apps/MultiM1Machine/states/MultiControllerState.h renamed to src/apps/M1DemoMachineROS/states/MultiControllerState.h

@@ -13,7 +13,7 @@
 
 #include "State.h"
 #include "RobotM1.h"
-#include "MultiM1MachineROS.h"
+#include "M1MachineROS.h"
 
 // dynamic reconfigure
 #include <dynamic_reconfigure/server.h>
@@ -26,14 +26,14 @@
  */
 class MultiControllerState : public State {
     RobotM1 *robot_;
-    MultiM1MachineROS *multiM1MachineRos_;
+    M1MachineROS *M1MachineRos_;
 
 public:
     void entry(void);
     void during(void);
     void exit(void);
-    MultiControllerState(StateMachine *m, RobotM1 *exo, MultiM1MachineROS *multiM1MachineRos, const char *name = NULL) :
-                        State(m, name), robot_(exo), multiM1MachineRos_(multiM1MachineRos){};
+    MultiControllerState(StateMachine *m, RobotM1 *exo, M1MachineROS *M1MachineRos, const char *name = NULL) :
+                        State(m, name), robot_(exo), M1MachineRos_(M1MachineRos){};
 
     int cali_stage;
     int cali_velocity;

src/core/main.cpp

@@ -51,8 +51,8 @@ static int rt_thread_epoll_fd; /*!< epoll file descriptor for rt thread */
 static int rtControlPriority = 80; /*!< priority of application thread */
 static void *rt_control_thread(void *arg);
 static pthread_t rt_control_thread_id;
-const float controlLoopPeriodInms = 10;   /*!< Define the control loop period (in ms): the period of rt_control_thread loop. */
-const float CANUpdateLoopPeriodInms = 10; /*!< Define the CAN PDO sync message period (and so PDO update rate). In ms. Less than 3 can lead to unstable communication  */
+const float controlLoopPeriodInms = 3;   /*!< Define the control loop period (in ms): the period of rt_control_thread loop. */
+const float CANUpdateLoopPeriodInms = 3; /*!< Define the CAN PDO sync message period (and so PDO update rate). In ms. Less than 3 can lead to unstable communication  */
 CO_NMT_reset_cmd_t reset_local = CO_RESET_NOT;
 
 /** @brief Task Timer used for the Control Loop*/