Webots+ROS学习记录(4)——六轮全地形移动机器人

有了以上经验,可以创造出一个全地形的移动机器人如图1
图1

第一步,创建robot节点,并给robot节点编写相应参数

注意,这里的机身不能再使用单纯的shape,因为其相对四足有坐标变换,所以应该使用transform

第二步,依次创建HingeJoint、soid、tramsform等节点,完成每一个轮子的node tree

注意,在这里有些地方我使用了group节点:group可以直接派生出transform。group节点包含子节点,但不引入新的坐标转换。group节点可能不包含后续solid、device或robot节点。

以下是我自己创建的world文件,供大家参考

#VRML_OBJ R2019a utf8
Robot {
  translation 4 0.4 0
  rotation 0 0 1 0
  children [
    DEF SIXWHEEL_BODY Transform {
      children [
        Shape {
          appearance Appearance {
            material Material {
            }
          }
          geometry DEF BODY Box {
            size 1.6 0.4 0.5
          }
        }
      ]
    }
    DEF HJ_BodyToHinge1 HingeJoint {
      jointParameters HingeJointParameters {
        axis 0 0 1
        anchor 0.65 0.1 -0.26
      }
      device [
        DEF ForLeftMoter1 RotationalMotor {
        }
      ]
      endPoint Solid {
        translation 0.65 0.1 -0.26
        rotation 0 0 -1 0.5000000000000004
        children [
          DEF BodyToHinge1 Transform {
            rotation 0 0 1 0
            children [
              Shape {
                appearance Appearance {
                }
                geometry Box {
                  size 0.02 0.1 0.01
                }
              }
            ]
          }
          DEF HingeToSlider SliderJoint {
            jointParameters JointParameters {
              axis 0 1 0
            }
            device [
              DEF ForLeFtLineMoter LinearMotor {
              }
            ]
            endPoint Solid {
              translation 0 -0.15 0
              children [
                DEF Slider1 Transform {
                  children [
                    Shape {
                      appearance Appearance {
                      }
                      geometry Box {
                        size 0.02 0.15 0.01
                      }
                    }
                  ]
                }
                DEF SliderToLeg HingeJoint {
                  jointParameters HingeJointParameters {
                    axis 0 0 1
                    anchor 0 -0.060000000000000005 -0.015
                  }
                  device [
                    DEF ForLeftMoter2 RotationalMotor {
                      name ""
                    }
                  ]
                  endPoint Solid {
                    translation 0 -0.060000000000000005 -0.015
                    rotation 0 0 1 0
                    children [
                      DEF Leg1 Transform {
                        rotation 0 0 1 1.6
                        children [
                          Shape {
                            appearance Appearance {
                            }
                            geometry Box {
                              size 0.02 0.5 0.01
                            }
                          }
                        ]
                      }
                      DEF LegToWheel1 HingeJoint {
                        jointParameters HingeJointParameters {
                          axis 0 0 1
                          anchor 0.24 0 -0.033
                        }
                        device [
                          DEF WheelMoter1 RotationalMotor {
                            name "wheel1"
                            maxVelocity 100
                            maxTorque 20
                          }
                        ]
                        endPoint Solid {
                          translation 0.24 0 -0.033
                          rotation 0 0 1 0
                          children [
                            DEF Wheel1 Transform {
                              rotation 1 0 0 1.5707963
                              children [
                                Shape {
                                  appearance Appearance {
                                  }
                                  geometry Cylinder {
                                    height 0.05
                                    radius 0.2
                                  }
                                }
                              ]
                            }
                          ]
                          boundingObject USE Wheel1
                          physics DEF MY_WHEEL_PH Physics {
                            density -1
                            mass 0.05
                          }
                        }
                      }
                    ]
                    boundingObject USE Leg1
                    physics DEF MY_LEG_PH Physics {
                      density -1
                      mass 0.05
                    }
                  }
                }
              ]
              boundingObject USE Slider1
              physics DEF MY_SLIDER_PH Physics {
                density -1
                mass 0.05
              }
            }
          }
        ]
        boundingObject USE BodyToHinge1
        physics DEF MY_HINGE_PH Physics {
          density -1
          mass 0.05
        }
      }
    }
    DEF MidHinge1 HingeJoint {
      jointParameters HingeJointParameters {
        axis 0 0 1
        anchor 0.32299999769066834 0.008000001494273701 -0.275
      }
      device [
        DEF ConnectorMoter1 RotationalMotor {
        }
      ]
      endPoint Solid {
        translation 0.32295089466010324 0.0060253712513336465 -0.2750812021895668
        rotation -0.8584282350645976 0.26759023243174207 -0.4376030538638943 0.00038163349067606503
        children [
          DEF Group1 Group {
            children [
              Transform {
                translation -0.009 0.028999999999999998 0
                rotation 0 0 1 -0.5236003061004253
                children [
                  Shape {
                    appearance Appearance {
                    }
                    geometry Box {
                      size 0.01 0.04 0.018
                    }
                  }
                ]
              }
              Transform {
                translation 0.020999999999999998 0.033 0
                rotation 0 0 1 1.0472003061004251
                children [
                  Shape {
                    appearance Appearance {
                    }
                    geometry Box {
                      size 0.01 0.13 0.018
                    }
                  }
                ]
              }
              Transform {
                rotation 0 0 1 1.0472003061004251
                children [
                  Shape {
                    appearance Appearance {
                    }
                    geometry Box {
                      size 0.01 0.13 0.018
                    }
                  }
                ]
              }
              Transform {
                translation -0.004 0.026 -0.019
                rotation 0 0 1 1.0472003061004251
                children [
                  Shape {
                    appearance Appearance {
                    }
                    geometry Box {
                      size 0.05 0.16 0.018
                    }
                  }
                ]
              }
              Transform {
                translation -0.004 0.026 0.017
                rotation 0 0 1 1.0472003061004251
                children [
                  Shape {
                    appearance Appearance {
                    }
                    geometry Box {
                      size 0.05 0.16 0.009
                    }
                  }
                ]
              }
            ]
          }
        ]
        name "midhinge"
        boundingObject USE Group1
        physics Physics {
          density -1
          mass 0.05
        }
      }
    }
    DEF HJ_BodyToHinge2 HingeJoint {
      jointParameters HingeJointParameters {
        axis 0 0 1
        anchor 0.65 0.1 0.26
      }
      device [
        DEF ForRightMoter1 RotationalMotor {
        }
      ]
      endPoint Solid {
        translation 0.65 0.1 0.26
        rotation 0 0 -1 0.5000000000000004
        children [
          DEF BodyToHinge2 Transform {
            rotation 0 0 1 0
            children [
              Shape {
                appearance Appearance {
                }
                geometry Box {
                  size 0.02 0.1 0.01
                }
              }
            ]
          }
          DEF HingeToSlider2 SliderJoint {
            jointParameters JointParameters {
              axis 0 1 0
            }
            device [
              DEF ForRightLineMoter LinearMotor {
              }
            ]
            endPoint Solid {
              translation 0 -0.15 0
              children [
                DEF Slider2 Transform {
                  children [
                    Shape {
                      appearance Appearance {
                      }
                      geometry Box {
                        size 0.02 0.15 0.01
                      }
                    }
                  ]
                }
                DEF Slider2ToLeg HingeJoint {
                  jointParameters HingeJointParameters {
                    axis 0 0 1
                    anchor 0 -0.060000000000000005 0.015
                  }
                  device [
                    DEF ForRightMoter2 RotationalMotor {
                    }
                  ]
                  endPoint Solid {
                    translation 0 -0.060000000000000005 0.015
                    rotation 0 0 1 0
                    children [
                      DEF Leg2 Transform {
                        rotation 0 0 1 1.6
                        children [
                          Shape {
                            appearance Appearance {
                            }
                            geometry Box {
                              size 0.02 0.5 0.01
                            }
                          }
                        ]
                      }
                      DEF LegToWheel2 HingeJoint {
                        jointParameters HingeJointParameters {
                          axis 0 0 1
                          anchor 0.24 0 0.033
                        }
                        device [
                          DEF WheelMoter2 RotationalMotor {
                            name "wheel2"
                            maxVelocity 100
                            maxTorque 20
                          }
                        ]
                        endPoint Solid {
                          translation 0.24 0 0.033
                          rotation 0 0 1 0
                          children [
                            DEF Wheel2 Transform {
                              rotation 1 0 0 1.5707963
                              children [
                                Shape {
                                  appearance Appearance {
                                  }
                                  geometry Cylinder {
                                    height 0.05
                                    radius 0.2
                                  }
                                }
                              ]
                            }
                          ]
                          boundingObject USE Wheel2
                          physics USE MY_WHEEL_PH
                        }
                      }
                    ]
                    boundingObject USE Leg2
                    physics USE MY_LEG_PH
                  }
                }
              ]
              boundingObject USE Slider2
              physics USE MY_SLIDER_PH
            }
          }
        ]
        name "solid(1)"
        boundingObject USE BodyToHinge2
        physics Physics {
          density -1
          mass 0.05
        }
      }
    }
    DEF MidHinge2 HingeJoint {
      jointParameters HingeJointParameters {
        axis 0 0 1
        anchor 0.32299999769066834 0.008000001494273701 0.275
      }
      device [
        RotationalMotor {
        }
      ]
      endPoint Solid {
        translation 0.3229999976244453 0.006040038943473158 0.2750000107903539
        rotation -0.9999731385339321 1.2772224258984438e-07 -0.007329543681641848 1.097632116311221e-05
        children [
          DEF GROUP2 Group {
            children [
              Transform {
                translation -0.009 0.028999999999999998 0
                rotation 0 0 1 -0.5236003061004253
                children [
                  Shape {
                    appearance Appearance {
                    }
                    geometry Box {
                      size 0.01 0.04 0.018
                    }
                  }
                ]
              }
              Transform {
                translation 0.020999999999999998 0.033 0
                rotation 0 0 1 1.0472003061004251
                children [
                  Shape {
                    appearance Appearance {
                    }
                    geometry Box {
                      size 0.01 0.13 0.018
                    }
                  }
                ]
              }
              Transform {
                rotation 0 0 1 1.0472003061004251
                children [
                  Shape {
                    appearance Appearance {
                    }
                    geometry Box {
                      size 0.01 0.13 0.018
                    }
                  }
                ]
              }
              Transform {
                translation -0.004 0.026 -0.019
                rotation 0 0 1 1.0472003061004251
                children [
                  Shape {
                    appearance Appearance {
                    }
                    geometry Box {
                      size 0.05 0.16 0.018
                    }
                  }
                ]
              }
              Transform {
                translation -0.004 0.026 0.017
                rotation 0 0 1 1.0472003061004251
                children [
                  Shape {
                    appearance Appearance {
                    }
                    geometry Box {
                      size 0.05 0.16 0.009
                    }
                  }
                ]
              }
            ]
          }
        ]
        name "midhinge(1)"
        boundingObject USE GROUP2
        physics Physics {
          density -1
          mass 0.05
        }
      }
    }
    DEF HJ_Wheel3 HingeJoint {
      jointParameters HingeJointParameters {
        axis 0 0 1
        anchor 0 -0.2 -0.28
      }
      device [
        RotationalMotor {
          name "wheel3"
          maxVelocity 100
        }
      ]
      endPoint Solid {
        translation 0 -0.19999999999999998 -0.275
        rotation 0 0 1 0
        children [
          DEF Wheel3 Transform {
            rotation 1 0 0 1.5707963
            children [
              Shape {
                appearance Appearance {
                }
                geometry Cylinder {
                  height 0.05
                  radius 0.2
                }
              }
            ]
          }
        ]
        name "solid(2)"
        boundingObject USE Wheel3
        physics USE MY_WHEEL_PH
      }
    }
    DEF HJ_Wheel4 HingeJoint {
      jointParameters HingeJointParameters {
        axis 0 0 1
        anchor 0 -0.2 0.28
      }
      device [
        RotationalMotor {
          name "wheel4"
          maxVelocity 100
        }
      ]
      endPoint Solid {
        translation 0 -0.2 0.275
        rotation 0 0 1 0
        children [
          DEF Wheel4 Transform {
            rotation 1 0 0 1.5707963
            children [
              Shape {
                appearance Appearance {
                }
                geometry Cylinder {
                  height 0.05
                  radius 0.2
                }
              }
            ]
          }
        ]
        name "solid(3)"
        boundingObject USE Wheel4
        physics USE MY_WHEEL_PH
      }
    }
    DEF HJ_BodyToHinge5 HingeJoint {
      jointParameters HingeJointParameters {
        axis 0 0 1
        anchor -0.65 0.1 0.26
      }
      device [
        RotationalMotor {
        }
      ]
      endPoint Solid {
        translation -0.65 0.1 -0.26
        rotation 0 0 1 0.5000000000000004
        children [
          DEF BodyToHinge5 Transform {
            rotation 0 0 1 0
            children [
              Shape {
                appearance Appearance {
                }
                geometry Box {
                  size 0.02 0.1 0.01
                }
              }
            ]
          }
          DEF Hinge5ToSlider SliderJoint {
            jointParameters JointParameters {
              axis 0 1 0
            }
            device [
              LinearMotor {
              }
            ]
            endPoint Solid {
              translation 0 -0.15 0
              children [
                DEF Slider5 Transform {
                  children [
                    Shape {
                      appearance Appearance {
                      }
                      geometry Box {
                        size 0.02 0.15 0.01
                      }
                    }
                  ]
                }
                DEF Slider5ToLeg HingeJoint {
                  jointParameters HingeJointParameters {
                    axis 0 0 1
                    anchor 0 -0.060000000000000005 -0.015
                  }
                  device [
                    RotationalMotor {
                    }
                  ]
                  endPoint Solid {
                    translation 0 -0.060000000000000005 -0.015
                    rotation 0 0 1 3.141592653589793
                    children [
                      DEF Leg5 Transform {
                        rotation 0 0 1 1.6
                        children [
                          Shape {
                            appearance Appearance {
                            }
                            geometry Box {
                              size 0.02 0.5 0.01
                            }
                          }
                        ]
                      }
                      DEF Leg5ToWheel1 HingeJoint {
                        jointParameters HingeJointParameters {
                          axis 0 0 1
                          anchor 0.24 0 -0.033
                        }
                        device [
                          RotationalMotor {
                            name "wheel4"
                            maxVelocity 100
                          }
                        ]
                        endPoint Solid {
                          translation 0.24 0 -0.033
                          rotation 0 0 1 0
                          children [
                            DEF Wheel5 Transform {
                              rotation 1 0 0 1.5707963
                              children [
                                Shape {
                                  appearance Appearance {
                                  }
                                  geometry Cylinder {
                                    height 0.05
                                    radius 0.2
                                  }
                                }
                              ]
                            }
                          ]
                          boundingObject USE Wheel5
                          physics USE MY_WHEEL_PH
                        }
                      }
                    ]
                    boundingObject USE Leg5
                    physics USE MY_LEG_PH
                  }
                }
              ]
              boundingObject USE Slider5
              physics USE MY_SLIDER_PH
            }
          }
        ]
        name "solid(4)"
        boundingObject USE BodyToHinge5
        physics USE MY_HINGE_PH
      }
    }
    DEF MidHinge3 HingeJoint {
      jointParameters HingeJointParameters {
        axis 0 0 1
        anchor 0.32299999769066834 0.008000001494273701 -0.275
      }
      device [
        RotationalMotor {
        }
      ]
      endPoint Solid {
        translation -0.32300000037121457 0.0660401038494264 -0.27500064768412996
        rotation -3.1686134915985575e-06 -5.488125017754091e-06 -0.9999999999799203 2.094399795727144
        children [
          DEF Group3 Group {
            children [
              Transform {
                translation -0.009 0.028999999999999998 0
                rotation 0 0 1 -0.5236003061004253
                children [
                  Shape {
                    appearance Appearance {
                    }
                    geometry Box {
                      size 0.01 0.04 0.018
                    }
                  }
                ]
              }
              Transform {
                translation 0.020999999999999998 0.033 0
                rotation 0 0 1 1.0472003061004251
                children [
                  Shape {
                    appearance Appearance {
                    }
                    geometry Box {
                      size 0.01 0.13 0.018
                    }
                  }
                ]
              }
              Transform {
                rotation 0 0 1 1.0472003061004251
                children [
                  Shape {
                    appearance Appearance {
                    }
                    geometry Box {
                      size 0.01 0.13 0.018
                    }
                  }
                ]
              }
              Transform {
                translation -0.004 0.026 -0.019
                rotation 0 0 1 1.0472003061004251
                children [
                  Shape {
                    appearance Appearance {
                    }
                    geometry Box {
                      size 0.05 0.16 0.018
                    }
                  }
                ]
              }
              Transform {
                translation -0.004 0.026 0.017
                rotation 0 0 1 1.0472003061004251
                children [
                  Shape {
                    appearance Appearance {
                    }
                    geometry Box {
                      size 0.05 0.16 0.009
                    }
                  }
                ]
              }
            ]
          }
        ]
        name "midhinge(2)"
        boundingObject USE Group3
        physics Physics {
          density -1
          mass 0.05
        }
      }
    }
    DEF HJ_BodyToHinge6 HingeJoint {
      jointParameters HingeJointParameters {
        axis 0 0 1
        anchor -0.65 0.1 0.26
      }
      device [
        RotationalMotor {
        }
      ]
      endPoint Solid {
        translation -0.65 0.1 0.26
        rotation 0 0 1 0.5000000000000004
        children [
          DEF BodyToHinge6 Transform {
            rotation 0 0 1 0
            children [
              Shape {
                appearance Appearance {
                }
                geometry Box {
                  size 0.02 0.1 0.01
                }
              }
            ]
          }
          DEF Hinge6ToSlider SliderJoint {
            jointParameters JointParameters {
              axis 0 1 0
            }
            device [
              LinearMotor {
              }
            ]
            endPoint Solid {
              translation 0 -0.15 0
              children [
                DEF Slider6 Transform {
                  children [
                    Shape {
                      appearance Appearance {
                      }
                      geometry Box {
                        size 0.02 0.15 0.01
                      }
                    }
                  ]
                }
                DEF Slider6ToLeg HingeJoint {
                  jointParameters HingeJointParameters {
                    axis 0 0 1
                    anchor 0 -0.060000000000000005 -0.015
                  }
                  device [
                    RotationalMotor {
                    }
                  ]
                  endPoint Solid {
                    translation 0 -0.060000000000000005 0.015
                    rotation 0 0 1 3.141592653589793
                    children [
                      DEF Leg6 Transform {
                        rotation 0 0 1 1.6
                        children [
                          Shape {
                            appearance Appearance {
                            }
                            geometry Box {
                              size 0.02 0.5 0.01
                            }
                          }
                        ]
                      }
                      DEF Leg6ToWheel1 HingeJoint {
                        jointParameters HingeJointParameters {
                          axis 0 0 1
                          anchor 0.24 0 0.033
                        }
                        device [
                          RotationalMotor {
                            name "wheel4"
                            maxVelocity 100
                          }
                        ]
                        endPoint Solid {
                          translation 0.24 0 0.033
                          rotation 0 0 1 0
                          children [
                            DEF Wheel6 Transform {
                              rotation 1 0 0 1.5707963
                              children [
                                Shape {
                                  appearance Appearance {
                                  }
                                  geometry Cylinder {
                                    height 0.05
                                    radius 0.2
                                  }
                                }
                              ]
                            }
                          ]
                          boundingObject USE Wheel6
                          physics USE MY_WHEEL_PH
                        }
                      }
                    ]
                    boundingObject USE Leg6
                    physics USE MY_LEG_PH
                  }
                }
              ]
              boundingObject USE Slider6
              physics USE MY_SLIDER_PH
            }
          }
        ]
        name "solid(5)"
        boundingObject USE BodyToHinge6
        physics USE MY_HINGE_PH
      }
    }
    DEF MidHinge4 HingeJoint {
      jointParameters HingeJointParameters {
        axis 0 0 1
        anchor 0.32299999769066834 0.008000001494273701 0.275
      }
      device [
        RotationalMotor {
        }
      ]
      endPoint Solid {
        translation -0.32300000666469797 0.06603931936095904 0.2749993682184271
        rotation -3.1698848045657166e-06 -5.490320911997462e-06 -0.9999999999799041 2.09439979575871
        children [
          DEF Group4 Group {
            children [
              Transform {
                translation -0.009 0.028999999999999998 0
                rotation 0 0 1 -0.5236003061004253
                children [
                  Shape {
                    appearance Appearance {
                    }
                    geometry Box {
                      size 0.01 0.04 0.018
                    }
                  }
                ]
              }
              Transform {
                translation 0.020999999999999998 0.033 0
                rotation 0 0 1 1.0472003061004251
                children [
                  Shape {
                    appearance Appearance {
                    }
                    geometry Box {
                      size 0.01 0.13 0.018
                    }
                  }
                ]
              }
              Transform {
                rotation 0 0 1 1.0472003061004251
                children [
                  Shape {
                    appearance Appearance {
                    }
                    geometry Box {
                      size 0.01 0.13 0.018
                    }
                  }
                ]
              }
              Transform {
                translation -0.004 0.026 -0.019
                rotation 0 0 1 1.0472003061004251
                children [
                  Shape {
                    appearance Appearance {
                    }
                    geometry Box {
                      size 0.05 0.16 0.018
                    }
                  }
                ]
              }
              Transform {
                translation -0.004 0.026 0.017
                rotation 0 0 1 1.0472003061004251
                children [
                  Shape {
                    appearance Appearance {
                    }
                    geometry Box {
                      size 0.05 0.16 0.009
                    }
                  }
                ]
              }
            ]
          }
        ]
        name "midhinge(3)"
        boundingObject USE Group4
        physics Physics {
          density -1
          mass 0.8
        }
      }
    }
  ]
  boundingObject DEF MY_BOUNDING Transform {
    children [
      USE BODY
    ]
  }
  physics Physics {
    density -1
    mass 0.05
  }
  controller "6x6controller"
}

在这里插入图片描述