Added end-effector to feet

nao_dcm_apps/nao_dcm_moveit_config/config/controllers_H21.yaml

@@ -32,8 +32,6 @@ controller_list:
       - LHipRoll
       - LHipPitch
       - LKneePitch
-      - LAnklePitch
-      - LAnkleRoll
   - name: nao_dcm/RightLeg_controller
     action_ns: follow_joint_trajectory
     type: FollowJointTrajectory
@@ -42,6 +40,18 @@ controller_list:
       - RHipRoll
       - RHipPitch
       - RKneePitch
+  - name: nao_dcm/LeftFoot_controller
+    action_ns: follow_joint_trajectory
+    type: FollowJointTrajectory
+    default: true
+    joints:
+      - LAnklePitch
+      - LAnkleRoll
+  - name: nao_dcm/RightFoot_controller
+    action_ns: follow_joint_trajectory
+    type: FollowJointTrajectory
+    default: true
+    joints:
       - RAnklePitch
       - RAnkleRoll
   - name: nao_dcm/Head_controller

nao_dcm_apps/nao_dcm_moveit_config/config/controllers_H25.yaml

@@ -46,8 +46,6 @@ controller_list:
       - LHipRoll
       - LHipPitch
       - LKneePitch
-      - LAnklePitch
-      - LAnkleRoll
   - name: nao_dcm/RightLeg_controller
     action_ns: follow_joint_trajectory
     type: FollowJointTrajectory
@@ -56,6 +54,18 @@ controller_list:
       - RHipRoll
       - RHipPitch
       - RKneePitch
+  - name: nao_dcm/LeftFoot_controller
+    action_ns: follow_joint_trajectory
+    type: FollowJointTrajectory
+    default: true
+    joints:
+      - LAnklePitch
+      - LAnkleRoll
+  - name: nao_dcm/RightFoot_controller
+    action_ns: follow_joint_trajectory
+    type: FollowJointTrajectory
+    default: true
+    joints:
       - RAnklePitch
       - RAnkleRoll
   - name: nao_dcm/Head_controller

nao_dcm_apps/nao_dcm_moveit_config/config/nao_robot_v4_H21.srdf

@@ -29,13 +29,17 @@
         <joint name="LHipRoll" />
         <joint name="LHipPitch" />
         <joint name="LKneePitch" />
-        <joint name="LAnklePitch" />
-        <joint name="LAnkleRoll" />
     </group>
     <group name="RightLeg">
         <joint name="RHipRoll" />
         <joint name="RHipPitch" />
         <joint name="RKneePitch" />
+    </group>
+    <group name="LeftFoot">
+        <joint name="LAnkleRoll" />
+        <joint name="LAnklePitch" />
+    </group>
+    <group name="RightFoot">
         <joint name="RAnklePitch" />
         <joint name="RAnkleRoll" />
     </group>
@@ -65,6 +69,9 @@
         <joint name="RAnklePitch" />
         <joint name="RAnkleRoll" />
     </group>
+    <!--END EFFECTOR: Purpose: Represent information about an end effector.-->
+    <end_effector name="right_foot_eef" parent_link="RKnee" group="RightFoot" parent_group="RightLeg" />
+    <end_effector name="left_foot_eef" parent_link="LKnee" group="LeftFoot" parent_group="LeftLeg" />
     <!--VIRTUAL JOINT: Purpose: this element defines a virtual joint between a robot link and an external frame of reference (considered fixed with respect to the robot)-->
     <virtual_joint name="virtual_joint" type="floating" parent_frame="odom" child_link="base_link" />
     <!--PASSIVE JOINT: Purpose: this element is used to mark joints that are not actuated-->

nao_dcm_apps/nao_dcm_moveit_config/config/nao_robot_v4_H25.srdf

@@ -29,13 +29,17 @@
         <joint name="LHipRoll" />
         <joint name="LHipPitch" />
         <joint name="LKneePitch" />
-        <joint name="LAnklePitch" />
-        <joint name="LAnkleRoll" />
     </group>
     <group name="RightLeg">
         <joint name="RHipRoll" />
         <joint name="RHipPitch" />
         <joint name="RKneePitch" />
+    </group>
+    <group name="LeftFoot">
+        <joint name="LAnkleRoll" />
+        <joint name="LAnklePitch" />
+    </group>
+    <group name="RightFoot">
         <joint name="RAnklePitch" />
         <joint name="RAnkleRoll" />
     </group>
@@ -96,6 +100,8 @@
     <!--END EFFECTOR: Purpose: Represent information about an end effector.-->
     <end_effector name="right_eef" parent_link="RElbowDummy" group="RightHand" parent_group="RightArm" />
     <end_effector name="left_eef" parent_link="LElbowDummy" group="LeftHand" parent_group="LeftArm" />
