minor changes

config/TaskSpaceParams.cfg

@@ -33,12 +33,12 @@ kp.add("k_pitch", double_t, 0, "Stiffness Gain for Rotation about Y",   100, 10,
 kp.add("k_yaw", double_t, 0, "Stiffness Gain for Rotation about Z",     100, 10,   250)
 
 kd = gen.add_group("Kd") # damping gain
-kd.add("d_x", double_t, 0, "Damping Gain for Translation along X",    40, 1,   40)
-kd.add("d_y", double_t, 0, "Damping Gain for Translation along Y",    40, 1,   40)
-kd.add("d_z", double_t, 0, "Damping Gain for Translation along Z",    40, 1,   40)
-kd.add("d_roll", double_t, 0, "Damping Gain for Rotation about X",    20, 1,   40)
-kd.add("d_pitch", double_t, 0, "Damping Gain for Rotation about Y",   20, 1,   40)
-kd.add("d_yaw", double_t, 0, "Damping Gain for Rotation about Z",     20, 1,   40)
+kd.add("d_x", double_t, 0, "Damping Gain for Translation along X",    40, 1,   80)
+kd.add("d_y", double_t, 0, "Damping Gain for Translation along Y",    40, 1,   80)
+kd.add("d_z", double_t, 0, "Damping Gain for Translation along Z",    40, 1,   80)
+kd.add("d_roll", double_t, 0, "Damping Gain for Rotation about X",    20, 1,   60)
+kd.add("d_pitch", double_t, 0, "Damping Gain for Rotation about Y",   20, 1,   60)
+kd.add("d_yaw", double_t, 0, "Damping Gain for Rotation about Z",     20, 1,   60)
 
 l = gen.add_group("L") # friction observer proportional gain
 l.add("l_0", double_t, 0, "Friction observer propotional gain for joint 0",    75, 20,   200)

include/gen3_compliant_controllers/TaskSpaceCompliantController.hpp

@@ -150,6 +150,7 @@ class TaskSpaceCompliantController
   Eigen::Isometry3d mDesiredEETransform;
   Eigen::Isometry3d mNominalEETransform;
   Eigen::Isometry3d mTrueDesiredEETransform;
+  Eigen::Isometry3d mLastDesiredEETransform;
 
   ros::Subscriber mSubCommand;
   void commandCallback(

include/gen3_compliant_controllers/helpers.hpp

@@ -58,7 +58,6 @@ class ExtendedJointPosition
   Eigen::VectorXd mPreviousSensorQ;
 
 public:
-  // Set to 7 and 3PI/2
   ExtendedJointPosition(
       unsigned int number_of_input_args, double threshold_of_change_args);
 

launch/controller.launch

@@ -64,6 +64,6 @@
 
   <node pkg="robot_state_publisher" type="robot_state_publisher" name="rob_st_pub" />
 
-  <rosparam file="$(find gen3_compliant_controllers)/config/config.yaml"/>
+  <!-- <rosparam file="$(find gen3_compliant_controllers)/config/config.yaml"/> -->
 
 </launch>

scripts/test_controllers.py

@@ -13,6 +13,7 @@
 from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
 
 if __name__ == "__main__":
+    rospy.init_node('test_controller_py')
     # get command line arguments for control mode
     controller_space = sys.argv[1]
     mode = "effort"
@@ -26,7 +27,7 @@
         sys.exit(0)
 
     tfBuffer = tf2_ros.Buffer()
-    # listener = tf2_ros.TransformListener(tfBuffer)
+    listener = tf2_ros.TransformListener(tfBuffer)
 
     rospy.wait_for_service("controller_manager/switch_controller")
     rospy.wait_for_service("set_control_mode")
@@ -65,26 +66,27 @@
         sys.exit(0)
 
     try:
-        rospy.init_node("jointgroup_test_py")
-        cmd_pub = (
-            rospy.Publisher(topic, CartesianTrajectoryPoint, queue_size=1)
-            if mode == "task"
-            else rospy.Publisher(topic, JointTrajectoryPoint, queue_size=1)
-        )
+        cmd_pub = None
+        if controller_space == "task":
+            cmd_pub = rospy.Publisher(topic, CartesianTrajectoryPoint, queue_size=1)
+        else:
+            cmd_pub = rospy.Publisher(topic, JointTrajectoryPoint, queue_size=1)
+        rate = rospy.Rate(100.0)
 
         while not rospy.is_shutdown():
             # wait for message from joint_states
             cmd = None
             if controller_space == "task":
                 cmd = CartesianTrajectoryPoint()
                 # get current end effector position using tf
-                while not tfBuffer.can_transform(
-                    "end_effector_link", "base_link", rospy.Time(0)
-                ):
-                    continue
-                transform = tfBuffer.lookup_transform(
-                    "end_effector_link", "base_link", rospy.Time(1)
-                )
+                transform = None
+                while True:
+                    try:
+                        transform = tfBuffer.lookup_transform('base_link', 'end_effector_link', rospy.Time())
+                        break
+                    except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException):
+                        rate.sleep()
+                        continue
                 print(
                     "Current end effector position: ", transform.transform.translation
                 )
@@ -107,13 +109,10 @@
                 dof = int(input("Enter dof (1 - NDOF): "))
                 value = float(input("Enter value: "))
                 tmp = np.array(joint_states.position)[1:]
-                print ("Tmp is ", tmp)
                 tmp[dof - 1] = value
                 cmd.positions = tmp.tolist()
                 cmd.velocities = [0] * len(tmp)
-
-            for i in range(100):
-                cmd_pub.publish(cmd)
+            cmd_pub.publish(cmd)
             input("Press Enter to continue...")
 
     except rospy.ROSInterruptException:

src/TaskSpaceCompliantController.cpp

@@ -281,6 +281,16 @@ void TaskSpaceCompliantController::update(
   {
     mDesiredPosition = mActualPosition;
     mDesiredVelocity.setZero();
+    Eigen::VectorXd q_pin_desired
+        = joint_ros_to_pinocchio(mDesiredPosition, *mModel);
+    pinocchio::forwardKinematics(*mModel, *mData, q_pin_desired, mZeros);
+    pinocchio::updateFramePlacement(*mModel, *mData, mEENode);
+    mDesiredEETransform = mData->oMf[mEENode].toHomogeneousMatrix_impl();
+    auto tmp = mDesiredEETransform.linear();
+    // make first and second column negative to account for axis convention
+    tmp.col(0) = -tmp.col(0);
+    tmp.col(1) = -tmp.col(1);
+    mExecuteDefaultCommand = false;
   }
   else
   {
@@ -298,6 +308,9 @@ void TaskSpaceCompliantController::update(
     {
       geometry_msgs::Pose command_pose = command.point.pose;
       tf::poseMsgToEigen(command_pose, mDesiredEETransform);
+      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;
     }
@@ -306,8 +319,9 @@ void TaskSpaceCompliantController::update(
   if (!mExtendedJoints->mIsInitialized)
   {
     mLastDesiredPosition = mDesiredPosition;
-    mExtendedJoints->initializeExtendedJointPosition(mActualPosition);
-    mExtendedJoints->estimateExtendedJoint(mActualPosition);
+    mLastDesiredEETransform = mDesiredEETransform;
+    mExtendedJoints->initializeExtendedJointPosition(mDesiredPosition);
+    mExtendedJoints->estimateExtendedJoint(mDesiredPosition);
     mNominalThetaPrev = mExtendedJoints->getExtendedJoint();
     mNominalThetaDotPrev = mActualVelocity;
     mTrueDesiredPosition = mExtendedJoints->getExtendedJoint();
@@ -316,9 +330,11 @@ void TaskSpaceCompliantController::update(
   }
 
   if (mDesiredPosition != mLastDesiredPosition
-      && mActualPosition != mDesiredPosition)
+      || !mDesiredEETransform.isApprox(mLastDesiredEETransform, 0.0001)
+             && mActualPosition != mDesiredPosition)
   {
     mLastDesiredPosition = mDesiredPosition;
+    mLastDesiredEETransform = mDesiredEETransform;
     mExtendedJoints->estimateExtendedJoint(mDesiredPosition);
     mTrueDesiredPosition = mExtendedJoints->getExtendedJoint();
     mTrueDesiredVelocity = mDesiredVelocity;
@@ -369,15 +385,6 @@ void TaskSpaceCompliantController::update(
   Eigen::VectorXd dart_error(6);
   Eigen::MatrixXd dart_nominal_jacobian(6, mNumControlledDofs);
   {
-    Eigen::VectorXd q_pin_true_desired
-        = joint_ros_to_pinocchio(mTrueDesiredPosition, *mModel);
-    pinocchio::forwardKinematics(*mModel, *mData, q_pin_true_desired, mZeros);
-    pinocchio::updateFramePlacement(*mModel, *mData, mEENode);
-    mTrueDesiredEETransform = mData->oMf[mEENode].toHomogeneousMatrix_impl();
-    auto tmp = mTrueDesiredEETransform.linear();
-    // make first and second column negative to account for axis convention
-    tmp.col(0) = -tmp.col(0);
-    tmp.col(1) = -tmp.col(1);
     Eigen::Quaterniond ee_quat_d(mTrueDesiredEETransform.linear());
 
     Eigen::VectorXd q_pin_nominal_prev