在凉亭里走一个LLE机器人

Question

我想在凉亭中模拟下肢外骨骼（LLE）。我试图让它在凉亭中移动（迈出一步）。但它没有迈出一步。有趣的是，我在终端中看不到任何错误。我使用XML（xacro格式）来描述机器人，启动控制和凉亭的文件以及步行的python。 这是我的代码： 1.xacro文件：（lle_sample4.xacro）

  <robot name="lle_sample">
  <xacro:include filename="$(find lle_sample_description)/urdf  /materials.xacro"/>
  <xacro:include filename="$(find lle_sample_description)/urdf/lle_sample2.gazebo"/>
 <link name="world"/>
 <joint name="joint_base" type="fixed">
 <parent link="world"/>
 <child link="base_link"/>
 </joint>

 <link name="base_link">
      <visual>
   <origin xyz="0 0 0.885" rpy="0 0 0"/>
    <geometry>
    <box size="0.45 0.07 0.05"/>              
     </geometry>
     <material name="blue">
     <color rgba="0 0 1 1"/>              
     </material>
    </visual>    
  <collision>
    <origin xyz="0 0 0.885" rpy="0 0 0"/>
    <geometry>
     <box size="0.45 0.07 0.05"/>              
      </geometry>
</collision>
<inertial>
<mass value="0.4"/>
<inertia ixx="0.0086" ixy="00" ixz="0" iyy="0.015" iyz="0" izz="0.007"/> 
</inertial>
</link>

<!-- Right femur-->
 <link name="right_femur">
    <visual>
      <origin xyz="-0.025 0 -0.225" rpy="0 0 0"/>
      <geometry>
 <box size="0.02 0.07 0.5"/>         
</geometry> 
<material name="darkgray">
    <color rgba=".2 .2 .2 1"/>
    </material>
     </visual>
    <collision>
          <origin xyz="-0.025 0 -0.225" rpy="0 0 0"/>
          <geometry>
            <box size="0.02 0.07 0.5"/>         
      </geometry>  
</collision>
<inertial>
          <mass value="0.5"/>
          <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.00052"/>
</inertial>
</link>

<joint name="joint_right_hip" type="revolute">
<parent link="base_link"/>
<child link="right_femur"/>
 <origin xyz="0.26 0 0.885" rpy="0 0 0"/>
<axis xyz="1 0 0"/>
 <limit effort="30" velocity="1.0" lower="-2" upper="2" />

</joint>

 <!-- Right Leg-->
<link name="right_leg">
    <visual>
      <origin xyz="-0.025 0 -.20" rpy="0 0 0"/>
      <geometry>
       <box size="0.02 0.07 0.45"/>         
      </geometry> 
<material name="darkgray">
<color rgba=".2 .2 .2 1"/>
</material>
    </visual>
<collision>
          <origin xyz="-0.025 0 -0.20" rpy="0 0 0"/>
          <geometry>
            <box size="0.02 0.07 0.45"/>         
      </geometry>  
</collision>
<inertial>
          <mass value="0.5"/>
          <inertia ixx="0.0088" ixy="0" ixz="0" iyy="0.0085" iyz="0" izz="0.0005"/>
</inertial>
</link>

<joint name="joint_right_knee" type="revolute">
<parent link="right_femur"/>
<child link="right_leg"/>
<origin xyz="0.02 0 -0.45" rpy="0 0 0"/>
 <axis xyz="1 0 0"/>
 <limit effort="30" velocity="1.0" lower="-2" upper="0" />
</joint>



<link name="right_foot">
    <visual>
      <origin xyz="0 0.025 0" rpy="0 0 0"/>
      <geometry>
            <box size="0.1 0.25 0.02"/>         
      </geometry> 
<material name="darkgray">
<color rgba=".2 .2 .2 1"/>
</material>
    </visual>
<collision>
           <origin xyz="0 0.025 0" rpy="0 0 0"/>
          <geometry>
            <box size="0.1 0.25 0.02"/>         
      </geometry>  
</collision>
<inertial>
          <mass value="0.2"/>
          <inertia ixx="0.0015" ixy="0" ixz="0" iyy="0.0008" iyz="0" izz="0.0015"/>
</inertial>
</link>
<joint name="joint_right_foot" type="revolute">
<parent link="right_leg"/>
<child link="right_foot"/>
 <origin xyz="-0.065 0 -0.425" rpy="0 0 0"/>
 <limit effort="30" velocity="1.0" lower="-0.1" upper="0.1" />
 <axis xyz="1 0 0"/>
 </joint>

<!-- Left Leg-->

<!-- Left femur-->
 <link name="left_femur">
    <visual>
      <origin xyz="0.025 0 -0.225" rpy="0 0 0"/>
      <geometry>
       <box size="0.02 0.07 0.5"/>         
      </geometry> 
<material name="darkgray">
<color rgba=".2 .2 .2 1"/>
</material>
    </visual>
