Merge pull request #8 from iory/wheel

Enable wheeled kxr

ros/kxr_controller/include/kxr_controller/kxr_robot_hardware.h

@@ -34,15 +34,18 @@ namespace kxr_controller {
 
     hardware_interface::JointStateInterface joint_state_interface;
     hardware_interface::PositionJointInterface joint_position_interface;
+    hardware_interface::VelocityJointInterface joint_velocity_interface;
 
     ros::Duration control_loop_period_;
     bool joint_state_received_;
     std::map<std::string, unsigned int> jointname_to_id_;
     ros::Subscriber joint_state_sub_;
     ros::Publisher joint_command_pub_;
+    ros::Publisher joint_velocity_command_pub_;
     sensor_msgs::JointState current_joint_state_;
     sensor_msgs::JointState command_joint_state_;
     std::vector<double> joint_position_command_;
+    std::vector<double> joint_velocity_command_;
     std::vector<double> joint_state_position_;
     std::vector<double> joint_state_velocity_;
     std::vector<double> joint_state_effort_;

ros/kxr_controller/launch/diff_drive_controller.launch

@@ -0,0 +1,10 @@
+<launch>
+
+  <rosparam command="load" file="$(find kxr_models)/config/kxrmw4a6h2m_diff_drive_controller.yaml" />
+
+  <node name="controller_spawner"
+        pkg="controller_manager" type="spawner"
+        respawn="false" output="screen"
+        args="diff_drive_controller" />
+
+</launch>

ros/kxr_controller/launch/kxr_controller.launch

@@ -1,7 +1,7 @@
 <launch>
 
-  <arg name="urdf_path" value="$(find kxr_models)/urdf/kxrl2l2a6h2m.urdf" />
-  <arg name="servo_config_path" value="$(find kxr_models)/config/kxrl2l2a6h2m_servo_config.yaml" />
+  <arg name="urdf_path" default="$(find kxr_models)/urdf/kxrl2l2a6h2m.urdf" />
+  <arg name="servo_config_path" default="$(find kxr_models)/config/kxrl2l2a6h2m_servo_config.yaml" />
   <arg name="namespace" default="" />
 
   <group if="$(eval len(arg('namespace')) > 0)" ns="$(arg namespace)" >

ros/kxr_controller/launch/kxr_controller_for_wheel.launch

@@ -0,0 +1,13 @@
+<launch>
+
+  <arg name="urdf_path" default="$(find kxr_models)/urdf/kxrmw4a6h2m.urdf" />
+  <arg name="servo_config_path" default="$(find kxr_models)/config/kxrmw4a6h2m_servo_config.yaml" />
+
+  <include file="$(find kxr_controller)/launch/kxr_controller.launch" >
+    <arg name="urdf_path" value="$(arg urdf_path)" />
+    <arg name="servo_config_path" value="$(arg servo_config_path)" />
+  </include>
+
+  <include file="$(find kxr_controller)/launch/diff_drive_controller.launch" />
+
+</launch>

ros/kxr_controller/node_scripts/rcb4_ros_bridge.py

@@ -40,7 +40,14 @@ def load_yaml(file_path, Loader=yaml.SafeLoader):
         raise OSError('{} not exists'.format(str(file_path)))
     with open(osp.expanduser(file_path), 'r') as f:
         data = yaml.load(f, Loader=Loader)
-    return data
+    data = data['joint_name_to_servo_id']
+    joint_name_to_id = {}
+    for name in data:
+        if isinstance(data[name], int):
+            joint_name_to_id[name] = data[name]
+        else:
+            joint_name_to_id[name] = data[name]['id']
+    return joint_name_to_id, data
 
 
 def run_robot_state_publisher(namespace=None):
@@ -85,8 +92,7 @@ def __init__(self):
                 r.load_urdf_file(f)
 
         servo_config_path = rospy.get_param('~servo_config_path')
-        self.joint_name_to_id = load_yaml(servo_config_path)[
-            'joint_name_to_servo_id']
+        self.joint_name_to_id, servo_infos = load_yaml(servo_config_path)
 
         joint_list = [j for j in r.joint_list
                       if j.__class__.__name__ != 'FixedJoint']
@@ -100,7 +106,6 @@ def __init__(self):
         set_robot_description(
             urdf_path,
             param_name=clean_namespace + '/robot_description')
-        set_fullbody_controller(self.joint_names)
         set_joint_state_controler()
         self.current_joint_states_pub = rospy.Publisher(
             clean_namespace + '/current_joint_states',
@@ -115,8 +120,34 @@ def __init__(self):
         arm.search_wheel_sids()
         arm.all_jointbase_sensors()
         arm.send_stretch(40)
-
         self.id_to_index = self.arm.servo_id_to_index()
+        self._prev_velocity_command = None
+
+        print(arm.joint_to_actuator_matrix)
+        print(arm._actuator_to_joint_matrix)
+        for _, info in servo_infos.items():
+            if isinstance(info, int):
+                continue
+            servo_id = info['id']
+            direction = info['direction']
+            idx = self.id_to_index[servo_id]
+            arm._joint_to_actuator_matrix[idx, idx] = \
+                direction * arm._joint_to_actuator_matrix[idx, idx]
+        print(arm.joint_to_actuator_matrix)
+
+        self.fullbody_jointnames = []
+        for jn in self.joint_names:
+            if jn not in self.joint_name_to_id:
+                continue
+            servo_id = self.joint_name_to_id[jn]
+            if servo_id in arm.wheel_servo_sorted_ids:
+                continue
+            self.fullbody_jointnames.append(jn)
+        set_fullbody_controller(self.fullbody_jointnames)
+        # set_fullbody_controller(self.joint_names)
+        print('Fullbody jointnames')
+        print(self.fullbody_jointnames)
+
         self.servo_id_to_worm_id = self.arm.servo_id_to_worm_id
 
         self.joint_servo_on = {jn: False for jn in self.joint_names}
@@ -139,6 +170,11 @@ def __init__(self):
             JointState, queue_size=1,
             callback=self.command_joint_state_callback)
 
+        self.velocity_command_joint_state_sub = rospy.Subscriber(
+            clean_namespace + '/velocity_command_joint_state',
+            JointState, queue_size=1,
+            callback=self.velocity_command_joint_state_callback)
+
         self.servo_on_off_server = actionlib.SimpleActionServer(
             clean_namespace
             + '/kxr_fullbody_controller/servo_on_off_real_interface',
@@ -154,6 +190,51 @@ def __init__(self):
         self.proc_robot_state_publisher = run_robot_state_publisher(
             clean_namespace)
 
+    def velocity_command_joint_state_callback(self, msg):
+        servo_ids = []
+        av_length = len(self.arm.servo_sorted_ids)
+        svs = np.zeros(av_length)
+        valid_indices = []
+        for name, angle in zip(msg.name, msg.position):
+            if name not in self.joint_name_to_id:
+                continue
+            if name in self.joint_servo_on \
+               and self.joint_servo_on[name] is False:
+                continue
+            idx = self.joint_name_to_id[name]
+
+            # should ignore duplicated index.
+            if idx not in self.id_to_index:
+                continue
+            # skip wheel joint
+            if idx not in self.arm.wheel_servo_sorted_ids:
+                continue
+            if self.id_to_index[idx] in valid_indices:
+                continue
+
+            angle = np.rad2deg(angle)
+            svs[self.id_to_index[idx]] = angle
+            valid_indices.append(self.id_to_index[idx])
+            servo_ids.append(idx)
+        tmp_av = np.ones(av_length + 1)
+        if len(valid_indices) == 0:
+            return
+        try:
+            tmp_av[:len(svs)] = np.array(svs)
+        except Exception:
+            return
+        svs = np.matmul(self.arm.joint_to_actuator_matrix, tmp_av)[
+            np.array(valid_indices)]
+        indices = np.argsort(servo_ids)
+        svs = svs[indices]
+        servo_ids = np.array(servo_ids)[indices]
+        if self._prev_velocity_command is not None and np.allclose(
+                self._prev_velocity_command, svs):
+            return
+        self._prev_velocity_command = svs
+        with self.lock:
+            self.arm.servo_angle_vector(servo_ids, svs, velocity=0)
+
     def command_joint_state_callback(self, msg):
         servo_ids = []
         av_length = len(self.arm.servo_sorted_ids)
@@ -172,6 +253,9 @@ def command_joint_state_callback(self, msg):
             # should ignore duplicated index.
             if idx not in self.id_to_index:
                 continue
+            # skip wheel joint
+            if idx in self.arm.wheel_servo_sorted_ids:
+                continue
             if self.id_to_index[idx] in valid_indices:
                 continue
 

ros/kxr_controller/package.xml

@@ -26,6 +26,7 @@
   <exec_depend>joint_trajectory_controller</exec_depend>
   <exec_depend>position_controllers</exec_depend>
   <exec_depend>angles</exec_depend>
+  <exec_depend>diff_drive_controller</exec_depend>
   <exec_depend>urdf</exec_depend>
   <exec_depend>cmake_modules</exec_depend>
   <exec_depend>kxr_models</exec_depend>

ros/kxr_controller/src/kxr_robot_hardware.cpp

@@ -42,6 +42,7 @@ namespace kxr_controller {
     control_loop_period_ = ros::Duration(1.0 / control_loop_rate);
 
     joint_position_command_.resize(model.joints_.size());
+    joint_velocity_command_.resize(model.joints_.size());
     joint_state_position_.resize(model.joints_.size());
     joint_state_velocity_.resize(model.joints_.size());
     joint_state_effort_.resize(model.joints_.size());
@@ -60,14 +61,19 @@ namespace kxr_controller {
       joint_state_interface.registerHandle(state_handle);
       hardware_interface::JointHandle pos_handle(joint_state_interface.getHandle(jointname), &joint_position_command_[i]);
       joint_position_interface.registerHandle(pos_handle);
+      hardware_interface::JointHandle vel_handle(joint_state_interface.getHandle(jointname), &joint_velocity_command_[i]);
+      joint_velocity_interface.registerHandle(vel_handle);
+
       ++i;
     }
 
     registerInterface(&joint_state_interface);
     registerInterface(&joint_position_interface);
+    registerInterface(&joint_velocity_interface);
 
     joint_state_sub_ = nh_.subscribe<sensor_msgs::JointState>(clean_namespace + "/current_joint_states", 1, &KXRRobotHW::jointStateCallback, this);
     joint_command_pub_ = nh_.advertise<sensor_msgs::JointState>(clean_namespace + "/command_joint_state", 1);
+    joint_velocity_command_pub_ = nh_.advertise<sensor_msgs::JointState>(clean_namespace + "/velocity_command_joint_state", 1);
 
     ROS_INFO("Waiting for action server to start.");
     servo_on_off_action_server_ = std::make_shared<ServoOnOffActionServer>(robot_hw_nh, clean_namespace + "/kxr_fullbody_controller/servo_on_off",
@@ -99,14 +105,20 @@ namespace kxr_controller {
   {
     boost::mutex::scoped_lock lock(mutex_);
     sensor_msgs::JointState joint_state_msg;
+    sensor_msgs::JointState joint_velocity_state_msg;
     joint_state_msg.header.stamp = getTime();
+    joint_velocity_state_msg.header.stamp = getTime();
     for (const auto& pair : jointname_to_id_) {
       const std::string& joint_name = pair.first;
       unsigned int joint_id = pair.second;
       joint_state_msg.position.push_back(joint_position_command_[joint_id]);
       joint_state_msg.name.push_back(joint_name);
+
+      joint_velocity_state_msg.position.push_back(joint_velocity_command_[joint_id]);
+      joint_velocity_state_msg.name.push_back(joint_name);
     }
     joint_command_pub_.publish(joint_state_msg);
+    joint_velocity_command_pub_.publish(joint_velocity_state_msg);
   }
 
   void KXRRobotHW::jointStateCallback(const sensor_msgs::JointState::ConstPtr& msg)

ros/kxr_models/config/kxrmw4a6h2m_diff_drive_controller.yaml

@@ -0,0 +1,32 @@
+diff_drive_controller:
+  type: "diff_drive_controller/DiffDriveController"
+  left_wheel: ['LLEG_JOINT0', 'LLEG_JOINT1']
+  right_wheel: ['RLEG_JOINT0', 'RLEG_JOINT1']
+  publish_rate: 50
+  pose_covariance_diagonal: [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 0.03]
+  twist_covariance_diagonal: [0.001, 0.001, 0.001, 1000000.0, 1000000.0, 0.03]
+  cmd_vel_timeout: 0.25
+
+  # Odometry fused with IMU is published by robot_localization, so
+  # no need to publish a TF based on encoders alone.
+  enable_odom_tf: true
+  base_frame_id: bodyset94855827795112
+
+  # Wheel separation and radius multipliers
+  wheel_separation_multiplier: 1.5 # default: 1.0
+  wheel_radius_multiplier    : 1.0 # default: 1.0
+
+  # Velocity and acceleration limits
+  # Whenever a min_* is unspecified, default to -max_*
+  linear:
+    x:
+      has_velocity_limits    : true
+      max_velocity           : 0.1   # m/s
+      has_acceleration_limits: true
+      max_acceleration       : 20.0   # m/s^2
+  angular:
+    z:
+      has_velocity_limits    : true
+      max_velocity           : 4.0   # rad/s
+      has_acceleration_limits: true
+      max_acceleration       : 25.0   # rad/s^2

ros/kxr_models/config/kxrmw4a6h2m_servo_config.yaml

@@ -0,0 +1,24 @@
+joint_name_to_servo_id: {
+  HEAD_JOINT0: 32,
+  HEAD_JOINT1: 34,
+  LARM_JOINT0: 3,
+  LARM_JOINT1: 5,
+  LARM_JOINT2: 23,
+  LARM_JOINT3: 7,
+  LARM_JOINT4: 27,
+  LARM_JOINT5: 29,
+  LARM_JOINT6: 19,
+  LARM_JOINT7: 19,
+  LLEG_JOINT0: {'id': 25, 'direction': -1},
+  LLEG_JOINT1: {'id': 17, 'direction': -1},
+  RARM_JOINT0: 2,
+  RARM_JOINT1: 4,
+  RARM_JOINT2: 22,
+  RARM_JOINT3: 6,
+  RARM_JOINT4: 26,
+  RARM_JOINT5: 28,
+  RARM_JOINT6: 18,
+  RARM_JOINT7: 18,
+  RLEG_JOINT0: {'id': 24, 'direction': 1},
+  RLEG_JOINT1: {'id': 16, 'direction': 1}
+}