keyboard disturbances + separate conveyor belt control + cycle count updated

Author: Elise-J

dual_arm_control/include/dual_arm_control_iam/DualArmControlSim.hpp

@@ -85,14 +85,20 @@ struct StateMachine {
   bool isThrowing;
   bool isPlacing;
   bool isPlaceTossing;
-  bool isDisturbTarget;
   bool releaseAndretract;
   int dualTaskSelector;
   float desVtoss;
   float desiredVelImp;
   float placingPosHeight;
   float releasePosY;
 
+  //disturbance
+  bool incrementReleasePos;
+  Eigen::Vector3f deltaRelPos;
+  Eigen::Vector3f deltaPos;
+  float trackingFactor;
+  bool adaptationActive;
+
   bool startlogging;
 };
 
@@ -127,7 +133,6 @@ struct tossingTaskVariables {
 class DualArmControlSim {
 
 private:
-  int cycleCount_;
   double periodT_;
 
   // ---- Robot
@@ -140,9 +145,6 @@ class DualArmControlSim {
   bool objCtrlKey_;
   bool userSelect_ = true;
 
-  // ---- Target Disturbance
-  bool isDisturbTarget_ = false;
-
   // ====================================================================================================================
 
   int contactState_;            // Contact state with the object
@@ -224,10 +226,8 @@ class DualArmControlSim {
   float trackingFactor_;
 
   bool incrementReleasePos_ = false;
-  bool ctrlModeConveyorBelt_ = false;
   SphericalPosition releasePos_;
 
-  int desSpeedConveyorBelt_;
   Eigen::Vector2f dualPathLenAvgSpeed_;
   bool isIntercepting_ = false;
   float betaVelMod_;
@@ -338,9 +338,10 @@ class DualArmControlSim {
                                  Vector7f jointVelocity[],
                                  Vector7f jointTorques[],
                                  Eigen::Vector3f robotBasePos[],
-                                 Eigen::Vector4f robotBaseOrientation[]);
+                                 Eigen::Vector4f robotBaseOrientation[],
+                                 int cycleCount);
   void updateContactState();
-  void computeCommands(Eigen::Vector3f eePose[], Eigen::Vector4f eeOrientation[]);
+  void computeCommands(Eigen::Vector3f eePose[], Eigen::Vector4f eeOrientation[], int cycleCount);
   void updatePoses();
 
   void updateReleasePosition();

dual_arm_control/src/DualArmControlSim.cpp

@@ -334,8 +334,6 @@ void DualArmControlSim::reset() {
 bool DualArmControlSim::init() {
 
   // TODO PUT IN YAML FILE?
-
-  cycleCount_ = 0;
   gravity_ << 0.0f, 0.0f, -9.80665f;
 
   for (int k = 0; k < NB_ROBOTS; k++) {
@@ -405,7 +403,6 @@ bool DualArmControlSim::init() {
   //Keyboard interaction
   deltaRelPos_.setZero();
   deltaPos_.setZero();
-  isDisturbTarget_ = false;
 
   // TODO LOGGING OR NOT HERE?
   // startlogging_ = false;
@@ -546,7 +543,8 @@ CommandStruct DualArmControlSim::generateCommands(float firstEigenPassiveDamping
                                                   Vector7f jointVelocity[],
                                                   Vector7f jointTorques[],
                                                   Eigen::Vector3f robotBasePos[],
-                                                  Eigen::Vector4f robotBaseOrientation[]) {
+                                                  Eigen::Vector4f robotBaseOrientation[],
+                                                  int cycleCount) {
 
   d1_[LEFT] = firstEigenPassiveDamping[LEFT];
   d1_[RIGHT] = firstEigenPassiveDamping[RIGHT];
@@ -569,7 +567,7 @@ CommandStruct DualArmControlSim::generateCommands(float firstEigenPassiveDamping
             robotBaseOrientation);
 
   updatePoses();
-  computeCommands(eePose, eeOrientation);
+  computeCommands(eePose, eeOrientation, cycleCount);
 
   for (int k = 0; k < NB_ROBOTS; k++) {
     commandGenerated_.axisAngleDes[k] = robot_.getAxisAngleDes(k);
@@ -601,7 +599,7 @@ void DualArmControlSim::updateReleasePosition() {
   if (tossVar_.releasePosition(0) > 0.70) { tossVar_.releasePosition(0) = 0.70; }
 }
 
-void DualArmControlSim::computeCommands(Eigen::Vector3f eePose[], Eigen::Vector4f eeOrientation[]) {
+void DualArmControlSim::computeCommands(Eigen::Vector3f eePose[], Eigen::Vector4f eeOrientation[], int cycleCount) {
   // Update contact state
   updateContactState();
 
@@ -638,7 +636,7 @@ void DualArmControlSim::computeCommands(Eigen::Vector3f eePose[], Eigen::Vector4
   intercepLimits[3] = interceptMax(1);// y_max
 
   // ---------- Application ----------
-  if ((!releaseAndretract_) && (fmod(cycleCount_, 20) == 0)) {
+  if ((!releaseAndretract_) && (fmod(cycleCount, 20) == 0)) {
     dualPathLenAvgSpeed_ = freeMotionCtrlEstim_.predictRobotTranslation(robot_.getWHEE(),
                                                                         object_.getWHGp(),
                                                                         robot_.getWHEEStandby(),
@@ -898,17 +896,6 @@ void DualArmControlSim::computeCommands(Eigen::Vector3f eePose[], Eigen::Vector4
 
   // Extract linear velocity commands and desired axis angle command
   this->prepareCommands(robot_.getVelDesEE(), robot_.getQd(), object_.getVGpO());
-
-  // ---------- Control of conveyor belt speed ----------
-  float omegaPert = 2.f * M_PI / 1;
-  float deltaOmegaPert = 0.1f * (2.f * (float) std::rand() / RAND_MAX - 1.0f) * omegaPert;
-
-  // TODO
-  // if (ctrlModeConveyorBelt_) {
-  //   desSpeedConveyorBelt_ = (int) (nominalSpeedConveyorBelt_
-  //                                  + (int) isDisturbTarget_ * magniturePertConveyorBelt_
-  //                                      * sin((omegaPert + deltaOmegaPert) * dt_ * cycleCount_));
-  // }
 }
 
 Eigen::Vector3f DualArmControlSim::computeInterceptWithTarget(const Eigen::Vector3f& xTarget,
@@ -1210,7 +1197,6 @@ StateMachine DualArmControlSim::getStateMachine() {
   stateMachine.isThrowing = isThrowing_;
   stateMachine.isPlacing = isPlacing_;
   stateMachine.isPlaceTossing = isPlaceTossing_;
-  stateMachine.isDisturbTarget = isDisturbTarget_;
   stateMachine.dualTaskSelector = dualTaskSelector_;
   stateMachine.releaseAndretract = releaseAndretract_;
   stateMachine.desVtoss = desVtoss_;
@@ -1219,6 +1205,12 @@ StateMachine DualArmControlSim::getStateMachine() {
   stateMachine.releasePosY = tossVar_.releasePosition(1);
   stateMachine.startlogging = startlogging_;
 
+  stateMachine.incrementReleasePos = incrementReleasePos_;
+  stateMachine.deltaRelPos = deltaRelPos_;
+  stateMachine.deltaPos = deltaPos_;
+  stateMachine.trackingFactor = trackingFactor_;
+  stateMachine.adaptationActive = adaptationActive_;
+
   return stateMachine;
 }
 
@@ -1229,13 +1221,17 @@ void DualArmControlSim::updateStateMachine(StateMachine stateMachine) {
   isThrowing_ = stateMachine.isThrowing;
   isPlacing_ = stateMachine.isPlacing;
   isPlaceTossing_ = stateMachine.isPlaceTossing;
-  isDisturbTarget_ = stateMachine.isDisturbTarget;
   dualTaskSelector_ = stateMachine.dualTaskSelector;
   releaseAndretract_ = stateMachine.releaseAndretract;
   desiredVelImp_ = stateMachine.desiredVelImp;
   tossVar_.releasePosition(1) = stateMachine.releasePosY;
   xPlacing_(2) = stateMachine.placingPosHeight;
 
+  deltaRelPos_ = stateMachine.deltaRelPos;
+  deltaPos_ = stateMachine.deltaPos;
+  trackingFactor_ = stateMachine.trackingFactor;
+  adaptationActive_ = stateMachine.adaptationActive;
+
   if (desVtoss_ != stateMachine.desVtoss) {
     desVtoss_ = stateMachine.desVtoss;
     dsThrowing_.setTossLinearVelocity(desVtoss_ * tossVar_.releaseLinearVelocity.normalized());

ros_dual_arm/include/RosDualArmCommunication.hpp

@@ -48,11 +48,20 @@
 typedef Eigen::Matrix<float, 7, 1> Vector7f;
 typedef Eigen::Matrix<float, 6, 1> Vector6f;
 
+struct ConveyorBeltState {
+  bool ctrlModeConveyorBelt;
+  bool isDisturbTarget;
+  int modeConveyorBelt;
+  int nominalSpeedConveyorBelt;
+  int magniturePertConveyorBelt;
+};
+
 class RosDualArmCommunication {
 private:
   // ---- ROS
   ros::NodeHandle nh_;// Ros node handle
   ros::Rate loopRate_;// Ros loop rate [Hz]
+  double frequency_;
 
   bool isSimulation_ = true;
   std::vector<float> objectDimVect_;
@@ -93,10 +102,19 @@ class RosDualArmCommunication {
   // params update
   std::string dsDampingTopicParams_[NB_ROBOTS];
 
+  // ---- Conveyor belt control
+  bool ctrlModeConveyorBelt_;
+  bool isDisturbTarget_;
+  int modeConveyorBelt_;
+  int nominalSpeedConveyorBelt_;
+  int magniturePertConveyorBelt_;
+  int desSpeedConveyorBelt_;
+
 public:
   // For controllers
   float firstEigenPassiveDamping_[NB_ROBOTS];
-  Eigen::Vector3f eePose_[NB_ROBOTS], objectPose_, targetPose_, eeVelLin_[NB_ROBOTS], eeVelAng_[NB_ROBOTS], robotBasePos_[NB_ROBOTS];
+  Eigen::Vector3f eePose_[NB_ROBOTS], objectPose_, targetPose_, eeVelLin_[NB_ROBOTS], eeVelAng_[NB_ROBOTS],
+      robotBasePos_[NB_ROBOTS];
   Eigen::Vector4f eeOrientation_[NB_ROBOTS], objectOrientation_, targetOrientation_, robotBaseOrientation_[NB_ROBOTS];
   Vector7f jointPosition_[NB_ROBOTS], jointVelocity_[NB_ROBOTS], jointTorques_[NB_ROBOTS];
   Eigen::Matrix<float, 6, 1> robotWrench_[NB_ROBOTS];
@@ -105,7 +123,7 @@ class RosDualArmCommunication {
   // Robot ID: left or right
   enum Robot { LEFT = 0, RIGHT = 1 };
 
-  RosDualArmCommunication(ros::NodeHandle& n, double frequency) : nh_(n), loopRate_(frequency){};
+  RosDualArmCommunication(ros::NodeHandle& n, double frequency) : nh_(n), loopRate_(frequency), frequency_(frequency){};
 
   ~RosDualArmCommunication(){};
 
@@ -126,11 +144,9 @@ class RosDualArmCommunication {
     initDampingTopicCtrl();
     initRosSubscribers();
     initRosPublisher();
+    initConveyorBelt();
 
-    for (int k = 0; k < NB_ROBOTS; k++) {
-      firstEigenPassiveDamping_[k] = 1.0f;
-    }
-
+    for (int k = 0; k < NB_ROBOTS; k++) { firstEigenPassiveDamping_[k] = 1.0f; }
 
     if (nh_.ok()) {
       // Wait for poses being published
@@ -387,17 +403,6 @@ class RosDualArmCommunication {
     pubDistAttractorEE_[LEFT] = nh_.advertise<std_msgs::Float64>(topicDistAttractorEE[LEFT], 1);
     pubDistAttractorEE_[RIGHT] = nh_.advertise<std_msgs::Float64>(topicDistAttractorEE[RIGHT], 1);
 
-    // Conveyor Belt
-    std::string topicConveyorBeltMode, topicConveyorBeltSpeed;
-    while (!nh_.getParam(nh_.getNamespace() + "/conveyor_belt/desired_mode", topicConveyorBeltMode)) {
-      ROS_INFO("Waitinng for param: conveyor_belt/desired_mode ");
-    }
-    while (!nh_.getParam(nh_.getNamespace() + "/conveyor_belt/desired_speed", topicConveyorBeltSpeed)) {
-      ROS_INFO("Waitinng for param: conveyor_belt/desired_speed ");
-    }
-    pubConveyorBeltMode_ = nh_.advertise<std_msgs::Int32>(topicConveyorBeltMode, 1);
-    pubConveyorBeltSpeed_ = nh_.advertise<std_msgs::Int32>(topicConveyorBeltSpeed, 1);
-
     return true;
   }
 
@@ -521,11 +526,6 @@ class RosDualArmCommunication {
       msgFnormMoment.torque.z = -appliedWrench[k](5);
       pubAppliedFNormMoment_[k].publish(msgFnormMoment);
     }
-
-    // // Send speed command to the conveyor belt TODO
-    // std_msgs::Int32 speedMsgConveyorBelt;
-    // speedMsgConveyorBelt.data = desSpeedConveyorBelt_;
-    // if (ctrlModeConveyorBelt_ && (fmod(cycleCount_, 30) == 0)) { pubConveyorBeltSpeed_.publish(speedMsgConveyorBelt); }
   }
 
   bool initDampingTopicCtrl() {
@@ -568,6 +568,36 @@ class RosDualArmCommunication {
     return true;
   }
 
+  bool initConveyorBelt() {
+    // Publisher
+    std::string topicConveyorBeltMode, topicConveyorBeltSpeed;
+    while (!nh_.getParam("conveyor_belt/desired_mode", topicConveyorBeltMode)) {
+      ROS_INFO("Waitinng for param: conveyor_belt/desired_mode ");
+    }
+    while (!nh_.getParam("conveyor_belt/desired_speed", topicConveyorBeltSpeed)) {
+      ROS_INFO("Waitinng for param: conveyor_belt/desired_speed ");
+    }
+    pubConveyorBeltMode_ = nh_.advertise<std_msgs::Int32>(topicConveyorBeltMode, 1);
+    pubConveyorBeltSpeed_ = nh_.advertise<std_msgs::Int32>(topicConveyorBeltSpeed, 1);
+
+    // Params
+    while (!nh_.getParam("conveyor_belt/control_mode", ctrlModeConveyorBelt_)) {
+      ROS_INFO("Waitinng for param: conveyor_belt/control_mode ");
+    }
+    while (!nh_.getParam("conveyor_belt/nominal_speed", nominalSpeedConveyorBelt_)) {
+      ROS_INFO("Waitinng for param: conveyor_belt/nominal_speed");
+    }
+    while (!nh_.getParam("conveyor_belt/magnitude_perturbation", magniturePertConveyorBelt_)) {
+      ROS_INFO("Waitinng for param: conveyor_belt/magnitude_perturbation");
+    }
+    desSpeedConveyorBelt_ = nominalSpeedConveyorBelt_;
+
+    modeConveyorBelt_ = 0;
+    isDisturbTarget_ = false;
+
+    return true;
+  }
+
   // ---- Callback functions
 
   void objectPoseCallback(const geometry_msgs::Pose::ConstPtr& msg) {
@@ -639,4 +669,51 @@ class RosDualArmCommunication {
     firstEigenPassiveDamping_[RIGHT] = paramValues[0];
     if (firstEigenPassiveDamping_[RIGHT] < FLT_EPSILON) { firstEigenPassiveDamping_[RIGHT] = 150.0f; }
   }
+
+  // ---- Conveyor belt control
+  ConveyorBeltState getConveyorBeltStatus() {
+    ConveyorBeltState conveyorBeltState;
+    conveyorBeltState.ctrlModeConveyorBelt = ctrlModeConveyorBelt_;
+    conveyorBeltState.modeConveyorBelt = modeConveyorBelt_;
+
+    conveyorBeltState.nominalSpeedConveyorBelt = nominalSpeedConveyorBelt_;
+    conveyorBeltState.magniturePertConveyorBelt = magniturePertConveyorBelt_;
+    conveyorBeltState.isDisturbTarget = isDisturbTarget_;
+
+    return conveyorBeltState;
+  }
+
+  void updateConveyorBeltStatus(ConveyorBeltState conveyorBeltState) {
+    nominalSpeedConveyorBelt_ = conveyorBeltState.nominalSpeedConveyorBelt;
+    magniturePertConveyorBelt_ = conveyorBeltState.magniturePertConveyorBelt;
+    isDisturbTarget_ = conveyorBeltState.isDisturbTarget;
+
+    if (ctrlModeConveyorBelt_ && modeConveyorBelt_ != conveyorBeltState.modeConveyorBelt) {
+      modeConveyorBelt_ = conveyorBeltState.modeConveyorBelt;
+      sendConveyorBeltMode();
+    }
+  }
+
+  void sendConveyorBeltMode() {
+    std_msgs::Int32 modeMessage;
+    modeMessage.data = modeConveyorBelt_;
+    pubConveyorBeltMode_.publish(modeMessage);
+  }
+
+  void sendConveyorBeltSpeed(int cycleCount) {
+    // Compute conveyor belt speed command
+    float omegaPert = 2.f * M_PI / 1;
+    float deltaOmegaPert = 0.1f * (2.f * (float) std::rand() / RAND_MAX - 1.0f) * omegaPert;
+
+    if (ctrlModeConveyorBelt_) {
+      desSpeedConveyorBelt_ = (int) (nominalSpeedConveyorBelt_
+                                     + (int) isDisturbTarget_ * magniturePertConveyorBelt_
+                                         * sin((omegaPert + deltaOmegaPert) * (1 / frequency_) * cycleCount));
+
+      // Send speed command to the conveyor belt
+      std_msgs::Int32 speedMsgConveyorBelt;
+      speedMsgConveyorBelt.data = desSpeedConveyorBelt_;
+      if (ctrlModeConveyorBelt_ && (fmod(cycleCount, 30) == 0)) { pubConveyorBeltSpeed_.publish(speedMsgConveyorBelt); }
+    }
+  }
 };