Final changes

README.md

@@ -28,7 +28,7 @@ cd .. && catkin build
 ## Usage
 It is preferred to have the robot and the system running ROS on the same network and connected via ethernet (especially important for torque control). For safety purposes, **make sure to have the e-stop next to you when testing.**
 
-To ensure correct biasing of the robot's torque sensors, position the robot to the candlestick configuration with all joint positions set to zero. Then, zero out the actuator torque offset for each joint using the web application. For testing compliant control, return the robot to a general position like HOME; avoid testing at the candlestick position due to potential instability.
+To ensure correct biasing of the robot's torque sensors, position the robot to the candlestick configuration with all joint positions set to zero (we recommend using the Kinova web app for this part). Then, zero out the actuator torque offset for each joint using the web application. For testing compliant control, return the robot to a general position like HOME; avoid testing at the candlestick position due to potential instability.
 
 Running instructions for testing the controllers:
 ```

config/JointSpaceParams.cfg

@@ -60,4 +60,4 @@ lp.add("lp_4", double_t, 0, "Friction observer derivative gain for joint 4",
 lp.add("lp_5", double_t, 0, "Friction observer derivative gain for joint 5",    4, 1,   30)
 lp.add("lp_6", double_t, 0, "Friction observer derivative gain for joint 6",    4, 1,   30)
 
-exit(gen.generate(PACKAGE, "joint_space_compliance", "JointSpaceCompliantController"))
+exit(gen.generate(PACKAGE, "joint_space_compliance", "JointSpaceCompliantController"))

config/TaskSpaceParams.cfg

@@ -58,4 +58,4 @@ lp.add("lp_4", double_t, 0, "Friction observer derivative gain for joint 4",
 lp.add("lp_5", double_t, 0, "Friction observer derivative gain for joint 5",    4, 1,   30)
 lp.add("lp_6", double_t, 0, "Friction observer derivative gain for joint 6",    4, 1,   30)
 
-exit(gen.generate(PACKAGE, "task_space_compliance", "TaskSpaceCompliantController"))
+exit(gen.generate(PACKAGE, "task_space_compliance", "TaskSpaceCompliantController"))

config/gen3_6dof_compliant_controllers.yaml

@@ -84,4 +84,4 @@ task_space_compliant_controller:
 joint_space_compliant_controller:
   type: gen3_compliant_controllers/JointSpaceCompliantController
   joints: [joint_1, joint_2, joint_3,
-    joint_4, joint_5, joint_6]
+    joint_4, joint_5, joint_6]

launch/controller.launch

@@ -66,4 +66,4 @@
 
   <!-- <rosparam file="$(find gen3_compliant_controllers)/config/config.yaml"/> -->
 
-</launch>
+</launch>

scripts/test_controllers.py

@@ -29,7 +29,7 @@
     tfBuffer = tf2_ros.Buffer()
     listener = tf2_ros.TransformListener(tfBuffer)
 
-    print("Switching to ", controller_space, " space controller. The robot might jerk a bit as it is switching to effort mode.")
+    print("Switching to", controller_space, "space controller. The robot might jerk a bit as it is switching to effort mode.")
     rospy.wait_for_service("controller_manager/switch_controller")
     rospy.wait_for_service("set_control_mode")
     switch_controller = rospy.ServiceProxy(

src/JointSpaceCompliantController.cpp

@@ -240,7 +240,7 @@ void JointSpaceCompliantController::update(const ros::Time& time, const ros::Dur
     if (command.positions.size() != mNumControlledDofs || command.velocities.size() != mNumControlledDofs)
     {
       // assume last command
-      ROS_WARN_STREAM("Received command with wrong size. Assuming last command.");
+      ROS_WARN_STREAM("Received no command or command with wrong size. Assuming current position as goal.");
     }
     else
     {
@@ -262,7 +262,6 @@ void JointSpaceCompliantController::update(const ros::Time& time, const ros::Dur
   if (mDesiredPosition != mLastDesiredPosition && mCurrentPosition != mDesiredPosition)
   {
     mLastDesiredPosition = mDesiredPosition;
-    mExtendedJoints->estimateExtendedJoint(mDesiredPosition);
     mDesiredPosition = mExtendedJoints->getExtendedJoint();
   }
 
@@ -342,4 +341,4 @@ bool JointSpaceCompliantController::shouldStopExecution(std::string& message)
 } // namespace gen3_compliant_controllers
 
 //=============================================================================
-PLUGINLIB_EXPORT_CLASS(gen3_compliant_controllers::JointSpaceCompliantController, controller_interface::ControllerBase)
+PLUGINLIB_EXPORT_CLASS(gen3_compliant_controllers::JointSpaceCompliantController, controller_interface::ControllerBase)

src/TaskSpaceCompliantController.cpp

@@ -256,7 +256,6 @@ void TaskSpaceCompliantController::update(const ros::Time& time, const ros::Dura
   }
   else
   {
-    std::cout << "Trying to read from buffer ..." << std::endl;
     moveit_msgs::CartesianTrajectoryPoint& command = *mCommandsBuffer.readFromRT();
     if (command.point.pose.position.x == 0.0 && command.point.pose.position.y == 0.0 && command.point.pose.position.z == 0.0)
     {
@@ -269,7 +268,6 @@ void TaskSpaceCompliantController::update(const ros::Time& time, const ros::Dura
       auto tmp = mDesiredEETransform.linear();
       tmp.col(0) = -tmp.col(0);
       tmp.col(1) = -tmp.col(1);
-      std::cout << "Received command: " << std::endl << mDesiredEETransform.translation() << std::endl;
     }
   }
 
@@ -290,7 +288,6 @@ void TaskSpaceCompliantController::update(const ros::Time& time, const ros::Dura
   {
     mLastDesiredPosition = mDesiredPosition;
     mLastDesiredEETransform = mDesiredEETransform;
-    mExtendedJoints->estimateExtendedJoint(mDesiredPosition);
     mTrueDesiredPosition = mExtendedJoints->getExtendedJoint();
     mTrueDesiredVelocity = mDesiredVelocity;
     mTrueDesiredEETransform = mDesiredEETransform;
@@ -429,4 +426,4 @@ bool TaskSpaceCompliantController::shouldStopExecution(std::string& message)
 } // namespace gen3_compliant_controllers
 
 //=============================================================================
-PLUGINLIB_EXPORT_CLASS(gen3_compliant_controllers::TaskSpaceCompliantController, controller_interface::ControllerBase)
+PLUGINLIB_EXPORT_CLASS(gen3_compliant_controllers::TaskSpaceCompliantController, controller_interface::ControllerBase)