From d01d274cfe5ae457cd6c185e824ec88d3be0a3c1 Mon Sep 17 00:00:00 2001 From: Masaki Murooka Date: Mon, 27 Mar 2023 09:33:33 +0900 Subject: [PATCH 1/7] [cnoid] Add MCC_JVRC1_SampleStepladder.in.cnoid --- cnoid/model/MCC_materials.yaml | 6 + cnoid/model/SampleStepladder.body | 97 +++ cnoid/project/CMakeLists.txt | 1 + .../MCC_JVRC1_SampleStepladder.in.cnoid | 573 ++++++++++++++++++ 4 files changed, 677 insertions(+) create mode 100644 cnoid/model/SampleStepladder.body create mode 100644 cnoid/project/MCC_JVRC1_SampleStepladder.in.cnoid diff --git a/cnoid/model/MCC_materials.yaml b/cnoid/model/MCC_materials.yaml index 4c3da72..60242ec 100644 --- a/cnoid/model/MCC_materials.yaml +++ b/cnoid/model/MCC_materials.yaml @@ -8,6 +8,9 @@ materials: - name: Field roughness: 1.0 + - + name: Stepladder + roughness: 1.0 contactMaterials: - @@ -19,3 +22,6 @@ contactMaterials: - materials: [ Field, Floor ] friction: 0.5 + - + materials: [ Stepladder, Floor ] + friction: 0.5 diff --git a/cnoid/model/SampleStepladder.body b/cnoid/model/SampleStepladder.body new file mode 100644 index 0000000..c0dd26d --- /dev/null +++ b/cnoid/model/SampleStepladder.body @@ -0,0 +1,97 @@ +format: ChoreonoidBody +formatVersion: 1.0 +angleUnit: radian +name: SampleStepladder +rootLink: Root +links: + - + name: Root + jointType: fixed + - + name: HandrailLeft + parent: Root + jointType: fixed + material: Stepladder + translation: [ 0.0, 0.3, 0.0 ] + rotation: [ 0, 1, 0, 0.2617993877991494 ] + elements: + - + type: Shape + translation: [ 0.0, 0.0, 1.0 ] + rotation: [ 1, 0, 0, 1.5707963267948966 ] + geometry: { type: Cylinder, radius: 0.015, height: 2.0 } + appearance: { material: { diffuseColor: [ 0.3, 0.5, 0.2 ] }} + - + name: HandrailRight + parent: Root + jointType: fixed + material: Stepladder + translation: [ 0.0, -0.3, 0.0 ] + rotation: [ 0, 1, 0, 0.2617993877991494 ] + elements: + - + type: Shape + translation: [ 0.0, 0.0, 1.0 ] + rotation: [ 1, 0, 0, 1.5707963267948966 ] + geometry: { type: Cylinder, radius: 0.015, height: 2.0 } + appearance: { material: { diffuseColor: [ 0.3, 0.5, 0.2 ] }} + - + name: Step1 + parent: Root + jointType: fixed + material: Stepladder + translation: [ 0.05358983848622454, 0.0, 0.2 ] + elements: + - + type: Shape + translation: [ 0.0, 0.0, -0.01 ] + geometry: { type: Box, size: [ 0.08, 0.6, 0.02 ] } + appearance: { material: { diffuseColor: [ 0.3, 0.5, 0.2 ] }} + - + name: Step2 + parent: Root + jointType: fixed + material: Stepladder + translation: [ 0.10717967697244908, 0.0, 0.4 ] + elements: + - + type: Shape + translation: [ 0.0, 0.0, -0.01 ] + geometry: { type: Box, size: [ 0.08, 0.6, 0.02 ] } + appearance: { material: { diffuseColor: [ 0.3, 0.5, 0.2 ] }} + - + name: Step3 + parent: Root + jointType: fixed + material: Stepladder + translation: [ 0.1607695154586736, 0.0, 0.6 ] + elements: + - + type: Shape + translation: [ 0.0, 0.0, -0.01 ] + geometry: { type: Box, size: [ 0.08, 0.6, 0.02 ] } + appearance: { material: { diffuseColor: [ 0.3, 0.5, 0.2 ] }} + - + name: Step4 + parent: Root + jointType: fixed + material: Stepladder + translation: [ 0.21435935394489816, 0.0, 0.8 ] + elements: + - + type: Shape + translation: [ 0.0, 0.0, -0.01 ] + geometry: { type: Box, size: [ 0.08, 0.6, 0.02 ] } + appearance: { material: { diffuseColor: [ 0.3, 0.5, 0.2 ] }} + - + name: Step5 + parent: Root + jointType: fixed + material: Stepladder + translation: [ 0.2679491924311227, 0.0, 1.0 ] + elements: + - + type: Shape + translation: [ 0.0, 0.0, -0.01 ] + geometry: { type: Box, size: [ 0.08, 0.6, 0.02 ] } + appearance: { material: { diffuseColor: [ 0.3, 0.5, 0.2 ] }} diff --git a/cnoid/project/CMakeLists.txt b/cnoid/project/CMakeLists.txt index 4f78002..cc20879 100644 --- a/cnoid/project/CMakeLists.txt +++ b/cnoid/project/CMakeLists.txt @@ -14,6 +14,7 @@ endforeach() set(MCC_PROJECT_FILE_LIST MCC_JVRC1_SampleField.in.cnoid + MCC_JVRC1_SampleStepladder.in.cnoid ) foreach(PROJECT_FILE_IN IN LISTS MCC_PROJECT_FILE_LIST) string(REPLACE ".in." "." PROJECT_FILE_OUT "${PROJECT_FILE_IN}") diff --git a/cnoid/project/MCC_JVRC1_SampleStepladder.in.cnoid b/cnoid/project/MCC_JVRC1_SampleStepladder.in.cnoid new file mode 100644 index 0000000..40c7ab0 --- /dev/null +++ b/cnoid/project/MCC_JVRC1_SampleStepladder.in.cnoid @@ -0,0 +1,573 @@ +items: + id: 0 + name: "RootItem" + plugin: Base + class: RootItem + is_builtin_item: true + children: + - + id: 1 + name: "ModelLoader" + plugin: Base + class: ExtCommandItem + data: + command: openhrp-model-loader + executeOnLoading: true + waitingTimeAfterStarted: 0.1 + - + id: 2 + name: "World" + plugin: Body + class: WorldItem + data: + collision_detection: false + collision_detector: AISTCollisionDetector + default_material_table_file: "@MCC_MODEL_DIR@/MCC_materials.yaml" + children: + - + id: 3 + name: "JVRC1" + plugin: Body + class: BodyItem + is_selected: true + is_checked: true + data: + file: "@OPENHRP3_1_PREFIX@/share/OpenHRP-3.1/robot/JVRC1/main.wrl" + currentBaseLink: "PELVIS" + format: CHOREONOID-BODY + rootPosition: [ -0.0164712421, -0.00366884734, 0.823007651 ] + rootAttitude: [ + 0.999520541, 0.000990316288, -0.030946845, + -0.00104419639, 0.999997967, -0.00172494057, + 0.0309450738, 0.00175642811, 0.999519543 ] + jointDisplacements: [ + -21.7615, -0.462606, -0.048014, 42.5543, 0.121295, -19.0174, -21.6492, 1.31035, -0.046925, 42.2226, + -1.25581, -18.7932, 0.015642, 7.60384, 0.1661, -0.000401, 0.009855, 0.010886, -2.6477, -9.07027, + 0.126165, -29.262, 0.004927, 0.075287, 0.012834, 0.126624, 0.177674, -0.126108, -0.160657, -0.126108, + -0.160657, -2.66013, 9.13117, -0.108346, -29.2787, -0.00487, -0.065776, -0.012548, -0.127025, -0.173262, + 0.118258, 0.1551, 0.118258, 0.1551 ] + jointPositions: [ + -0.37981, -0.008074, -0.000838002, 0.742713, 0.002117, -0.331916, -0.37785, 0.0228699, -0.000818996, 0.736923, + -0.021918, -0.328003, 0.000273004, 0.132712, 0.00289899, -6.99877e-06, 0.000172002, 0.000189997, -0.0462111, -0.158306, + 0.00220199, -0.510718, 8.59924e-05, 0.00131401, 0.000223996, 0.00221001, 0.003101, -0.002201, -0.00280399, -0.002201, + -0.00280399, -0.046428, 0.159369, -0.00189099, -0.51101, -8.49975e-05, -0.00114801, -0.000219004, -0.002217, -0.00302399, + 0.00206399, 0.00270701, 0.00206399, 0.00270701 ] + initialRootPosition: [ -0.0164712421, -0.00366884734, 0.823007651 ] + initialRootAttitude: [ + 0.999520541, 0.000990316286, -0.030946845, + -0.00104419639, 0.999997967, -0.00172494057, + 0.0309450738, 0.00175642811, 0.999519543 ] + initialJointPositions: [ + -0.37981, -0.008074, -0.000838002, 0.742713, 0.002117, -0.331916, -0.37785, 0.0228699, -0.000818996, 0.736923, + -0.021918, -0.328003, 0.000273004, 0.132712, 0.00289899, -6.99877e-06, 0.000172002, 0.000189997, -0.0462111, -0.158306, + 0.00220199, -0.510718, 8.59924e-05, 0.00131401, 0.000223996, 0.00221001, 0.003101, -0.002201, -0.00280399, -0.002201, + -0.00280399, -0.046428, 0.159369, -0.00189099, -0.51101, -8.49975e-05, -0.00114801, -0.000219004, -0.002217, -0.00302399, + 0.00206399, 0.00270701, 0.00206399, 0.00270701 ] + fix_root: false + collisionDetection: true + selfCollisionDetection: false + location_editable: true + scene_sensitive: true + zmp: [ 0, 0, 0 ] + children: + - + id: 4 + name: "BodyRTC" + plugin: OpenRTM + class: BodyRTCItem + data: + isNoDelayMode: true + controllerOptions: "" + moduleName: "PDcontroller" + confFileName: "Virtual-JVRC1-RTC.conf" + configurationMode: "Use Configuration File" + autoConnect: true + instanceName: "JVRC1Controller(Robot)0" + bodyPeriodicRate: 0.005 + baseDirectory: "RTC directory" + - + id: 5 + name: "SensorVisualizer" + plugin: Body + class: SensorVisualizerItem + data: + sub_items: + - + class: ForceSensorVisualizerItem + name: ForceSensor + is_checked: true + ratio: 0.002 + threshold: 0.1 + offset: [ 0, 0, 0 ] + - + class: AccelerationSensorVisualizerItem + name: AccelerationSensor + ratio: 0.1 + threshold: 0.05 + offset: [ 0, 0, -9.8 ] + - + class: RateGyroSensorVisualizerItem + name: RateGyro + ratio: 0.4 + threshold: 0.01 + offset: [ 0, 0, 0 ] + - + class: CameraImageVisualizerItem + name: rcamera + - + class: CameraImageVisualizerItem + name: lcamera + - + class: CameraImageVisualizerItem + name: dcamera_Image + - + class: PointCloudVisualizerItem + name: dcamera + translation: [ 0, 0, 0 ] + rotation: [ 1, 0, 0, 0 ] + angle_unit: degree + rendering_mode: Point + point_size: 0 + voxel_size: 0.01 + is_editable: false + - + class: RangeSensorVisualizerItem + name: ranger + translation: [ 0, 0, 0 ] + rotation: [ 1, 0, 0, 0 ] + angle_unit: degree + rendering_mode: Point + point_size: 0 + voxel_size: 0.01 + is_editable: false + - + id: 6 + name: "FloorCheckeredPattern" + plugin: Body + class: BodyItem + is_checked: true + data: + file: "@MCC_MODEL_DIR@/FloorCheckeredPattern.body" + format: CHOREONOID-BODY + rootPosition: [ 0, 0, -0.1 ] + rootAttitude: [ + 1, 0, 0, + 0, 1, 0, + 0, 0, 1 ] + jointPositions: [ ] + initialRootPosition: [ 0, 0, -0.1 ] + initialRootAttitude: [ + 1, 0, 0, + 0, 1, 0, + 0, 0, 1 ] + fix_root: true + collisionDetection: true + selfCollisionDetection: false + location_editable: true + scene_sensitive: true + zmp: [ 0, 0, 0 ] + - + id: 7 + name: "SampleStepladder" + plugin: Body + class: BodyItem + is_checked: true + data: + file: "@MCC_MODEL_DIR@/SampleStepladder.body" + currentBaseLink: "Root" + format: CHOREONOID-BODY + rootPosition: [ 0.2, 0, 0 ] + rootAttitude: [ + 1, 0, 0, + 0, 1, 0, + 0, 0, 1 ] + jointPositions: [ ] + initialRootPosition: [ 0.2, 0, 0 ] + initialRootAttitude: [ + 1, 0, 0, + 0, 1, 0, + 0, 0, 1 ] + fix_root: true + collisionDetection: true + selfCollisionDetection: false + location_editable: true + scene_sensitive: true + zmp: [ 0, 0, 0 ] + - + id: 8 + name: "AISTSimulator" + plugin: Body + class: AISTSimulatorItem + data: + realtime_sync_mode: compensatory + recording: full + time_range_mode: unlimited + time_length: 500 + is_active_control_time_range_mode: false + output_all_link_positions: true + output_device_states: true + use_controller_threads: true + record_collision_data: false + controller_options: "" + block_scene_view_edit_mode: false + dynamicsMode: "Forward dynamics" + integrationMode: runge-kutta + gravity: [ 0, 0, -9.80665 ] + min_friction_coefficient: 0 + max_friction_coefficient: 100 + cullingThresh: 0.01 + contactCullingDepth: 0.05 + errorCriterion: 0.001 + maxNumIterations: 100 + contactCorrectionDepth: 0.0002 + contactCorrectionVelocityRatio: 1 + kinematicWalking: false + 2Dmode: false + oldAccelSensorMode: false + - + id: 9 + name: "sim_mc.py" + plugin: PythonSimScript + class: PythonSimScriptItem + is_checked: true + data: + timing: After init. + delay: 0 + simulationOnly: true + background_execution: true + file: "@HRPSYSBASE_PREFIX@/share/hrpsys/samples/JVRC1/sim_mc.py" + format: PYTHON-SCRIPT-FILE +views: + - + id: 0 + plugin: Base + class: ItemPropertyView + mounted: true + - + id: 1 + plugin: Base + class: ItemTreeView + mounted: true + state: + expanded: [ 2, 3, 4, 7, 8 ] + - + id: 2 + plugin: Base + class: LocationView + state: + show_rpy: true + unique_rpy: false + show_quaternion: false + show_rotation_matrix: false + coordinate: parent + - + id: 3 + plugin: Base + class: MessageView + mounted: true + - + id: 4 + plugin: Base + class: MultiSE3SeqGraphView + state: + mode: view + editMode: freeLine + original: true + velocity: false + acceleration: false + limits: true + grid: true + gridWidth: 0.2 + gridHeight: 0.2 + lineWidth: 1 + rulers: false + sync: true + controlPointStep: 1 + controlPointOffset: 0 + controlPointHeighlight: false + scrollMode: continuous + lower: -10 + upper: 10 + visibleElements: [ 0, 1, 2 ] + - + id: 5 + plugin: Base + class: MultiValueSeqGraphView + state: + mode: view + editMode: freeLine + original: true + velocity: false + acceleration: false + limits: true + grid: true + gridWidth: 0.2 + gridHeight: 0.2 + lineWidth: 1 + rulers: false + sync: true + controlPointStep: 1 + controlPointOffset: 0 + controlPointHeighlight: false + scrollMode: continuous + lower: -10 + upper: 10 + - + id: 6 + plugin: Base + class: SceneView + mounted: true + state: + editMode: false + viewpointOperationMode: thirdPerson + visible_polygon_elements: [ face ] + highlighting: false + collisionLines: true + cameras: + - + camera: [ System, Perspective ] + isCurrent: true + eye: [ 4.50599, 2.27716, 3.22118 ] + direction: [ -0.694451, -0.485365, -0.531186 ] + up: [ -0.435386, -0.304299, 0.847255 ] + - + camera: [ System, Orthographic ] + orthoHeight: 20 + lighting_mode: normal + culling_mode: enabled + shading_mode: smooth + world_light: true + world_light_intensity: 0.5 + ambient_light: true + ambient_light_intensity: 0.3 + head_light: true + head_light_intensity: 0.75 + world_light_shadow: true + shadow_lights: + - { index: 0, enabled: false } + - { index: 1, enabled: false } + background_color: [ 1, 1, 1 ] + line_width: 1 + point_size: 1 + field_of_view: 39.981 + clip_distances: [ 0.01, 10000 ] + floorGrid: false + floorGridSpan: 10 + floorGridInterval: 0.5 + xzGrid: false + xzGridSpan: 10 + xzGridInterval: 0.5 + yzGrid: false + yzGridSpan: 10 + yzGridInterval: 0.5 + xy_grid_color: [ 0.9, 0.9, 0.9 ] + xz_grid_color: [ 0.9, 0.9, 0.9 ] + yz_grid_color: [ 0.9, 0.9, 0.9 ] + - + id: 7 + plugin: Base + class: TaskView + state: + layoutMode: horizontal + isAutoMode: false + - + id: 8 + plugin: Base + class: TextEditView + - + id: 9 + plugin: Body + class: BodyLinkView + state: + showRotationMatrix: false + - + id: 10 + plugin: Body + class: BodyStateView + mounted: true + - + id: 11 + plugin: Body + class: JointDisplacementView + state: + show_selected_joints: false + show_joint_ids: true + show_joint_names: false + overlap_joint_names: false + show_sliders: true + show_dials: false + show_phases: true + limit_ranges: false + - + id: 12 + plugin: Body + class: JointGraphView + state: + mode: view + editMode: freeLine + original: true + velocity: false + acceleration: false + limits: true + grid: true + gridWidth: 0.2 + gridHeight: 0.2 + lineWidth: 1 + rulers: false + sync: true + controlPointStep: 1 + controlPointOffset: 0 + controlPointHeighlight: false + scrollMode: continuous + lower: -10 + upper: 10 + - + id: 13 + plugin: Body + class: JointStateView + mounted: true + - + id: 14 + plugin: Body + class: LinkDeviceListView + state: + element_type: link + listing_mode: list + - + id: 15 + plugin: Body + class: LinkPositionView + state: + coordinate_mode: world + preferred_coordinate_mode: base + show_rpy: true + unique_rpy: false + show_quaternion: false + show_rotation_matrix: false + target_link_type: ik_link + - + id: 16 + plugin: Body + class: LinkPropertyView + - + id: 17 + plugin: Corba + class: NameServerView + - + id: 18 + plugin: OpenRTM + class: RTSNameServerView + state: + NameServers: + - + isDefaultNameServer: false + host: "localhost" + port: 2809 + - + id: 19 + plugin: Python + class: PythonConsoleView + mounted: true +toolbars: + "TimeBar": + current_time: 0 + min_time: 0 + max_time: 60 + frame_rate: 1000 + playback_frame_rate: 50 + idle_loop_driven_mode: false + playback_speed_ratio: 1 + sync_to_ongoing_updates: true + auto_expansion: true + "LeggedBodyBar": + stanceWidth: 0.15 + "KinematicsBar": + forward_kinematics: true + inverse_kinematics: true + preset_kinematics: true + enablePositionDragger: true + penetrationBlock: true + collisionLinkHighlight: false + snapDistance: 0.025 + penetrationBlockDepth: 0.0005 + lazyCollisionDetectionMode: true +Body: + "BodySelectionManager": + current_body_item: 3 + current_link: "PELVIS" + "KinematicFaultChecker": + checkJointPositions: true + angleMargin: 0 + translationMargin: 0 + checkJointVelocities: true + velocityLimitRatio: 100 + targetJoints: all + checkSelfCollisions: true + onlyTimeBarRange: false + "OperableSceneBody": + scene_bodies: + - + body_item: 3 + show_cm: false + show_cm_projection: false + show_zmp: false + - + body_item: 6 + show_cm: false + show_cm_projection: false + show_zmp: false + - + body_item: 7 + show_cm: false + show_cm_projection: false + show_zmp: false + "SplineFilterDialog": + isInputFrameRateSpecified: false + inputFrameRate: 200 + isOutputFrameRateSpecified: false + outputFrameRate: 200 +OpenRTM: + "deleteUnmanagedRTCsOnStartingSimulation": true +Python: + "moduleSearchPath": + - @HRPSYSBASE_PREFIX@/lib/python2.7/dist-packages/hrpsys +viewAreas: + - + type: embedded + tabs: true + contents: + type: splitter + orientation: vertical + sizes: [ 1271, 293 ] + children: + - + type: splitter + orientation: horizontal + sizes: [ 304, 1216 ] + children: + - + type: splitter + orientation: vertical + sizes: [ 579, 684 ] + children: + - + type: pane + views: [ 1, 13 ] + current: 1 + - + type: pane + views: [ 0 ] + - + type: pane + views: [ 6 ] + - + type: pane + views: [ 3, 19, 10 ] + current: 3 +layoutOfToolBars: + rows: + - + - { name: "FileBar", x: 0, priority: 2 } + - { name: "SimulationBar", x: 0, priority: 1 } + - { name: "TimeBar", x: 47, priority: 0 } + - { name: "SceneBar", x: 1640, priority: 3 } From 7d3d5b0cf84fbe0863bacc562ed898d414f98a3f Mon Sep 17 00:00:00 2001 From: Masaki Murooka Date: Mon, 27 Mar 2023 09:44:26 +0900 Subject: [PATCH 2/7] [description] Add SampleStepladder.urdf --- description/urdf/SampleField.urdf | 8 +- description/urdf/SampleStepladder.urdf | 139 +++++++++++++++++++++++++ 2 files changed, 140 insertions(+), 7 deletions(-) create mode 100644 description/urdf/SampleStepladder.urdf diff --git a/description/urdf/SampleField.urdf b/description/urdf/SampleField.urdf index 412aa6a..7a191c4 100644 --- a/description/urdf/SampleField.urdf +++ b/description/urdf/SampleField.urdf @@ -2,13 +2,7 @@ - - - - - - - + diff --git a/description/urdf/SampleStepladder.urdf b/description/urdf/SampleStepladder.urdf new file mode 100644 index 0000000..53c88e0 --- /dev/null +++ b/description/urdf/SampleStepladder.urdf @@ -0,0 +1,139 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + From 17062f39585d2cd28a0b2a563afc16d2a54141fe Mon Sep 17 00:00:00 2001 From: Masaki Murooka Date: Mon, 27 Mar 2023 12:08:06 +0900 Subject: [PATCH 3/7] [cnoid, description] Update ampleStepladder. --- cnoid/model/SampleStepladder.body | 16 ++------------- description/urdf/SampleStepladder.urdf | 27 ++++---------------------- 2 files changed, 6 insertions(+), 37 deletions(-) diff --git a/cnoid/model/SampleStepladder.body b/cnoid/model/SampleStepladder.body index c0dd26d..02af25c 100644 --- a/cnoid/model/SampleStepladder.body +++ b/cnoid/model/SampleStepladder.body @@ -19,7 +19,7 @@ links: type: Shape translation: [ 0.0, 0.0, 1.0 ] rotation: [ 1, 0, 0, 1.5707963267948966 ] - geometry: { type: Cylinder, radius: 0.015, height: 2.0 } + geometry: { type: Box, size: [ 0.02, 2.0, 0.02 ] } appearance: { material: { diffuseColor: [ 0.3, 0.5, 0.2 ] }} - name: HandrailRight @@ -33,7 +33,7 @@ links: type: Shape translation: [ 0.0, 0.0, 1.0 ] rotation: [ 1, 0, 0, 1.5707963267948966 ] - geometry: { type: Cylinder, radius: 0.015, height: 2.0 } + geometry: { type: Box, size: [ 0.02, 2.0, 0.02 ] } appearance: { material: { diffuseColor: [ 0.3, 0.5, 0.2 ] }} - name: Step1 @@ -83,15 +83,3 @@ links: translation: [ 0.0, 0.0, -0.01 ] geometry: { type: Box, size: [ 0.08, 0.6, 0.02 ] } appearance: { material: { diffuseColor: [ 0.3, 0.5, 0.2 ] }} - - - name: Step5 - parent: Root - jointType: fixed - material: Stepladder - translation: [ 0.2679491924311227, 0.0, 1.0 ] - elements: - - - type: Shape - translation: [ 0.0, 0.0, -0.01 ] - geometry: { type: Box, size: [ 0.08, 0.6, 0.02 ] } - appearance: { material: { diffuseColor: [ 0.3, 0.5, 0.2 ] }} diff --git a/description/urdf/SampleStepladder.urdf b/description/urdf/SampleStepladder.urdf index 53c88e0..ddb3f27 100644 --- a/description/urdf/SampleStepladder.urdf +++ b/description/urdf/SampleStepladder.urdf @@ -10,9 +10,9 @@ - + - + @@ -24,9 +24,9 @@ - + - + @@ -87,20 +87,6 @@ - - - - - - - - - - - - - - @@ -131,9 +117,4 @@ - - - - - From 83922b950a7bb0d11783f0f0ab9cff93983f6524 Mon Sep 17 00:00:00 2001 From: Masaki Murooka Date: Mon, 27 Mar 2023 12:08:39 +0900 Subject: [PATCH 4/7] [CI] Add MotionSampleStepladder.yaml --- .../config/MotionSampleStepladder.yaml | 360 ++++++++++++++++++ 1 file changed, 360 insertions(+) create mode 100644 .github/workflows/config/MotionSampleStepladder.yaml diff --git a/.github/workflows/config/MotionSampleStepladder.yaml b/.github/workflows/config/MotionSampleStepladder.yaml new file mode 100644 index 0000000..4bd28f9 --- /dev/null +++ b/.github/workflows/config/MotionSampleStepladder.yaml @@ -0,0 +1,360 @@ +transitions: + - [MCC::Initial_, OK, MCC::ConfigMotion1_, Auto] + - [MCC::ConfigMotion1_, OK, MCC::ConfigMotion2_, Auto] + +states: + MCC::Initial_: + base: MCC::Initial + configs: + autoStartTime: 2.0 + + MCC::ConfigMotion1_: + base: MCC::ConfigMotion + configs: + baseTime: Relative + stepCommandList: + - limb: LeftHand + pose: + translation: [0.5215390309173472, 0.3, 1.2] + rotation: [0.0, -1.3089969389957472, 0.0] + swingCommand: + type: Add + startTime: 3.0 + endTime: 7.0 + config: + approachDurationRatio: 0.15 + approachOffset: [0, 0, 0.03] + swingOffset: [0, 0, 0.12] + contactCommandList: + - time: 7.0 + constraint: + type: Empty + - time: 8.0 + constraint: + type: Grasp + fricCoeff: 1.0 + gripperCommandList: + - time: 2.0 + name: l_gripper + config: + opening: 1.0 + - time: 7.0 + name: l_gripper + config: + opening: 0.0 + - limb: RightHand + pose: + translation: [0.5215390309173472, -0.3, 1.2] + rotation: [0.0, -1.3089969389957472, 0.0] + swingCommand: + type: Add + startTime: 3.0 + endTime: 7.0 + config: + approachDurationRatio: 0.15 + approachOffset: [0, 0, 0.03] + swingOffset: [0, 0, 0.12] + contactCommandList: + - time: 7.0 + constraint: + type: Empty + - time: 8.0 + constraint: + type: Grasp + fricCoeff: 1.0 + gripperCommandList: + - time: 2.0 + name: r_gripper + config: + opening: 1.0 + - time: 7.0 + name: r_gripper + config: + opening: 0.0 + - limb: LeftFoot + type: Add + startTime: 9.0 + endTime: 11.0 + pose: + translation: [0.19358983848622455, 0.11, 0.2] + constraint: + type: Surface + fricCoeff: 0.5 + verticesName: LeftFootOnStep + swingConfig: + withdrawDurationRatio: 0.3 + withdrawOffset: [-0.03, 0, 0.1] + approachDurationRatio: 0.2 + approachOffset: [0, 0, 0.02] + swingOffset: [-0.05, 0, 0.1] + - limb: RightFoot + type: Add + startTime: 12.0 + endTime: 14.0 + pose: + translation: [0.19358983848622455, -0.11, 0.2] + constraint: + type: Surface + fricCoeff: 0.5 + verticesName: RightFootOnStep + swingConfig: + withdrawDurationRatio: 0.3 + withdrawOffset: [-0.03, 0, 0.1] + approachDurationRatio: 0.2 + approachOffset: [0, 0, 0.02] + swingOffset: [-0.05, 0, 0.1] + nominalCentroidalPoseList: + - time: 9.0 + pose: + translation: [-0.1, 0.0, 0.875] + - time: 11.5 + pose: + translation: [-0.07320508075688774, 0.0, 0.925] + - time: 14.0 + pose: + translation: [-0.046410161513775465, 0.0, 1.075] + + MCC::ConfigMotion2_: + base: MCC::ConfigMotion + configs: + baseTime: Relative + stepCommandList: + - limb: LeftHand + pose: + translation: [0.57512886940357176, 0.3, 1.4] + rotation: [0.0, -1.3089969389957472, 0.0] + swingCommand: + type: Add + startTime: 3.0 + endTime: 6.0 + config: + withdrawDurationRatio: 0.25 + withdrawOffset: [0, 0, 0.02] + approachDurationRatio: 0.25 + approachOffset: [0, 0, 0.02] + swingOffset: [0, 0, 0.05] + contactCommandList: + - time: 2.0 + constraint: + type: Empty + - time: 3.0 + constraint: {} + - time: 6.0 + constraint: + type: Empty + - time: 7.0 + constraint: + type: Grasp + fricCoeff: 1.0 + gripperCommandList: + - time: 2.0 + name: l_gripper + config: + opening: 1.0 + - time: 6.0 + name: l_gripper + config: + opening: 0.0 + - limb: RightHand + pose: + translation: [0.57512886940357176, -0.3, 1.4] + rotation: [0.0, -1.3089969389957472, 0.0] + swingCommand: + type: Add + startTime: 9.0 + endTime: 12.0 + config: + withdrawDurationRatio: 0.25 + withdrawOffset: [0, 0, 0.02] + approachDurationRatio: 0.25 + approachOffset: [0, 0, 0.02] + swingOffset: [0, 0, 0.05] + contactCommandList: + - time: 8.0 + constraint: + type: Empty + - time: 9.0 + constraint: {} + - time: 12.0 + constraint: + type: Empty + - time: 13.0 + constraint: + type: Grasp + fricCoeff: 1.0 + gripperCommandList: + - time: 8.0 + name: r_gripper + config: + opening: 1.0 + - time: 12.0 + name: r_gripper + config: + opening: 0.0 + - limb: LeftFoot + type: Add + startTime: 14.0 + endTime: 16.0 + pose: + translation: [0.24717967697244908, 0.11, 0.4] + constraint: + type: Surface + fricCoeff: 0.5 + verticesName: LeftFootOnStep + swingConfig: + withdrawDurationRatio: 0.3 + withdrawOffset: [-0.05, 0, 0.1] + approachDurationRatio: 0.2 + approachOffset: [0, 0, 0.02] + swingOffset: [-0.06, 0, 0.1] + - limb: RightFoot + type: Add + startTime: 17.0 + endTime: 19.0 + pose: + translation: [0.24717967697244908, -0.11, 0.4] + constraint: + type: Surface + fricCoeff: 0.5 + verticesName: RightFootOnStep + swingConfig: + withdrawDurationRatio: 0.3 + withdrawOffset: [-0.05, 0, 0.1] + approachDurationRatio: 0.2 + approachOffset: [0, 0, 0.02] + swingOffset: [-0.06, 0, 0.1] + nominalCentroidalPoseList: + - time: 14.0 + pose: + translation: [-0.046410161513775465, 0.0, 1.075] + - time: 16.5 + pose: + translation: [-0.019615242270663205, 0.0, 1.125] + - time: 19.0 + pose: + translation: [0.0071796769724490755, 0.0, 1.275] + +LimbTaskList: + - type: firstOrderImpedance + limb: LeftFoot + frame: LeftFootCenter + cutoffPeriod: 0.01 + weight: 1000.0 + - type: firstOrderImpedance + limb: RightFoot + frame: RightFootCenter + cutoffPeriod: 0.01 + weight: 1000.0 + - type: firstOrderImpedance + limb: LeftHand + frame: LeftHandGraspFrame + cutoffPeriod: 0.01 + weight: 1000.0 + - type: firstOrderImpedance + limb: RightHand + frame: RightHandGraspFrame + cutoffPeriod: 0.01 + weight: 1000.0 + +LimbManagerSet: + LimbManager: + Hand: + impedanceGains: + MultiContact: + damper: + linear: [5e4, 5e4, 5e4] + angular: [100, 100, 100] + spring: + linear: [0, 0, 0] + angular: [0, 0, 2000] + wrench: + linear: [1, 1, 1] + angular: [1, 1, 0] + +CentroidalManager: + nominalCentroidalPoseBaseFrame: World + mpcWeightParam: + runningPos: [10.0, 10.0, 10.0] + terminalPos: [10.0, 10.0, 10.0] + +OverwriteConfigList: + NoSensors: + LimbManagerSet: + LimbManager: + Hand: + impedanceGains: + SingleContact: + wrench: + linear: [0, 0, 0] + angular: [0, 0, 0] + MultiContact: + wrench: + linear: [0, 0, 0] + angular: [0, 0, 0] + Swing: + wrench: + linear: [0, 0, 0] + angular: [0, 0, 0] + +robots: + # Environment models + sample_stepladder: + module: MCC/SampleStepladder + init_pos: + translation: [0.2, 0.0, 0.0] + + # Robot-specific configurations + jvrc1: + frames: + - name: LeftHandGraspFrame + parent: l_wrist + X_p_f: + translation: [0.0, -0.0085, -0.09] + - name: RightHandGraspFrame + parent: r_wrist + X_p_f: + translation: [0.0, 0.0085, -0.09] + + PostureTask: + jointWeights: + WAIST_R: 1000 + WAIST_P: 1000 + WAIST_Y: 1000 + target: + R_SHOULDER_P: [0.2617993877991494] # 15 + R_ELBOW_P: [-1.5707963267948966] # -90 + R_ELBOW_Y: [1.5707963267948966] # 90 + R_WRIST_Y: [-1.5707963267948966] # -90 + L_SHOULDER_P: [0.2617993877991494] # 15 + L_ELBOW_P: [-1.5707963267948966] # -90 + L_ELBOW_Y: [-1.5707963267948966] # -90 + L_WRIST_Y: [1.5707963267948966] # 90 + + CentroidalManager: + nominalCentroidalPose: + translation: [0.0, 0.0, 0.85] + + Contacts: + Surface: + - name: LeftFoot + vertices: [[-0.1, -0.04, 0.0], [-0.1, 0.04, 0.0], [0.1, 0.04, 0.0], [0.1, -0.04, 0.0]] + - name: RightFoot + vertices: [[-0.1, -0.04, 0.0], [-0.1, 0.04, 0.0], [0.1, 0.04, 0.0], [0.1, -0.04, 0.0]] + - name: LeftFootOnStep + vertices: [[0.03, -0.04, 0.0], [0.03, 0.04, 0.0], [0.08, 0.04, 0.0], [0.08, -0.04, 0.0]] + - name: RightFootOnStep + vertices: [[0.03, -0.04, 0.0], [0.03, 0.04, 0.0], [0.08, 0.04, 0.0], [0.08, -0.04, 0.0]] + + Grasp: + - name: LeftHand + vertices: + - translation: [0.0, 0.015, 0] + rotation: [1.5707963267948966, 0, 0] + - translation: [0.0, -0.015, 0] + rotation: [-1.5707963267948966, 0, 0] + - name: RightHand + vertices: + - translation: [0.0, 0.015, 0] + rotation: [1.5707963267948966, 0, 0] + - translation: [0.0, -0.015, 0] + rotation: [-1.5707963267948966, 0, 0] From 34038106b8f5de4de5da87a9381e13b636ccc4a0 Mon Sep 17 00:00:00 2001 From: Masaki Murooka Date: Mon, 27 Mar 2023 13:33:35 +0900 Subject: [PATCH 5/7] [cnoid] Update the camera pose of MCC_JVRC1_SampleStepladder.in.cnoid --- cnoid/project/MCC_JVRC1_SampleStepladder.in.cnoid | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/cnoid/project/MCC_JVRC1_SampleStepladder.in.cnoid b/cnoid/project/MCC_JVRC1_SampleStepladder.in.cnoid index 40c7ab0..c4381b8 100644 --- a/cnoid/project/MCC_JVRC1_SampleStepladder.in.cnoid +++ b/cnoid/project/MCC_JVRC1_SampleStepladder.in.cnoid @@ -327,9 +327,9 @@ views: - camera: [ System, Perspective ] isCurrent: true - eye: [ 4.50599, 2.27716, 3.22118 ] - direction: [ -0.694451, -0.485365, -0.531186 ] - up: [ -0.435386, -0.304299, 0.847255 ] + eye: [ -0.522057, -3.37103, 2.57327 ] + direction: [ 0.381657, 0.833936, -0.398609 ] + up: [ 0.16588, 0.362454, 0.917121 ] - camera: [ System, Orthographic ] orthoHeight: 20 From 2f339796281a9b96e7981831406ee0bd4399e8ce Mon Sep 17 00:00:00 2001 From: Masaki Murooka Date: Mon, 27 Mar 2023 20:16:41 +0900 Subject: [PATCH 6/7] [CI] Update config/MotionSampleStepladder.yaml --- .github/workflows/config/MotionSampleStepladder.yaml | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/.github/workflows/config/MotionSampleStepladder.yaml b/.github/workflows/config/MotionSampleStepladder.yaml index 4bd28f9..56aed53 100644 --- a/.github/workflows/config/MotionSampleStepladder.yaml +++ b/.github/workflows/config/MotionSampleStepladder.yaml @@ -106,13 +106,13 @@ states: nominalCentroidalPoseList: - time: 9.0 pose: - translation: [-0.1, 0.0, 0.875] + translation: [-0.1, 0.0, 0.85] - time: 11.5 pose: translation: [-0.07320508075688774, 0.0, 0.925] - time: 14.0 pose: - translation: [-0.046410161513775465, 0.0, 1.075] + translation: [-0.026410161513775465, 0.0, 1.05] MCC::ConfigMotion2_: base: MCC::ConfigMotion @@ -226,13 +226,13 @@ states: nominalCentroidalPoseList: - time: 14.0 pose: - translation: [-0.046410161513775465, 0.0, 1.075] + translation: [-0.046410161513775465, 0.0, 1.05] - time: 16.5 pose: translation: [-0.019615242270663205, 0.0, 1.125] - time: 19.0 pose: - translation: [0.0071796769724490755, 0.0, 1.275] + translation: [0.0271796769724490755, 0.0, 1.25] LimbTaskList: - type: firstOrderImpedance From 62c6481b44dd81ab62062dff652837fc61a87955 Mon Sep 17 00:00:00 2001 From: Masaki Murooka Date: Mon, 27 Mar 2023 20:16:41 +0900 Subject: [PATCH 7/7] [CI] Update config/MotionSampleStepladder.yaml --- .github/workflows/config/MotionSampleStepladder.yaml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/workflows/config/MotionSampleStepladder.yaml b/.github/workflows/config/MotionSampleStepladder.yaml index 56aed53..ae813ab 100644 --- a/.github/workflows/config/MotionSampleStepladder.yaml +++ b/.github/workflows/config/MotionSampleStepladder.yaml @@ -309,11 +309,11 @@ robots: - name: LeftHandGraspFrame parent: l_wrist X_p_f: - translation: [0.0, -0.0085, -0.09] + translation: [0.0, -0.0085, -0.08] - name: RightHandGraspFrame parent: r_wrist X_p_f: - translation: [0.0, 0.0085, -0.09] + translation: [0.0, 0.0085, -0.08] PostureTask: jointWeights: