minor fixes

Author: Thibault Poignonec

cartesian_vic_controller/include/cartesian_vic_controller/cartesian_vic_state.hpp

@@ -58,7 +58,10 @@ class VicInputData
     nullspace_joint_stiffness = Eigen::VectorXd::Zero(num_joints);
     nullspace_joint_damping = Eigen::VectorXd::Zero(num_joints);
 
-    // Allocate cartesian state
+    // Reset cartesian state
+    robot_current_velocity.setZero();
+    robot_estimated_acceleration.setZero();
+    robot_current_wrench_at_ft_frame_.setZero();
     /*
     natural_joint_space_inertia = \
       Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic>::Zero(num_joints, num_joints);
@@ -150,6 +153,7 @@ class VicInputData
   // Cartesian state (control frame w.r.t. robot base frame)
   Eigen::Isometry3d robot_current_pose;
   Eigen::Matrix<double, 6, 1> robot_current_velocity;
+  Eigen::Matrix<double, 6, 1> robot_estimated_acceleration;
 
   /// Natural inertia matrix (from dynamic model)
   // Eigen::Matrix<double, 6, 6> natural_cartesian_inertia;

cartesian_vic_controller/src/cartesian_vic_state.cpp

@@ -71,6 +71,7 @@ VicState::to_msg(cartesian_control_msgs::msg::VicControllerState & vic_state_msg
   if (input_data.has_ft_sensor()) {
     vic_state_msg.wrench = WrenchToMsg(input_data.get_ft_sensor_wrench());
   } else {
+    // TODO(tpoignonec): add flag to say if the ft sensor is available!
     vic_state_msg.wrench.force.x = 0.0;
     vic_state_msg.wrench.force.y = 0.0;
     vic_state_msg.wrench.force.z = 0.0;