[WIP] auto-stabilizer for jaxon wheel

hrpsys_choreonoid_tutorials/CMakeLists.txt

@@ -183,6 +183,7 @@ configure_file(${PROJECT_SOURCE_DIR}/config/JAXON_RED_STEPS.cnoid.in ${PROJECT_S
 ###
 if (${jsk_models_FOUND})
 configure_file(${PROJECT_SOURCE_DIR}/config/JAXON_BLUE_RH_FLAT.cnoid.in ${PROJECT_SOURCE_DIR}/config/JAXON_BLUE_RH_FLAT.cnoid @ONLY)
+configure_file(${PROJECT_SOURCE_DIR}/config/JAXON_BLUE_RH_LOAD_OBJ.cnoid.in ${PROJECT_SOURCE_DIR}/config/JAXON_BLUE_RH_LOAD_OBJ.cnoid @ONLY)
 endif()
 
 ###

hrpsys_choreonoid_tutorials/config/JAXON_BLUE_RH_LOAD_OBJ.cnoid.in

@@ -0,0 +1,366 @@
+items:
+  id: 0
+  name: "Root"
+  plugin: Base
+  class: RootItem
+  children:
+    -
+      id: 1
+      name: "World"
+      plugin: Body
+      class: WorldItem
+      data:
+        collisionDetection: false
+        collisionDetector: AISTCollisionDetector
+      children:
+        -
+          id: 2
+          name: "JAXON_BLUE"
+          plugin: Body
+          class: BodyItem
+          data:
+            modelFile: "@JSK_MODELS_DIR@/JAXON_BLUE/JAXON_BLUEmain_bush.wrl"
+            format: CHOREONOID-BODY
+            currentBaseLink: "WAIST"
+            rootPosition: [ 0.0, 0.0, 1.0235 ]
+            rootAttitude: [
+              0, -1, 0,
+              1, 0, 0,
+              0, 0, 1 ]
+            jointPositions: [
+               0.000054, -0.003093, -0.262419,  0.681091, -0.418672,  0.003093,  0.000054, -0.003093, -0.262401,  0.681084,
+              -0.418684,  0.003093,  0.000000,  0.000000,  0.000000,  0.000000,  0.000000,  0.523599, -0.349066,
+              -0.087266, -1.396263,  0.000000,  0.000000, -0.349066,  0.523599,  0.349066,  0.087266, -1.396263,
+               0.000000,  0.000000, -0.349066 ]
+            initialRootPosition: [ 0.0, 0.0, 1.0185 ]
+            initialRootAttitude: [
+              0, -1, 0,
+              1, 0, 0,
+              0, 0, 1 ]
+            initialJointPositions: [
+               0.000054, -0.003093, -0.262419,  0.681091, -0.418672,  0.003093,  0.000054, -0.003093, -0.262401,  0.681084,
+              -0.418684,  0.003093,  0.000000,  0.000000,  0.000000,  0.000000,  0.000000,  0.523599, -0.349066,
+              -0.087266, -1.396263,  0.000000,  0.000000, -0.349066,  0.523599,  0.349066,  0.087266, -1.396263,
+               0.000000,  0.000000, -0.349066 ]
+            zmp: [ 0, 0, 0 ]
+            collisionDetection: true
+            selfCollisionDetection: false
+            isEditable: true
+          children:
+            -
+              id: 3
+              name: "BodyRTC"
+              plugin: OpenRTM
+              class: BodyRTCItem
+              data:
+                isImmediateMode: true
+                moduleName: "@JVRC_RTC_DIRECTORY@/lib/RobotHardware_choreonoid"
+                confFileName: "@JVRC_CONF_DIRECTORY@/BodyRTC_JAXON_BLUE.RH.conf"
+                configurationMode: Use Configuration File
+                AutoConnect: false
+                InstanceName: JAXON_BLUE(Robot)0
+                bodyPeriodicRate: 0.002
+        -
+          id: 4
+          name: "AISTSimulator"
+          plugin: Body
+          class: AISTSimulatorItem
+          data:
+            realtimeSync: true
+            recording: full
+            timeRangeMode: TimeBar range
+            onlyActiveControlPeriod: true
+            timeLength: 12000
+            allLinkPositionOutputMode: false
+            deviceStateOutput: true
+            controllerThreads: true
+            recordCollisionData: false
+            dynamicsMode: Forward dynamics
+            integrationMode: Runge Kutta
+            gravity: [ 0, 0, -9.80665 ]
+            staticFriction: 1
+            slipFriction: 1
+            cullingThresh: 0.005
+            contactCullingDepth: 0.03
+            errorCriterion: 0.001
+            maxNumIterations: 1000
+            contactCorrectionDepth: 0.0001
+            contactCorrectionVelocityRatio: 1
+            kinematicWalking: false
+            2Dmode: false
+    -
+      id: 5
+      name: "ros_service_server.py"
+      plugin: Python
+      class: PythonScriptItem
+      data:
+        file: "@JVRC_RTC_DIRECTORY@/scripts/ros_service_server.py"
+        executionOnLoading: true
+        backgroundExecution: false
+    -
+      id: 6
+      name: "add_objects.py"
+      plugin: Python
+      class: PythonScriptItem
+      data: 
+        file: "@JVRC_RTC_DIRECTORY@/launch/add_objects.py"
+        executionOnLoading: true
+        backgroundExecution: false
+views:
+  -
+    id: 0
+    name: "CameraImage"
+    plugin: Base
+    class: ImageView
+    mounted: true
+  -
+    id: 1
+    plugin: Base
+    class: ItemPropertyView
+    mounted: true
+  -
+    id: 2
+    plugin: Base
+    class: ItemTreeView
+    mounted: true
+    state:
+      selected: [ 9 ]
+      checked: [ 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, ]
+      expanded: [ 1 ]
+  -
+    id: 3
+    plugin: Base
+    class: MessageView
+    mounted: true
+  -
+    id: 4
+    plugin: Base
+    class: SceneView
+    mounted: true
+    state:
+      editMode: true
+      viewpointControlMode: thirdPerson
+      collisionLines: false
+      polygonMode: fill
+      defaultHeadLight: true
+      defaultHeadLightIntensity: 0.75
+      headLightLightingFromBack: false
+      worldLight: true
+      worldLightIntensity: 0.5
+      worldLightAmbient: 0.3
+      additionalLights: true
+      floorGrid: true
+      floorGridSpan: 10
+      floorGridInterval: 0.5
+      xzGridSpan: 10
+      xzGridInterval: 0.5
+      xzGrid: false
+      yzGridSpan: 10
+      yzGridInterval: 0.5
+      texture: true
+      lineWidth: 1
+      pointSize: 1
+      normalVisualization: false
+      normalLength: 0.01
+      coordinateAxes: true
+      showFPS: false
+      enableNewDisplayListDoubleRendering: false
+      useBufferForPicking: true
+      cameras:
+        -
+          camera: [ System, Perspective ]
+          isCurrent: true
+          fieldOfView: 0.6978
+          near: 0.01
+          far: 100
+          eye: [ 2.7, -2.7, 2 ]
+          direction: [ -0.7, 0.7, -0.3 ]
+          up: [ -0.25, 0.25, 1]
+        -
+          camera: [ System, Orthographic ]
+          orthoHeight: 20
+          near: 0.01
+          far: 100
+      backgroundColor: [ 0.100000001, 0.100000001, 0.300000012 ]
+      gridColor: [ 0.899999976, 0.899999976, 0.899999976, 1 ]
+      xzgridColor: [ 0.899999976, 0.899999976, 0.899999976, 1 ]
+      yzgridColor: [ 0.899999976, 0.899999976, 0.899999976, 1 ]
+      dedicatedItemTreeViewChecks: false
+  -
+    id: 5
+    name: "Task"
+    plugin: Base
+    class: TaskView
+    state:
+      layoutMode: horizontal
+      isAutoMode: false
+  -
+    id: 6
+    plugin: Body
+    class: BodyLinkView
+    mounted: true
+    state:
+      showRotationMatrix: false
+  -
+    id: 7
+    plugin: Body
+    class: JointSliderView
+    mounted: true
+    state:
+      showAllJoints: true
+      jointId: false
+      name: true
+      numColumns: 1
+      spinBox: true
+      slider: true
+      labelOnLeft: true
+  -
+    id: 8
+    plugin: Body
+    class: LinkSelectionView
+    mounted: true
+    state:
+      listingMode: "Link List"
+  -
+    id: 10
+    plugin: Python
+    class: PythonConsoleView
+    mounted: true
+toolbars:
+  "TimeBar":
+    minTime: 0
+    maxTime: 12000
+    frameRate: 1000
+    playbackFrameRate: 50
+    idleLoopDrivenMode: false
+    currentTime: 0
+    speedScale: 1
+    syncToOngoingUpdates: true
+    autoExpansion: true
+  "KinematicsBar":
+    mode: AUTO
+    enablePositionDragger: true
+    penetrationBlock: false
+    collisionLinkHighlight: false
+    snapDistance: 0.025
+    penetrationBlockDepth: 0.0005
+    lazyCollisionDetectionMode: true
+  "LeggedBodyBar":
+    stanceWidth: 0.15
+  "BodyMotionGenerationBar":
+    autoGenerationForNewBody: true
+    balancer: false
+    autoGeneration: false
+    timeScaleRatio: 1
+    preInitialDuration: 1
+    postFinalDuration: 1
+    onlyTimeBarRange: false
+    makeNewBodyItem: true
+    stealthyStepMode: true
+    stealthyHeightRatioThresh: 2
+    flatLiftingHeight: 0.005
+    flatLandingHeight: 0.005
+    impactReductionHeight: 0.005
+    impactReductionTime: 0.04
+    autoZmp: true
+    minZmpTransitionTime: 0.1
+    zmpCenteringTimeThresh: 0.03
+    zmpTimeMarginBeforeLiftingSpin: 0
+    zmpMaxDistanceFromCenter: 0.02
+    allLinkPositions: false
+    lipSyncMix: false
+    timeToStartBalancer: 0
+    balancerIterations: 2
+    plainBalancerMode: false
+    boundaryConditionType: position
+    boundarySmootherType: quintic
+    boundarySmootherTime: 0.5
+    boundaryCmAdjustment: false
+    boundaryCmAdjustmentTime: 1
+    waistHeightRelaxation: false
+    gravity: 9.8
+    dynamicsTimeRatio: 1
+Body:
+  "BodyMotionEngine":
+    updateJointVelocities: false
+  "EditableSceneBody":
+    editableSceneBodies:
+      -
+        bodyItem: 2
+        showCenterOfMass: false
+        showPpcom: false
+        showZmp: false
+      -
+        bodyItem: 3
+        showCenterOfMass: false
+        showPpcom: false
+        showZmp: false
+      -
+        bodyItem: 4
+        showCenterOfMass: false
+        showPpcom: false
+        showZmp: false
+    staticModelEditing: false
+  "KinematicFaultChecker":
+    checkJointPositions: true
+    angleMargin: 0
+    translationMargin: 0
+    checkJointVelocities: true
+    velocityLimitRatio: 100
+    targetJoints: all
+    checkSelfCollisions: true
+    onlyTimeBarRange: false
+OpenRTM:
+  "deleteUnmanagedRTCsOnStartingSimulation": false
+viewAreas:
+  -
+    type: embedded
+    tabs: true
+    contents:
+      type: splitter
+      orientation: horizontal
+      sizes: [ 310, 2195 ]
+      children:
+        -
+          type: splitter
+          orientation: vertical
+          sizes: [ 768, 767 ]
+          children:
+            -
+              type: pane
+              views: [ 2 ]
+              current: 2
+            -
+              type: pane
+              views: [ 1, 8 ]
+              current: 1
+        -
+          type: splitter
+          orientation: vertical
+          sizes: [ 1205, 330 ]
+          children:
+            -
+              type: splitter
+              orientation: horizontal
+              sizes: [ 449, 1740 ]
+              children:
+                -
+                  type: pane
+                  views: [ 6, 7, 0 ]
+                  current: 6
+                -
+                  type: pane
+                  views: [ 4 ]
+                  current: 4
+            -
+              type: pane
+              views: [ 3, 10 ]
+              current: 10
+layoutOfToolBars:
+  rows:
+    -
+      - { name: "FileBar", x: 0, priority: 0 }
+      - { name: "ScriptBar", x: 47, priority: 3 }
+      - { name: "TimeBar", x: 47, priority: 1 }
+      - { name: "SceneBar", x: 1455, priority: 2 }
+      - { name: "SimulationBar", x: 1464, priority: 0 }