+    <end_effector name="right_foot_eef" parent_link="RKnee" group="RightFoot" parent_group="RightLeg" />
+    <end_effector name="left_foot_eef" parent_link="LKnee" group="LeftFoot" parent_group="LeftLeg" />
     <!--VIRTUAL JOINT: Purpose: this element defines a virtual joint between a robot link and an external frame of reference (considered fixed with respect to the robot)-->
     <virtual_joint name="virtual_joint" type="floating" parent_frame="odom" child_link="base_link" />
     <!--PASSIVE JOINT: Purpose: this element is used to mark joints that are not actuated-->

nao_dcm_common/nao_dcm_control/config/nao_trajectory_control.yaml

@@ -44,14 +44,20 @@ nao_dcm:
       - LHipRoll
       - LHipPitch
       - LKneePitch
-      - LAnklePitch
-      - LAnkleRoll
   RightLeg_controller:
     type: position_controllers/JointTrajectoryController
     joints:
       - RHipRoll
       - RHipPitch
       - RKneePitch
+  LeftFoot_controller:
+    type: position_controllers/JointTrajectoryController
+    joints:
+      - LAnklePitch
+      - LAnkleRoll
+  RightFoot_controller:
+    type: position_controllers/JointTrajectoryController
+    joints:
       - RAnklePitch
       - RAnkleRoll
   Head_controller:

nao_dcm_common/nao_dcm_control/config/nao_trajectory_control_H21.yaml

@@ -34,14 +34,20 @@ nao_dcm:
       - LHipRoll
       - LHipPitch
       - LKneePitch
-      - LAnklePitch
-      - LAnkleRoll
   RightLeg_controller:
     type: position_controllers/JointTrajectoryController
     joints:
       - RHipRoll
       - RHipPitch
       - RKneePitch
+  LeftFoot_controller:
+    type: position_controllers/JointTrajectoryController
+    joints:
+      - LAnklePitch
+      - LAnkleRoll
+  RightFoot_controller:
+    type: position_controllers/JointTrajectoryController
+    joints:
       - RAnklePitch
       - RAnkleRoll
   Head_controller:

nao_dcm_common/nao_dcm_control/launch/nao_dcm_trajectory_control_H21.launch

@@ -6,7 +6,7 @@
 
   <!-- load the controllers -->
   <node name="nao_trajectory_controller" pkg="controller_manager" type="spawner" respawn="false"
-    output="screen" ns="/nao_dcm" args="/nao_dcm/RightArm_controller /nao_dcm/LeftArm_controller /nao_dcm/Head_controller /nao_dcm/Pelvis_controller /nao_dcm/LeftLeg_controller /nao_dcm/RightLeg_controller /nao_dcm/joint_state_controller"/>
+    output="screen" ns="/nao_dcm" args="/nao_dcm/RightArm_controller /nao_dcm/LeftArm_controller /nao_dcm/Head_controller /nao_dcm/Pelvis_controller /nao_dcm/LeftLeg_controller /nao_dcm/RightLeg_controller /nao_dcm/LeftFoot_controller /nao_dcm/RightFoot_controller /nao_dcm/joint_state_controller"/>
 
   <!-- convert joint states to TF transforms for rviz, etc -->
   <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"

nao_dcm_common/nao_dcm_control/launch/nao_dcm_trajectory_control_H25.launch

@@ -6,7 +6,7 @@
 
   <!-- load the controllers -->
   <node name="nao_trajectory_controller" pkg="controller_manager" type="spawner" respawn="false"
-    output="screen" ns="/nao_dcm" args="/nao_dcm/RightArm_controller /nao_dcm/RightHand_controller /nao_dcm/LeftArm_controller /nao_dcm/LeftHand_controller /nao_dcm/Head_controller /nao_dcm/Pelvis_controller /nao_dcm/LeftLeg_controller /nao_dcm/RightLeg_controller /nao_dcm/joint_state_controller"/>
+    output="screen" ns="/nao_dcm" args="/nao_dcm/RightArm_controller /nao_dcm/RightHand_controller /nao_dcm/LeftArm_controller /nao_dcm/LeftHand_controller /nao_dcm/Head_controller /nao_dcm/Pelvis_controller /nao_dcm/LeftLeg_controller /nao_dcm/RightLeg_controller /nao_dcm/LeftFoot_controller /nao_dcm/RightFoot_controller  /nao_dcm/joint_state_controller"/>
 
   <!-- convert joint states to TF transforms for rviz, etc -->
   <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"