Refactoring ik controller

Author: evan-palmer

testing.py

@@ -7,58 +7,12 @@
 # Load the URDF model using a free-flyer as root joint
 model = pinocchio.buildModelFromUrdf("/home/ubuntu/ws_ros/src/auv_controllers/ik_solvers/examples/urdf/uvms.urdf", pinocchio.JointModelFreeFlyer())
 
-joint_names_to_lock = [
-    "alpha_rs1_130_joint",
-    "alpha_rs1_139_joint",
-    "alpha_axis_a",
-    "thruster1_joint",
-    "thruster2_joint",
-    "thruster3_joint",
-    "thruster4_joint",
-    "thruster5_joint",
-    "thruster6_joint",
-    "thruster7_joint",
-    "thruster8_joint"
-]
-
-# Convert the joint names to joint IDs
-joint_ids_to_lock = [
-    model.getJointId(name) for name in joint_names_to_lock if model.existJointName(name)
-]
+controlled_joints = ["universe", "root_joint", "alpha_axis_b", "alpha_axis_c", "alpha_axis_d", "alpha_axis_e"]
+locked_joints = [model.getJointId(model.names[idx]) for idx, joint in enumerate(model.joints) if model.names[idx] not in controlled_joints]
 
 q_ref = pinocchio.neutral(model)
-model = pinocchio.buildReducedModel(model, joint_ids_to_lock, q_ref)
+model = pinocchio.buildReducedModel(model, locked_joints, q_ref)
 data = model.createData()
 
-# rot = R.random()
-q = pinocchio.neutral(model)
-# q[:3] = np.random.rand(3)
-# q[3:7] = rot.as_quat().reshape(-1)
-
-for idx, joint in enumerate(model.joints):
-    joint_name = model.names[idx]
-    nq = joint.nq  # Number of configuration variables for this joint
-    print(f"{joint_name}: index in q = {joint.idx_q}, nq = {nq}")
-
-print(q)
-print(len(q))
-
-# Perform the forward kinematics over the kinematic tree
-pinocchio.forwardKinematics(model, data, q)
-pinocchio.updateFramePlacements(model, data)
-pinocchio.computeJointJacobians(model, data, q)
-
-# # Get the index of the base link
-joint_name = "alpha_axis_b"
-joint_id = model.getJointId(joint_name)
-joint_idx = model.joints[joint_id].idx_v
-
-frame_name = "alpha_tcp"
-frame_id = model.getFrameId(frame_name)
-print(f"Frame ID of {frame_name}: {frame_id}")
-
-J = pinocchio.getFrameJacobian(model, data, model.getFrameId(frame_name), pinocchio.ReferenceFrame.LOCAL_WORLD_ALIGNED)
-# J = pinocchio.getJointJacobian(model, data, joint_id, pinocchio.ReferenceFrame.LOCAL)
-
-print(f"Jacobian of {joint_name}:")
-print(J)
+for i, joint in enumerate(model.names):
+    print(model.getJointId(joint), joint)

whole_body_controllers/include/whole_body_controllers/ik_controller.hpp

@@ -86,28 +86,25 @@ class IKController : public controller_interface::ChainableControllerInterface
   // that publish the vehicle state without the manipulator states
   std::shared_ptr<rclcpp::Subscription<nav_msgs::msg::Odometry>> vehicle_state_sub_;
   realtime_tools::RealtimeBuffer<nav_msgs::msg::Odometry> vehicle_state_;
-  std::vector<double> system_state_values_;
-
-  std::shared_ptr<rclcpp::Subscription<std_msgs::msg::String>> robot_description_sub_;
+  std::vector<double> position_state_values_, velocity_state_values_;
 
   std::unique_ptr<pluginlib::ClassLoader<ik_solvers::IKSolver>> loader_;
   std::shared_ptr<ik_solvers::IKSolver> solver_;
 
   std::unique_ptr<ik_controller::ParamListener> param_listener_;
   ik_controller::Params params_;
 
-  bool model_initialized_{false};
-
-  std::vector<std::string> pos_dofs_, vel_dofs_, manipulator_dofs_;
-  std::size_t n_pos_dofs_, n_vel_dofs_, n_manipulator_dofs_;
-
   // make the free-flyer position and velocity dof names "static"
   std::vector<std::string> free_flyer_pos_dofs_{"x", "y", "z", "qx", "qy", "qz", "qw"};
   std::vector<std::string> free_flyer_vel_dofs_{"x", "y", "z", "rx", "ry", "rz"};
 
-  // keep track of the command interfaces
-  bool has_position_interface_{false}, has_velocity_interface_{false};
-  std::size_t n_command_interfaces_{0};
+  // store the names of the position and velocity interfaces
+  // this is stored in the same order as the pinocchio model to simplify integration with the solver
+  std::vector<std::string> position_interface_names_, velocity_interface_names_;
+
+  // track the interfaces used by the controller
+  bool use_position_commands_, use_velocity_commands_, use_position_states_, use_velocity_states_;
+  std::size_t n_command_interfaces_, n_state_interfaces_;
 
   rclcpp::Logger logger_{rclcpp::get_logger("ik_controller")};
 };