<collision>
          <origin xyz="0.025 0 -0.225" rpy="0 0 0"/>
          <geometry>
            <box size="0.02 0.07 0.5"/>         
      </geometry>  
</collision>
<inertial>
          <mass value="0.5"/>
          <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.0005"/>
</inertial>
</link>

<joint name="joint_left_hip" type="revolute">
<parent link="base_link"/>
<child link="left_femur"/>
<origin xyz="-0.26 0 0.885" rpy="0 0 0"/>
<axis xyz="1 0 0"/>
 <limit effort="30" velocity="1.0" lower="-2" upper="2" />

</joint>


<link name="left_leg">
    <visual>
      <origin xyz="0.025  0 -0.2" rpy="0 0 0"/>
      <geometry>
            <box size="0.02 0.07 0.45"/>         
      </geometry> 
<material name="darkgray">
<color rgba=".2 .2 .2 1"/>
</material>
    </visual>
<collision>
          <origin xyz="0.025 0 -0.2" rpy="0 0 0"/>
          <geometry>
            <box size="0.02 0.07 0.45"/>
      </geometry>  
</collision>
<inertial>
          <mass value="0.5"/>
          <inertia ixx="0.0088" ixy="0" ixz="0" iyy="0.085" iyz="0" izz="0.0005"/>
</inertial>
</link>

<joint name="joint_left_knee" type="revolute">
<parent link="left_femur"/>
<child link="left_leg"/>
 <origin xyz="-0.02 0 -0.45" rpy="0 0 0"/>
<axis xyz="1 0 0"/>
<limit effort="30" velocity="1.0" lower="-2" upper="0" />
</joint>

<link name="left_foot">
    <visual>
      <origin xyz="0 0.025 0" rpy="0 0 0"/>
      <geometry>
            <box size="0.1 0.25 0.02"/>
      </geometry> 
 <material name="darkgray">
<color rgba=".2 .2 .2 1"/>
</material>
    </visual>
<collision>
           <origin xyz="0 0.025 0" rpy="0 0 0"/>
          <geometry>
            <box size="0.1 0.25 0.02"/>         
      </geometry>  
</collision>
<inertial>
          <mass value="0.2"/>
          <inertia ixx="0.0015" ixy="0" ixz="0" iyy="0.0008" iyz="0" izz="0.0015"/>
</inertial>
</link>
<joint name="joint_left_foot" type="revolute">
<parent link="left_leg"/>
<child link="left_foot"/>
<origin xyz="0.065 0 -0.425" rpy="0 0 0"/>
 <limit effort="30" velocity="1.0" lower="-0.1" upper="0.1" />
<axis xyz="1 0 0"/>
</joint>

<transmission name="tran1">
<type>transmission_interface/SimpleTransmission</type>
<joint name="joint_right_hip">
  <hardwareInterface>EffortJointInterface</hardwareInterface>
</joint>
<actuator name="motor1">
  <hardwareInterface>EffortJointInterface</hardwareInterface>
  <mechanicalReduction>1</mechanicalReduction>
 </actuator>
</transmission>

<transmission name="tran2">
<type>transmission_interface/SimpleTransmission</type>
<joint name="joint_left_hip">
  <hardwareInterface>EffortJointInterface</hardwareInterface>
</joint>
<actuator name="motor2">
  <hardwareInterface>EffortJointInterface</hardwareInterface>
  <mechanicalReduction>1</mechanicalReduction>
</actuator>
 </transmission>

<transmission name="tran3">
<type>transmission_interface/SimpleTransmission</type>
<joint name="joint_right_knee">
  <hardwareInterface>EffortJointInterface</hardwareInterface>
</joint>
<actuator name="motor3">
  <hardwareInterface>EffortJointInterface</hardwareInterface>
  <mechanicalReduction>1</mechanicalReduction>
 </actuator>
  </transmission>

   <transmission name="tran4">
  <type>transmission_interface/SimpleTransmission</type>
  <joint name="joint_left_knee">
  <hardwareInterface>EffortJointInterface</hardwareInterface>
  </joint>
  <actuator name="motor4">
  <hardwareInterface>EffortJointInterface</hardwareInterface> 
    <mechanicalReduction>1</mechanicalReduction>
  </actuator>
 </transmission>
 </robot>

2.gazebo文件：（lle_gazebo2.gazebo）

 <robot>
 <!-- ros_control plugin -->
 <gazebo>
 <plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
  <robotNamespace>lle_sample</robotNamespace>
  <robotSimType>gazebo_ros_control/DefaultRobotHWSim</robotSimType>
  </plugin>
 </gazebo>

 <!-- Base Link -->
  <gazebo reference="base_link">
<kp>6000000.0</kp>
<kd>10000000.0</kd>
  <material>Gazebo/Red</material>
  </gazebo>

  <!-- Right Femur-->
 <gazebo reference="right_femur">
<kp>6000000.0</kp>
<kd>10000000.0</kd>
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<material>Gazebo/Green</material>
 </gazebo>

  <!-- Right Leg-->
  <gazebo reference="right_leg">
 <kp>6000000.0</kp>
 <kd>10000000.0</kd>
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<material>Gazebo/Blue</material>
  </gazebo>

  <!-- Right Foot -->
 <gazebo reference="right_foot">
    <material>Gazebo/Yellow</material>
 </gazebo>

 <!-- Left Femur-->
<gazebo reference="left_femur">
<kp>6000000.0</kp>
<kd>10000000.0</kd>
<mu1>0.2</mu1>
<mu2>0.2</mu2>
<material>Gazebo/Green</material>
 </gazebo>

 <!-- Left Leg-->
 <gazebo reference="left_leg">
 <kp>6000000.0</kp>
 <kd>10000000.0</kd>
<mu1>0.2</mu1>
<mu2>0.2</mu2>
 <material>Gazebo/Blue</material>
 </gazebo>

<!-- Left foot-->
 <gazebo reference="left_foot">
<material>Gazebo/Yellow</material>
 </gazebo>

 </robot>

3。用于pid控制的yaml文件（lle_sample_control2.yaml）

   lle_sample:
  # Publish all joint states -----------------------------------
   joint_state_controller:
    type: joint_state_controller/JointStateController
   publish_rate: 50 

  # Position Controllers ---------------------------------------
  joint_right_hip_position_controller:
   type: effort_controllers/JointPositionController
   joint: joint_right_hip
   pid: {p: 100.0, i: 0.01, d: 10.0}

 joint_right_knee_position_controller:
   type: effort_controllers/JointPositionController
   joint: joint_right_knee
   pid: {p: 100.0, i: 0.01, d: 10.0}

  joint_left_hip_position_controller:
   type: effort_controllers/JointPositionController
   joint: joint_left_hip
    pid: {p: 100.0, i: 0.01, d: 10.0}

 joint_left_knee_position_controller:
  type: effort_controllers/JointPositionController
  joint: joint_left_knee
  pid: {p: 100.0, i: 0.01, d: 10.0}

4。 lle_sample_control2.launch：

<launch>

<!-- Load joint controller configurations from YAML file to parameter server -->
<rosparam file="$(find lle_sample_control)/config/lle_sample_control2.yaml"/>

 <!-- load the controllers -->
 <node name="lle_sample_control2" pkg="controller_manager" type="spawner"   respawn="false"
   output="screen" ns="/lle_sample" args="--namespace=/lle_sample joint_state_controller joint_right_hip_position_controller joint_left_hip_position_controller joint_right_knee_position_controller joint_left_knee_position_controller --timeout 100"/>

  <!-- convert joint states to TF transforms for rviz, etc -->
  <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"
 respawn="false" output="screen">=
  <remap from="/joint_states" to="/lle_sample/joint_states" />
 </node>
 </launch>

5.lle_sample_gazebo.launch：

<launch>
 <!-- We resume the logic in gazebo_ros package empty_world.launch, -->
 <!-- changing only the name of the world to be launched -->

<include file="$(find lle_sample_control)/launch/lle_sample_control2.launch" />

 <include file="$(find gazebo_ros)/launch/empty_world.launch">
  <arg name="world_name" value="worlds/empty.world"/>
<arg name="paused" value="true"/>


 </include>

  <!-- Load the URDF into the ROS Parameter Server -->
   <param name="robot_description"
 command="$(find xacro)/xacro.py '$(find lle_sample_description)/urdf/lle_sample4.xacro'" />

   <!-- Spawn lle_sample into Gazebo -->
  <node name="spawn_urdf" pkg="gazebo_ros" type="spawn_model" respawn="false" output="screen"
 args="-param robot_description -urdf -model lle_sample" />

  <node name="lle_sample_walker" pkg="lle_sample_gazebo" type="walker.py" />
</launch>

最后，我为walker.py文件制作了一个rospy包。这是python文件的代码：

#!/usr/bin/env python

import rospy
 from  lle_sample_gazebo.lle_sample import LLE_sample


if __name__=="__main__":
rospy.init_node("walker_demo")

rospy.loginfo("Instantiating lle_sample Client")
lle_sample=LLE_sample()
rospy.sleep(1)

rospy.loginfo("lle_sample Walker Demo Starting")


lle_sample.set_walk_velocity(0.2,0,0)
rospy.sleep(3)
lle_sample.set_walk_velocity(1,0,0)
rospy.sleep(3)
lle_sample.set_walk_velocity(0,1,0)
rospy.sleep(3)
lle_sample.set_walk_velocity(0,-1,0)
rospy.sleep(3)
lle_sample.set_walk_velocity(-1,0,0)
rospy.sleep(3)
lle_sample.set_walk_velocity(0,0,0)

rospy.loginfo("lle_sample Walker Demo Finished")

在一个终端我使用$ roslaunch lle_sample_gazebo lle_sample_gazebo.launch 在其他终端我写$ rosrun lle_sample_gazebo walker.py 当我点击播放按钮时，我看到“lle_sample Walker Demo Starting”，但是凉亭中的模型是稳固的。