From 52c137deaf9497f3a32c64bf30691e3022a1fd8c Mon Sep 17 00:00:00 2001 From: Keitaro Murakami Date: Fri, 26 Nov 2021 16:28:20 +0900 Subject: [PATCH 1/9] [hrpsys_choreonoid_tutorials] rtc in the same order as trans_ros_bridge/urata_real_hrpsys_config.py --- .../scripts/chidori_rh_setup.py | 6 +++--- .../scripts/jaxon_blue_rh_setup.py | 10 +++++----- .../scripts/jaxon_red_rh_setup.py | 10 +++++----- hrpsys_choreonoid_tutorials/scripts/jaxon_red_setup.py | 10 +++++----- .../choreonoid_hrpsys_config.py | 6 +++--- 5 files changed, 21 insertions(+), 21 deletions(-) diff --git a/hrpsys_choreonoid_tutorials/scripts/chidori_rh_setup.py b/hrpsys_choreonoid_tutorials/scripts/chidori_rh_setup.py index d075287b..1ce9d1e1 100755 --- a/hrpsys_choreonoid_tutorials/scripts/chidori_rh_setup.py +++ b/hrpsys_choreonoid_tutorials/scripts/chidori_rh_setup.py @@ -13,15 +13,15 @@ def getRTCList (self): ['kf', "KalmanFilter"], #['vs', "VirtualForceSensor"], ['rmfo', "RemoveForceSensorLinkOffset"], + ['octd', "ObjectContactTurnaroundDetector"], ['es', "EmergencyStopper"], ['rfu', "ReferenceForceUpdater"], - ['octd', "ObjectContactTurnaroundDetector"], ['ic', "ImpedanceController"], ['abc', "AutoBalancer"], ['st', "Stabilizer"], - # ['tc', "TorqueController"], - # ['te', "ThermoEstimator"], # ['tl', "ThermoLimiter"], + # ['te', "ThermoEstimator"], + # ['tc', "TorqueController"], ['co', "CollisionDetector"], ['hes', "EmergencyStopper"], ['el', "SoftErrorLimiter"], diff --git a/hrpsys_choreonoid_tutorials/scripts/jaxon_blue_rh_setup.py b/hrpsys_choreonoid_tutorials/scripts/jaxon_blue_rh_setup.py index a632c136..0706cb78 100755 --- a/hrpsys_choreonoid_tutorials/scripts/jaxon_blue_rh_setup.py +++ b/hrpsys_choreonoid_tutorials/scripts/jaxon_blue_rh_setup.py @@ -13,16 +13,16 @@ def getRTCList (self): ['kf', "KalmanFilter"], ['vs', "VirtualForceSensor"], ['rmfo', "RemoveForceSensorLinkOffset"], + ['octd', "ObjectContactTurnaroundDetector"], ['es', "EmergencyStopper"], + ['rfu', "ReferenceForceUpdater"], ['ic', "ImpedanceController"], ['abc', "AutoBalancer"], ['st', "Stabilizer"], - ['co', "CollisionDetector"], - # ['tc', "TorqueController"], - # ['te', "ThermoEstimator"], # ['tl', "ThermoLimiter"], - ['rfu', "ReferenceForceUpdater"], - ['octd', "ObjectContactTurnaroundDetector"], + # ['te', "ThermoEstimator"], + # ['tc', "TorqueController"], + ['co', "CollisionDetector"], ['hes', "EmergencyStopper"], ['el', "SoftErrorLimiter"], ['log', "DataLogger"] diff --git a/hrpsys_choreonoid_tutorials/scripts/jaxon_red_rh_setup.py b/hrpsys_choreonoid_tutorials/scripts/jaxon_red_rh_setup.py index 4538ed10..060d2c18 100755 --- a/hrpsys_choreonoid_tutorials/scripts/jaxon_red_rh_setup.py +++ b/hrpsys_choreonoid_tutorials/scripts/jaxon_red_rh_setup.py @@ -13,16 +13,16 @@ def getRTCList (self): ['kf', "KalmanFilter"], ['vs', "VirtualForceSensor"], ['rmfo', "RemoveForceSensorLinkOffset"], + ['octd', "ObjectContactTurnaroundDetector"], ['es', "EmergencyStopper"], + ['rfu', "ReferenceForceUpdater"], ['ic', "ImpedanceController"], ['abc', "AutoBalancer"], ['st', "Stabilizer"], - ['co', "CollisionDetector"], - # ['tc', "TorqueController"], - # ['te', "ThermoEstimator"], # ['tl', "ThermoLimiter"], - ['rfu', "ReferenceForceUpdater"], - ['octd', "ObjectContactTurnaroundDetector"], + # ['te', "ThermoEstimator"], + # ['tc', "TorqueController"], + ['co', "CollisionDetector"], ['hes', "EmergencyStopper"], ['el', "SoftErrorLimiter"], ['log', "DataLogger"] diff --git a/hrpsys_choreonoid_tutorials/scripts/jaxon_red_setup.py b/hrpsys_choreonoid_tutorials/scripts/jaxon_red_setup.py index 366b38af..ac4b025b 100755 --- a/hrpsys_choreonoid_tutorials/scripts/jaxon_red_setup.py +++ b/hrpsys_choreonoid_tutorials/scripts/jaxon_red_setup.py @@ -13,16 +13,16 @@ def getRTCList (self): ['kf', "KalmanFilter"], ['vs', "VirtualForceSensor"], ['rmfo', "RemoveForceSensorLinkOffset"], + ['octd', "ObjectContactTurnaroundDetector"], ['es', "EmergencyStopper"], + ['rfu', "ReferenceForceUpdater"], ['ic', "ImpedanceController"], ['abc', "AutoBalancer"], ['st', "Stabilizer"], - ['co', "CollisionDetector"], - # ['tc', "TorqueController"], - # ['te', "ThermoEstimator"], # ['tl', "ThermoLimiter"], - ['rfu', "ReferenceForceUpdater"], - ['octd', "ObjectContactTurnaroundDetector"], + # ['te', "ThermoEstimator"], + # ['tc', "TorqueController"], + ['co', "CollisionDetector"], ['hes', "EmergencyStopper"], ['el', "SoftErrorLimiter"], ['log', "DataLogger"] diff --git a/hrpsys_choreonoid_tutorials/src/hrpsys_choreonoid_tutorials/choreonoid_hrpsys_config.py b/hrpsys_choreonoid_tutorials/src/hrpsys_choreonoid_tutorials/choreonoid_hrpsys_config.py index 56794004..b629c26d 100755 --- a/hrpsys_choreonoid_tutorials/src/hrpsys_choreonoid_tutorials/choreonoid_hrpsys_config.py +++ b/hrpsys_choreonoid_tutorials/src/hrpsys_choreonoid_tutorials/choreonoid_hrpsys_config.py @@ -17,10 +17,10 @@ def getRTCList (self): ['ic', "ImpedanceController"], ['abc', "AutoBalancer"], ['st', "Stabilizer"], - ['co', "CollisionDetector"], - # ['tc', "TorqueController"], - # ['te', "ThermoEstimator"], # ['tl', "ThermoLimiter"], + # ['te', "ThermoEstimator"], + # ['tc', "TorqueController"], + ['co', "CollisionDetector"], ['hes', "EmergencyStopper"], ['el', "SoftErrorLimiter"], ['log', "DataLogger"] From 26a291ef8fca99e4609ef62a0b59520c55803b7a Mon Sep 17 00:00:00 2001 From: Yuta Kojio Date: Thu, 31 Mar 2022 15:03:21 +0900 Subject: [PATCH 2/9] rm st rtc --- .travis | 2 +- hrpsys_choreonoid_tutorials/scripts/jaxon_blue_rh_setup.py | 4 ++-- hrpsys_choreonoid_tutorials/scripts/jaxon_red_rh_setup.py | 4 ++-- hrpsys_choreonoid_tutorials/scripts/jaxon_red_setup.py | 4 ++-- 4 files changed, 7 insertions(+), 7 deletions(-) diff --git a/.travis b/.travis index 0955a49e..54cfece8 160000 --- a/.travis +++ b/.travis @@ -1 +1 @@ -Subproject commit 0955a49eb558dbcc8f25c032668ad25774c41a8b +Subproject commit 54cfece8bf0ab09877f00070a64a93fc507b68e7 diff --git a/hrpsys_choreonoid_tutorials/scripts/jaxon_blue_rh_setup.py b/hrpsys_choreonoid_tutorials/scripts/jaxon_blue_rh_setup.py index 0706cb78..5a1128b9 100755 --- a/hrpsys_choreonoid_tutorials/scripts/jaxon_blue_rh_setup.py +++ b/hrpsys_choreonoid_tutorials/scripts/jaxon_blue_rh_setup.py @@ -18,7 +18,7 @@ def getRTCList (self): ['rfu', "ReferenceForceUpdater"], ['ic', "ImpedanceController"], ['abc', "AutoBalancer"], - ['st', "Stabilizer"], + # ['st', "Stabilizer"], # ['tl', "ThermoLimiter"], # ['te', "ThermoEstimator"], # ['tc', "TorqueController"], @@ -48,10 +48,10 @@ def startABSTIMP (self): # self.el_svc.setServoErrorLimit("LARM_F_JOINT0", sys.float_info.max) # self.el_svc.setServoErrorLimit("LARM_F_JOINT1", sys.float_info.max) ### - self.startAutoBalancer() # Suppress limit over message and behave like real robot that always angle-vector is in seq. # Latter four 0.0 are for hands. self.seq_svc.setJointAngles(self.jaxonResetPose(), 1.0) + self.abc_svc.startAutoBalancer(['rleg','lleg']) self.ic_svc.startImpedanceController("larm") self.ic_svc.startImpedanceController("rarm") self.startStabilizer() diff --git a/hrpsys_choreonoid_tutorials/scripts/jaxon_red_rh_setup.py b/hrpsys_choreonoid_tutorials/scripts/jaxon_red_rh_setup.py index 060d2c18..85dc94cb 100755 --- a/hrpsys_choreonoid_tutorials/scripts/jaxon_red_rh_setup.py +++ b/hrpsys_choreonoid_tutorials/scripts/jaxon_red_rh_setup.py @@ -18,7 +18,7 @@ def getRTCList (self): ['rfu', "ReferenceForceUpdater"], ['ic', "ImpedanceController"], ['abc', "AutoBalancer"], - ['st', "Stabilizer"], + # ['st', "Stabilizer"], # ['tl', "ThermoLimiter"], # ['te', "ThermoEstimator"], # ['tc', "TorqueController"], @@ -48,10 +48,10 @@ def startABSTIMP (self): self.el_svc.setServoErrorLimit("LARM_F_JOINT0", sys.float_info.max) self.el_svc.setServoErrorLimit("LARM_F_JOINT1", sys.float_info.max) ### - self.startAutoBalancer() # Suppress limit over message and behave like real robot that always angle-vector is in seq. # Latter four 0.0 are for hands. self.seq_svc.setJointAngles(self.jaxonResetPose()+[0.0, 0.0, 0.0, 0.0] , 1.0) + self.abc_svc.startAutoBalancer(['rleg','lleg']) self.ic_svc.startImpedanceController("larm") self.ic_svc.startImpedanceController("rarm") self.startStabilizer() diff --git a/hrpsys_choreonoid_tutorials/scripts/jaxon_red_setup.py b/hrpsys_choreonoid_tutorials/scripts/jaxon_red_setup.py index ac4b025b..feb7270c 100755 --- a/hrpsys_choreonoid_tutorials/scripts/jaxon_red_setup.py +++ b/hrpsys_choreonoid_tutorials/scripts/jaxon_red_setup.py @@ -18,7 +18,7 @@ def getRTCList (self): ['rfu', "ReferenceForceUpdater"], ['ic', "ImpedanceController"], ['abc', "AutoBalancer"], - ['st', "Stabilizer"], + # ['st', "Stabilizer"], # ['tl', "ThermoLimiter"], # ['te', "ThermoEstimator"], # ['tc', "TorqueController"], @@ -48,10 +48,10 @@ def startABSTIMP (self): self.el_svc.setServoErrorLimit("LARM_F_JOINT0", sys.float_info.max) self.el_svc.setServoErrorLimit("LARM_F_JOINT1", sys.float_info.max) ### - self.startAutoBalancer() # Suppress limit over message and behave like real robot that always angle-vector is in seq. # Latter four 0.0 are for hands. self.seq_svc.setJointAngles(self.jaxonResetPose()+[0.0, 0.0, 0.0, 0.0] , 1.0) + self.abc_svc.startAutoBalancer(['rleg','lleg']) self.ic_svc.startImpedanceController("larm") self.ic_svc.startImpedanceController("rarm") self.startStabilizer() From 117e28b2d1d125006076014f1b2656a8243a5471 Mon Sep 17 00:00:00 2001 From: Yuta Kojio Date: Tue, 15 Feb 2022 17:25:57 +0900 Subject: [PATCH 3/9] add JAXON_BLUE_RH_LOAD_OBJ.cnoid.in --- hrpsys_choreonoid_tutorials/CMakeLists.txt | 1 + .../config/JAXON_BLUE_RH_LOAD_OBJ.cnoid.in | 366 ++++++++++++++++++ .../launch/jaxon_blue_choreonoid.launch | 18 +- 3 files changed, 379 insertions(+), 6 deletions(-) create mode 100644 hrpsys_choreonoid_tutorials/config/JAXON_BLUE_RH_LOAD_OBJ.cnoid.in diff --git a/hrpsys_choreonoid_tutorials/CMakeLists.txt b/hrpsys_choreonoid_tutorials/CMakeLists.txt index e27d0c42..371d3804 100644 --- a/hrpsys_choreonoid_tutorials/CMakeLists.txt +++ b/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() ### diff --git a/hrpsys_choreonoid_tutorials/config/JAXON_BLUE_RH_LOAD_OBJ.cnoid.in b/hrpsys_choreonoid_tutorials/config/JAXON_BLUE_RH_LOAD_OBJ.cnoid.in new file mode 100644 index 00000000..ed1cbb53 --- /dev/null +++ b/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 } diff --git a/hrpsys_choreonoid_tutorials/launch/jaxon_blue_choreonoid.launch b/hrpsys_choreonoid_tutorials/launch/jaxon_blue_choreonoid.launch index f0a9de3a..1e52c736 100644 --- a/hrpsys_choreonoid_tutorials/launch/jaxon_blue_choreonoid.launch +++ b/hrpsys_choreonoid_tutorials/launch/jaxon_blue_choreonoid.launch @@ -4,6 +4,7 @@ + @@ -16,14 +17,19 @@ - - + name="taskname" value="LOAD_OBJ" /> + name="taskname" value="$(arg TASK)" /> + + + + + From 2c430dc51174d8f7080d9711621933d3d5ba8a24 Mon Sep 17 00:00:00 2001 From: Yuta Kojio Date: Thu, 31 Mar 2022 16:52:34 +0900 Subject: [PATCH 4/9] up --- hrpsys_choreonoid_tutorials/scripts/jaxon_blue_rh_setup.py | 4 ++-- hrpsys_choreonoid_tutorials/scripts/jaxon_red_rh_setup.py | 4 ++-- hrpsys_choreonoid_tutorials/scripts/jaxon_red_setup.py | 4 ++-- 3 files changed, 6 insertions(+), 6 deletions(-) diff --git a/hrpsys_choreonoid_tutorials/scripts/jaxon_blue_rh_setup.py b/hrpsys_choreonoid_tutorials/scripts/jaxon_blue_rh_setup.py index 5a1128b9..dc40b4d4 100755 --- a/hrpsys_choreonoid_tutorials/scripts/jaxon_blue_rh_setup.py +++ b/hrpsys_choreonoid_tutorials/scripts/jaxon_blue_rh_setup.py @@ -52,8 +52,8 @@ def startABSTIMP (self): # Latter four 0.0 are for hands. self.seq_svc.setJointAngles(self.jaxonResetPose(), 1.0) self.abc_svc.startAutoBalancer(['rleg','lleg']) - self.ic_svc.startImpedanceController("larm") - self.ic_svc.startImpedanceController("rarm") + # self.ic_svc.startImpedanceController("larm") + # self.ic_svc.startImpedanceController("rarm") self.startStabilizer() if __name__ == '__main__': diff --git a/hrpsys_choreonoid_tutorials/scripts/jaxon_red_rh_setup.py b/hrpsys_choreonoid_tutorials/scripts/jaxon_red_rh_setup.py index 85dc94cb..c8071cd3 100755 --- a/hrpsys_choreonoid_tutorials/scripts/jaxon_red_rh_setup.py +++ b/hrpsys_choreonoid_tutorials/scripts/jaxon_red_rh_setup.py @@ -52,8 +52,8 @@ def startABSTIMP (self): # Latter four 0.0 are for hands. self.seq_svc.setJointAngles(self.jaxonResetPose()+[0.0, 0.0, 0.0, 0.0] , 1.0) self.abc_svc.startAutoBalancer(['rleg','lleg']) - self.ic_svc.startImpedanceController("larm") - self.ic_svc.startImpedanceController("rarm") + # self.ic_svc.startImpedanceController("larm") + # self.ic_svc.startImpedanceController("rarm") self.startStabilizer() if __name__ == '__main__': diff --git a/hrpsys_choreonoid_tutorials/scripts/jaxon_red_setup.py b/hrpsys_choreonoid_tutorials/scripts/jaxon_red_setup.py index feb7270c..1c3301fc 100755 --- a/hrpsys_choreonoid_tutorials/scripts/jaxon_red_setup.py +++ b/hrpsys_choreonoid_tutorials/scripts/jaxon_red_setup.py @@ -52,8 +52,8 @@ def startABSTIMP (self): # Latter four 0.0 are for hands. self.seq_svc.setJointAngles(self.jaxonResetPose()+[0.0, 0.0, 0.0, 0.0] , 1.0) self.abc_svc.startAutoBalancer(['rleg','lleg']) - self.ic_svc.startImpedanceController("larm") - self.ic_svc.startImpedanceController("rarm") + # self.ic_svc.startImpedanceController("larm") + # self.ic_svc.startImpedanceController("rarm") self.startStabilizer() if __name__ == '__main__': From 63b8ba2c412a1dd3edb9e313be7faaec74161092 Mon Sep 17 00:00:00 2001 From: 98shimpei Date: Mon, 4 Apr 2022 14:21:16 +0900 Subject: [PATCH 5/9] disable footcoords --- hrpsys_choreonoid_tutorials/launch/jaxon_red_choreonoid.launch | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/hrpsys_choreonoid_tutorials/launch/jaxon_red_choreonoid.launch b/hrpsys_choreonoid_tutorials/launch/jaxon_red_choreonoid.launch index ab888ba3..1692b72c 100644 --- a/hrpsys_choreonoid_tutorials/launch/jaxon_red_choreonoid.launch +++ b/hrpsys_choreonoid_tutorials/launch/jaxon_red_choreonoid.launch @@ -13,7 +13,7 @@ - + Date: Mon, 28 Mar 2022 02:00:00 +0900 Subject: [PATCH 6/9] [JAXON_JVRC.PDgains/_sim.sav][JAXON_JVRCmain/_hrpsys/_bush.wrl] add wheel joint to VRML model files and gain files of JAXON_JVRC --- .../models/JAXON_JVRC.PDgains.sav | 5 + .../models/JAXON_JVRC.PDgains_sim.dat | 5 + jvrc_models/JAXON_JVRC/JAXON_JVRCmain.wrl | 316 ++++++++++++++++++ .../JAXON_JVRC/JAXON_JVRCmain_hrpsys.wrl | 316 ++++++++++++++++++ .../JAXON_JVRC/JAXON_JVRCmain_hrpsys_bush.wrl | 316 ++++++++++++++++++ 5 files changed, 958 insertions(+) diff --git a/hrpsys_choreonoid_tutorials/models/JAXON_JVRC.PDgains.sav b/hrpsys_choreonoid_tutorials/models/JAXON_JVRC.PDgains.sav index 5e9bd811..e3a6cfd0 100644 --- a/hrpsys_choreonoid_tutorials/models/JAXON_JVRC.PDgains.sav +++ b/hrpsys_choreonoid_tutorials/models/JAXON_JVRC.PDgains.sav @@ -36,3 +36,8 @@ 1 0 1 1 0 0 1 0 1 1 0 0 1 0 1 1 0 0 +1 0 1 1 0 0 +1 0 1 1 0 0 +1 0 1 1 0 0 +1 0 1 1 0 0 +0 0 0 0 0 0 \ No newline at end of file diff --git a/hrpsys_choreonoid_tutorials/models/JAXON_JVRC.PDgains_sim.dat b/hrpsys_choreonoid_tutorials/models/JAXON_JVRC.PDgains_sim.dat index 18d11fe4..d871e6cb 100644 --- a/hrpsys_choreonoid_tutorials/models/JAXON_JVRC.PDgains_sim.dat +++ b/hrpsys_choreonoid_tutorials/models/JAXON_JVRC.PDgains_sim.dat @@ -35,4 +35,9 @@ 400 10 1 0 400 10 1 0 400 10 1 0 +20000 100 0 0 +20000 100 0 0 +20000 100 0 0 +20000 100 0 0 100 5 1 0 +0 0 0 0 diff --git a/jvrc_models/JAXON_JVRC/JAXON_JVRCmain.wrl b/jvrc_models/JAXON_JVRC/JAXON_JVRCmain.wrl index f0089dd3..3ba62bc5 100644 --- a/jvrc_models/JAXON_JVRC/JAXON_JVRCmain.wrl +++ b/jvrc_models/JAXON_JVRC/JAXON_JVRCmain.wrl @@ -59,6 +59,182 @@ PROTO Segment [ } } +PROTO PressureSensor [ + exposedField SFFloat maxPressure -1 + exposedField SFVec3f translation 0 0 0 + exposedField SFRotation rotation 0 0 1 0 + exposedField SFInt32 sensorId -1 +] +{ + Transform { + translation IS translation + rotation IS rotation + } +} + +PROTO PhotoInterrupter [ + exposedField SFVec3f transmitter 0 0 0 + exposedField SFVec3f receiver 0 0 0 + exposedField SFInt32 sensorId -1 +] +{ + Transform{ + children [ + Transform{ + translation IS transmitter + } + Transform{ + translation IS receiver + } + ] + } +} + +PROTO CylinderSensorZ [ + exposedField SFFloat maxAngle -1 + exposedField SFFloat minAngle 0 + exposedField MFNode children [ ] +] +{ + Transform{ + rotation 1 0 0 1.5708 + children [ + DEF SensorY CylinderSensor{ + maxAngle IS maxAngle + minAngle IS minAngle + } + DEF AxisY Transform{ + children [ + Transform{ + rotation 1 0 0 -1.5708 + children IS children + } + ] + } + ] + } + ROUTE SensorY.rotation_changed TO AxisY.set_rotation +} + +PROTO CylinderSensorY [ + exposedField SFFloat maxAngle -1 + exposedField SFFloat minAngle 0 + exposedField MFNode children [ ] +] +{ + Transform{ + rotation 0 1 0 1.5708 + children [ + DEF SensorX CylinderSensor{ + maxAngle IS maxAngle + minAngle IS minAngle + } + DEF AxisX Transform{ + children [ + Transform{ + rotation 0 1 0 -1.5708 + children IS children + } + ] + } + ] + } + ROUTE SensorX.rotation_changed TO AxisX.set_rotation +} + +PROTO CylinderSensorX [ + exposedField SFFloat maxAngle -1 + exposedField SFFloat minAngle 0 + exposedField MFNode children [ ] +] +{ + Transform{ + rotation 0 0 1 -1.5708 + children [ + DEF SensorZ CylinderSensor{ + maxAngle IS maxAngle + minAngle IS minAngle + } + DEF AxisZ Transform{ + children [ + Transform{ + rotation 0 0 1 1.5708 + children IS children + } + ] + } + ] + } + ROUTE SensorZ.rotation_changed TO AxisZ.set_rotation +} + +PROTO WheelFront [ +] +{ + Shape { + geometry Cylinder { + radius 0.04 + height 0.12 + } + appearance Appearance { + material Material {} + } + } +} + +NavigationInfo { + avatarSize 0.5 + headlight TRUE + type ["EXAMINE", "ANY"] +} + +Background { + skyColor 0.4 0.6 0.4 +} + +Viewpoint { + position 3 0 0.835 + orientation 0.5770 0.5775 0.5775 2.0935 +} + +PROTO Surface [ + field SFVec3f bboxCenter 0 0 0 + field SFVec3f bboxSize -1 -1 -1 + field MFNode visual [ ] + field MFNode collision [ ] + eventIn MFNode addChildren + eventIn MFNode removeChildren +] +{ + Group { + addChildren IS addChildren + bboxCenter IS bboxCenter + bboxSize IS bboxSize + children IS visual + removeChildren IS removeChildren + } +} + +# If the body includes closed loop links, plase define the following proto node +PROTO ExtraJoint [ + # Name of the first link + exposedField SFString link1Name "" + # Name of the second link + exposedField SFString link2Name "" + # Connection (joint) position in the local coordinate of the first link + exposedField SFVec3f link1LocalPos 0 0 0 + # Connection (joint) position in the local coordinate of the second link + exposedField SFVec3f link2LocalPos 0 0 0 + # Constraint axes. Two or three orthogonal axes must be specified + # in the local coordinate of the first link + #exposedField MFVec3f axes [ ] + # Possible types are "piston" and "ball" + exposedField SFString jointType "ball" + exposedField SFVec3f jointAxis 0 1 0 +] +{ +} + PROTO Humanoid [ field SFVec3f bboxCenter 0 0 0 field SFVec3f bboxSize -1 -1 -1 @@ -1361,6 +1537,72 @@ DEF JAXON_JVRC Humanoid { } #Sensor ] } #Segment LLEG_LINK5 + DEF LLEG_JOINT6 Joint { #wheel main + jointType "rotate" + jointId 38 + jointAxis 0 1 0 + translation 0.12 0.0 0.02 + rotation 1 0 0 0 + llimit [-100] + ulimit [ 100] + lvlimit [-38.0] + uvlimit [ 38.0] + # climit[50.55] + # torqueConst 0.1 + # gearRatio 3.5 + climit[80] + torqueConst 0.02 + gearRatio 1000.0 + rotorInertia 1.0e-6 + children [ + DEF LLEG_LINK6_MOV CylinderSensorY { + maxAngle 100 + minAngle -100 + children [ + DEF LLEG_LINK6 Segment { + centerOfMass 0.0 -0.0455 0.0 + #mass 1.6718 + mass 2.4689 # 1.6718 + 0.5072 + 0.2899 + momentsOfInertia [0.0038 0.0 0.0 0.0 0.0041 0.0 0.0 0.0 0.0038] + children [ + DEF WHEEL_FRONT_L WheelFront {} + ] + } + ] + } + ] + } # End of LLEG_JOINT6 + DEF LLEG_JOINT7 Joint { #wheel rear + jointType "rotate" + jointId 40 + jointAxis 0 1 0 + translation -0.11 0.0 0.02 + rotation 1 0 0 0 + llimit [-100] + ulimit [ 100] + lvlimit [-17.4444] + uvlimit [ 17.4444] + climit[50.55] + torqueConst 0.1 + gearRatio 1000 + rotorInertia 1.0e-6 + children [ + DEF LLEG_LINK7_MOV CylinderSensorY { + maxAngle 100 + minAngle -100 + children [ + DEF LLEG_LINK7 Segment { + centerOfMass 0.0 -0.0387 0.0 + mass 0.5072 + momentsOfInertia [0.0011 0.0 0.0 0.0 0.0001 0.0 0.0 0.0 0.0011] + children [ + DEF WHEEL_REAR_L WheelFront {} + ] + } + ] + } + ] + } # End of LLEG_JOINT7 ] } #Joint LLEG_JOINT5 ] @@ -1552,6 +1794,72 @@ DEF JAXON_JVRC Humanoid { } #Sensor ] } #Segment RLEG_LINK5 + DEF RLEG_JOINT6 Joint { #wheel main + jointType "rotate" + jointId 37 + jointAxis 0 1 0 + translation 0.12 0.0 0.02 + rotation 1 0 0 0 + llimit [-100] + ulimit [ 100] + lvlimit [-38.0] + uvlimit [ 30.0] + # climit[50.55] + # torqueConst 0.1 + # gearRatio 3.5 + climit[80] + torqueConst 0.02 + gearRatio 1000.0 + rotorInertia 1.0e-6 + children [ + DEF RLEG_LINK6_MOV CylinderSensorY { + maxAngle 100 + minAngle -100 + children [ + DEF RLEG_LINK6 Segment { + centerOfMass 0.0 0.0455 0.0 + #mass 1.6718 + mass 2.4689 # 1.6718 + 0.5072 + 0.2899 + momentsOfInertia [0.0038 0.0 0.0 0.0 0.0041 0.0 0.0 0.0 0.0038] + children [ + DEF WHEEL_FRONT_R WheelFront {} + ] + } + ] + } + ] + } # End of RLEG_JOINT6 + DEF RLEG_JOINT7 Joint { #wheel center + jointType "rotate" + jointId 39 + jointAxis 0 1 0 + translation -0.11 0.0 0.02 + rotation 1 0 0 0 + llimit [-100] + ulimit [ 100] + lvlimit [-17.4444] + uvlimit [ 17.4444] + climit[50.55] + torqueConst 0.1 + gearRatio 1000 + rotorInertia 1.0e-6 + children [ + DEF RLEG_LINK7_MOV CylinderSensorY { + maxAngle 100 + minAngle -100 + children [ + DEF RLEG_LINK7 Segment { + centerOfMass 0.0 -0.0387 0.0 + mass 0.5072 + momentsOfInertia [0.0011 0.0 0.0 0.0 0.0001 0.0 0.0 0.0 0.0011] + children [ + DEF WHEEL_REAR_R WheelFront {} + ] + } + ] + } + ] + } # End of RLEG_JOINT7 ] } #Joint RLEG_JOINT5 ] @@ -1596,12 +1904,16 @@ DEF JAXON_JVRC Humanoid { USE LLEG_JOINT3, USE LLEG_JOINT4, USE LLEG_JOINT5, + USE LLEG_JOINT6, + USE LLEG_JOINT7, USE RLEG_JOINT0, USE RLEG_JOINT1, USE RLEG_JOINT2, USE RLEG_JOINT3, USE RLEG_JOINT4, USE RLEG_JOINT5, + USE RLEG_JOINT6, + USE RLEG_JOINT7, USE LARM_F_JOINT0, USE LARM_F_JOINT1, USE RARM_F_JOINT0, @@ -1637,12 +1949,16 @@ DEF JAXON_JVRC Humanoid { USE LLEG_LINK3, USE LLEG_LINK4, USE LLEG_LINK5, + USE LLEG_LINK6, + USE LLEG_LINK7, USE RLEG_LINK0, USE RLEG_LINK1, USE RLEG_LINK2, USE RLEG_LINK3, USE RLEG_LINK4, USE RLEG_LINK5, + USE RLEG_LINK6, + USE RLEG_LINK7, USE LARM_FINGER0, USE LARM_FINGER1, USE RARM_FINGER0, diff --git a/jvrc_models/JAXON_JVRC/JAXON_JVRCmain_hrpsys.wrl b/jvrc_models/JAXON_JVRC/JAXON_JVRCmain_hrpsys.wrl index fcda23ae..edfffdd6 100644 --- a/jvrc_models/JAXON_JVRC/JAXON_JVRCmain_hrpsys.wrl +++ b/jvrc_models/JAXON_JVRC/JAXON_JVRCmain_hrpsys.wrl @@ -59,6 +59,182 @@ PROTO Segment [ } } +PROTO PressureSensor [ + exposedField SFFloat maxPressure -1 + exposedField SFVec3f translation 0 0 0 + exposedField SFRotation rotation 0 0 1 0 + exposedField SFInt32 sensorId -1 +] +{ + Transform { + translation IS translation + rotation IS rotation + } +} + +PROTO PhotoInterrupter [ + exposedField SFVec3f transmitter 0 0 0 + exposedField SFVec3f receiver 0 0 0 + exposedField SFInt32 sensorId -1 +] +{ + Transform{ + children [ + Transform{ + translation IS transmitter + } + Transform{ + translation IS receiver + } + ] + } +} + +PROTO CylinderSensorZ [ + exposedField SFFloat maxAngle -1 + exposedField SFFloat minAngle 0 + exposedField MFNode children [ ] +] +{ + Transform{ + rotation 1 0 0 1.5708 + children [ + DEF SensorY CylinderSensor{ + maxAngle IS maxAngle + minAngle IS minAngle + } + DEF AxisY Transform{ + children [ + Transform{ + rotation 1 0 0 -1.5708 + children IS children + } + ] + } + ] + } + ROUTE SensorY.rotation_changed TO AxisY.set_rotation +} + +PROTO CylinderSensorY [ + exposedField SFFloat maxAngle -1 + exposedField SFFloat minAngle 0 + exposedField MFNode children [ ] +] +{ + Transform{ + rotation 0 1 0 1.5708 + children [ + DEF SensorX CylinderSensor{ + maxAngle IS maxAngle + minAngle IS minAngle + } + DEF AxisX Transform{ + children [ + Transform{ + rotation 0 1 0 -1.5708 + children IS children + } + ] + } + ] + } + ROUTE SensorX.rotation_changed TO AxisX.set_rotation +} + +PROTO CylinderSensorX [ + exposedField SFFloat maxAngle -1 + exposedField SFFloat minAngle 0 + exposedField MFNode children [ ] +] +{ + Transform{ + rotation 0 0 1 -1.5708 + children [ + DEF SensorZ CylinderSensor{ + maxAngle IS maxAngle + minAngle IS minAngle + } + DEF AxisZ Transform{ + children [ + Transform{ + rotation 0 0 1 1.5708 + children IS children + } + ] + } + ] + } + ROUTE SensorZ.rotation_changed TO AxisZ.set_rotation +} + +PROTO WheelFront [ +] +{ + Shape { + geometry Cylinder { + radius 0.04 + height 0.12 + } + appearance Appearance { + material Material {} + } + } +} + +NavigationInfo { + avatarSize 0.5 + headlight TRUE + type ["EXAMINE", "ANY"] +} + +Background { + skyColor 0.4 0.6 0.4 +} + +Viewpoint { + position 3 0 0.835 + orientation 0.5770 0.5775 0.5775 2.0935 +} + +PROTO Surface [ + field SFVec3f bboxCenter 0 0 0 + field SFVec3f bboxSize -1 -1 -1 + field MFNode visual [ ] + field MFNode collision [ ] + eventIn MFNode addChildren + eventIn MFNode removeChildren +] +{ + Group { + addChildren IS addChildren + bboxCenter IS bboxCenter + bboxSize IS bboxSize + children IS visual + removeChildren IS removeChildren + } +} + +# If the body includes closed loop links, plase define the following proto node +PROTO ExtraJoint [ + # Name of the first link + exposedField SFString link1Name "" + # Name of the second link + exposedField SFString link2Name "" + # Connection (joint) position in the local coordinate of the first link + exposedField SFVec3f link1LocalPos 0 0 0 + # Connection (joint) position in the local coordinate of the second link + exposedField SFVec3f link2LocalPos 0 0 0 + # Constraint axes. Two or three orthogonal axes must be specified + # in the local coordinate of the first link + #exposedField MFVec3f axes [ ] + # Possible types are "piston" and "ball" + exposedField SFString jointType "ball" + exposedField SFVec3f jointAxis 0 1 0 +] +{ +} + PROTO Humanoid [ field SFVec3f bboxCenter 0 0 0 field SFVec3f bboxSize -1 -1 -1 @@ -1362,6 +1538,72 @@ DEF JAXON_JVRC Humanoid { } #Sensor ] } #Segment LLEG_LINK5 + DEF LLEG_JOINT6 Joint { #wheel main + jointType "rotate" + jointId 38 + jointAxis 0 1 0 + translation 0.12 0.0 0.02 + rotation 1 0 0 0 + llimit [-100] + ulimit [ 100] + lvlimit [-38.0] + uvlimit [ 38.0] + # climit[50.55] + # torqueConst 0.1 + # gearRatio 3.5 + climit[80] + torqueConst 0.02 + gearRatio 1000.0 + rotorInertia 1.0e-6 + children [ + DEF LLEG_LINK6_MOV CylinderSensorY { + maxAngle 100 + minAngle -100 + children [ + DEF LLEG_LINK6 Segment { + centerOfMass 0.0 -0.0455 0.0 + #mass 1.6718 + mass 2.4689 # 1.6718 + 0.5072 + 0.2899 + momentsOfInertia [0.0038 0.0 0.0 0.0 0.0041 0.0 0.0 0.0 0.0038] + children [ + DEF WHEEL_FRONT_L WheelFront {} + ] + } + ] + } + ] + } # End of LLEG_JOINT6 + DEF LLEG_JOINT7 Joint { #wheel rear + jointType "rotate" + jointId 40 + jointAxis 0 1 0 + translation -0.11 0.0 0.02 + rotation 1 0 0 0 + llimit [-100] + ulimit [ 100] + lvlimit [-17.4444] + uvlimit [ 17.4444] + climit[50.55] + torqueConst 0.1 + gearRatio 1000 + rotorInertia 1.0e-6 + children [ + DEF LLEG_LINK7_MOV CylinderSensorY { + maxAngle 100 + minAngle -100 + children [ + DEF LLEG_LINK7 Segment { + centerOfMass 0.0 -0.0387 0.0 + mass 0.5072 + momentsOfInertia [0.0011 0.0 0.0 0.0 0.0001 0.0 0.0 0.0 0.0011] + children [ + DEF WHEEL_REAR_L WheelFront {} + ] + } + ] + } + ] + } # End of LLEG_JOINT7 ] } #Joint LLEG_JOINT5 ] @@ -1553,6 +1795,72 @@ DEF JAXON_JVRC Humanoid { } #Sensor ] } #Segment RLEG_LINK5 + DEF RLEG_JOINT6 Joint { #wheel main + jointType "rotate" + jointId 37 + jointAxis 0 1 0 + translation 0.12 0.0 0.02 + rotation 1 0 0 0 + llimit [-100] + ulimit [ 100] + lvlimit [-38.0] + uvlimit [ 30.0] + # climit[50.55] + # torqueConst 0.1 + # gearRatio 3.5 + climit[80] + torqueConst 0.02 + gearRatio 1000.0 + rotorInertia 1.0e-6 + children [ + DEF RLEG_LINK6_MOV CylinderSensorY { + maxAngle 100 + minAngle -100 + children [ + DEF RLEG_LINK6 Segment { + centerOfMass 0.0 0.0455 0.0 + #mass 1.6718 + mass 2.4689 # 1.6718 + 0.5072 + 0.2899 + momentsOfInertia [0.0038 0.0 0.0 0.0 0.0041 0.0 0.0 0.0 0.0038] + children [ + DEF WHEEL_FRONT_R WheelFront {} + ] + } + ] + } + ] + } # End of RLEG_JOINT6 + DEF RLEG_JOINT7 Joint { #wheel center + jointType "rotate" + jointId 39 + jointAxis 0 1 0 + translation -0.11 0.0 0.02 + rotation 1 0 0 0 + llimit [-100] + ulimit [ 100] + lvlimit [-17.4444] + uvlimit [ 17.4444] + climit[50.55] + torqueConst 0.1 + gearRatio 1000 + rotorInertia 1.0e-6 + children [ + DEF RLEG_LINK7_MOV CylinderSensorY { + maxAngle 100 + minAngle -100 + children [ + DEF RLEG_LINK7 Segment { + centerOfMass 0.0 -0.0387 0.0 + mass 0.5072 + momentsOfInertia [0.0011 0.0 0.0 0.0 0.0001 0.0 0.0 0.0 0.0011] + children [ + DEF WHEEL_REAR_R WheelFront {} + ] + } + ] + } + ] + } # End of RLEG_JOINT7 ] } #Joint RLEG_JOINT5 ] @@ -1597,12 +1905,16 @@ DEF JAXON_JVRC Humanoid { USE LLEG_JOINT3, USE LLEG_JOINT4, USE LLEG_JOINT5, + USE LLEG_JOINT6, + USE LLEG_JOINT7, USE RLEG_JOINT0, USE RLEG_JOINT1, USE RLEG_JOINT2, USE RLEG_JOINT3, USE RLEG_JOINT4, USE RLEG_JOINT5, + USE RLEG_JOINT6, + USE RLEG_JOINT7, USE LARM_F_JOINT0, USE LARM_F_JOINT1, USE RARM_F_JOINT0, @@ -1638,12 +1950,16 @@ DEF JAXON_JVRC Humanoid { USE LLEG_LINK3, USE LLEG_LINK4, USE LLEG_LINK5, + USE LLEG_LINK6, + USE LLEG_LINK7, USE RLEG_LINK0, USE RLEG_LINK1, USE RLEG_LINK2, USE RLEG_LINK3, USE RLEG_LINK4, USE RLEG_LINK5, + USE RLEG_LINK6, + USE RLEG_LINK7, USE LARM_FINGER0, USE LARM_FINGER1, USE RARM_FINGER0, diff --git a/jvrc_models/JAXON_JVRC/JAXON_JVRCmain_hrpsys_bush.wrl b/jvrc_models/JAXON_JVRC/JAXON_JVRCmain_hrpsys_bush.wrl index f14a0b1a..d9491348 100644 --- a/jvrc_models/JAXON_JVRC/JAXON_JVRCmain_hrpsys_bush.wrl +++ b/jvrc_models/JAXON_JVRC/JAXON_JVRCmain_hrpsys_bush.wrl @@ -59,6 +59,182 @@ PROTO Segment [ } } +PROTO PressureSensor [ + exposedField SFFloat maxPressure -1 + exposedField SFVec3f translation 0 0 0 + exposedField SFRotation rotation 0 0 1 0 + exposedField SFInt32 sensorId -1 +] +{ + Transform { + translation IS translation + rotation IS rotation + } +} + +PROTO PhotoInterrupter [ + exposedField SFVec3f transmitter 0 0 0 + exposedField SFVec3f receiver 0 0 0 + exposedField SFInt32 sensorId -1 +] +{ + Transform{ + children [ + Transform{ + translation IS transmitter + } + Transform{ + translation IS receiver + } + ] + } +} + +PROTO CylinderSensorZ [ + exposedField SFFloat maxAngle -1 + exposedField SFFloat minAngle 0 + exposedField MFNode children [ ] +] +{ + Transform{ + rotation 1 0 0 1.5708 + children [ + DEF SensorY CylinderSensor{ + maxAngle IS maxAngle + minAngle IS minAngle + } + DEF AxisY Transform{ + children [ + Transform{ + rotation 1 0 0 -1.5708 + children IS children + } + ] + } + ] + } + ROUTE SensorY.rotation_changed TO AxisY.set_rotation +} + +PROTO CylinderSensorY [ + exposedField SFFloat maxAngle -1 + exposedField SFFloat minAngle 0 + exposedField MFNode children [ ] +] +{ + Transform{ + rotation 0 1 0 1.5708 + children [ + DEF SensorX CylinderSensor{ + maxAngle IS maxAngle + minAngle IS minAngle + } + DEF AxisX Transform{ + children [ + Transform{ + rotation 0 1 0 -1.5708 + children IS children + } + ] + } + ] + } + ROUTE SensorX.rotation_changed TO AxisX.set_rotation +} + +PROTO CylinderSensorX [ + exposedField SFFloat maxAngle -1 + exposedField SFFloat minAngle 0 + exposedField MFNode children [ ] +] +{ + Transform{ + rotation 0 0 1 -1.5708 + children [ + DEF SensorZ CylinderSensor{ + maxAngle IS maxAngle + minAngle IS minAngle + } + DEF AxisZ Transform{ + children [ + Transform{ + rotation 0 0 1 1.5708 + children IS children + } + ] + } + ] + } + ROUTE SensorZ.rotation_changed TO AxisZ.set_rotation +} + +PROTO WheelFront [ +] +{ + Shape { + geometry Cylinder { + radius 0.04 + height 0.12 + } + appearance Appearance { + material Material {} + } + } +} + +NavigationInfo { + avatarSize 0.5 + headlight TRUE + type ["EXAMINE", "ANY"] +} + +Background { + skyColor 0.4 0.6 0.4 +} + +Viewpoint { + position 3 0 0.835 + orientation 0.5770 0.5775 0.5775 2.0935 +} + +PROTO Surface [ + field SFVec3f bboxCenter 0 0 0 + field SFVec3f bboxSize -1 -1 -1 + field MFNode visual [ ] + field MFNode collision [ ] + eventIn MFNode addChildren + eventIn MFNode removeChildren +] +{ + Group { + addChildren IS addChildren + bboxCenter IS bboxCenter + bboxSize IS bboxSize + children IS visual + removeChildren IS removeChildren + } +} + +# If the body includes closed loop links, plase define the following proto node +PROTO ExtraJoint [ + # Name of the first link + exposedField SFString link1Name "" + # Name of the second link + exposedField SFString link2Name "" + # Connection (joint) position in the local coordinate of the first link + exposedField SFVec3f link1LocalPos 0 0 0 + # Connection (joint) position in the local coordinate of the second link + exposedField SFVec3f link2LocalPos 0 0 0 + # Constraint axes. Two or three orthogonal axes must be specified + # in the local coordinate of the first link + #exposedField MFVec3f axes [ ] + # Possible types are "piston" and "ball" + exposedField SFString jointType "ball" + exposedField SFVec3f jointAxis 0 1 0 +] +{ +} + PROTO Humanoid [ field SFVec3f bboxCenter 0 0 0 field SFVec3f bboxSize -1 -1 -1 @@ -1534,6 +1710,72 @@ DEF JAXON_JVRC Humanoid { } # surface ] } # End of LLEG_LINK5_LOWER + DEF LLEG_JOINT6 Joint { #wheel main + jointType "rotate" + jointId 38 + jointAxis 0 1 0 + translation 0.12 0.0 0.02 + rotation 1 0 0 0 + llimit [-100] + ulimit [ 100] + lvlimit [-38.0] + uvlimit [ 38.0] + # climit[50.55] + # torqueConst 0.1 + # gearRatio 3.5 + climit[80] + torqueConst 0.02 + gearRatio 1000.0 + rotorInertia 1.0e-6 + children [ + DEF LLEG_LINK6_MOV CylinderSensorY { + maxAngle 100 + minAngle -100 + children [ + DEF LLEG_LINK6 Segment { + centerOfMass 0.0 -0.0455 0.0 + #mass 1.6718 + mass 2.4689 # 1.6718 + 0.5072 + 0.2899 + momentsOfInertia [0.0038 0.0 0.0 0.0 0.0041 0.0 0.0 0.0 0.0038] + children [ + DEF WHEEL_FRONT_L WheelFront {} + ] + } + ] + } + ] + } # End of LLEG_JOINT6 + DEF LLEG_JOINT7 Joint { #wheel rear + jointType "rotate" + jointId 40 + jointAxis 0 1 0 + translation -0.11 0.0 0.02 + rotation 1 0 0 0 + llimit [-100] + ulimit [ 100] + lvlimit [-17.4444] + uvlimit [ 17.4444] + climit[50.55] + torqueConst 0.1 + gearRatio 1000 + rotorInertia 1.0e-6 + children [ + DEF LLEG_LINK7_MOV CylinderSensorY { + maxAngle 100 + minAngle -100 + children [ + DEF LLEG_LINK7 Segment { + centerOfMass 0.0 -0.0387 0.0 + mass 0.5072 + momentsOfInertia [0.0011 0.0 0.0 0.0 0.0001 0.0 0.0 0.0 0.0011] + children [ + DEF WHEEL_REAR_L WheelFront {} + ] + } + ] + } + ] + } # End of LLEG_JOINT7 ] } # End of LLEG_BUSH_PITCH ] @@ -1782,6 +2024,72 @@ DEF JAXON_JVRC Humanoid { } # surface ] } # End of RLEG_LINK5_LOWER + DEF RLEG_JOINT6 Joint { #wheel main + jointType "rotate" + jointId 37 + jointAxis 0 1 0 + translation 0.12 0.0 0.02 + rotation 1 0 0 0 + llimit [-100] + ulimit [ 100] + lvlimit [-38.0] + uvlimit [ 30.0] + # climit[50.55] + # torqueConst 0.1 + # gearRatio 3.5 + climit[80] + torqueConst 0.02 + gearRatio 1000.0 + rotorInertia 1.0e-6 + children [ + DEF RLEG_LINK6_MOV CylinderSensorY { + maxAngle 100 + minAngle -100 + children [ + DEF RLEG_LINK6 Segment { + centerOfMass 0.0 0.0455 0.0 + #mass 1.6718 + mass 2.4689 # 1.6718 + 0.5072 + 0.2899 + momentsOfInertia [0.0038 0.0 0.0 0.0 0.0041 0.0 0.0 0.0 0.0038] + children [ + DEF WHEEL_FRONT_R WheelFront {} + ] + } + ] + } + ] + } # End of RLEG_JOINT6 + DEF RLEG_JOINT7 Joint { #wheel center + jointType "rotate" + jointId 39 + jointAxis 0 1 0 + translation -0.11 0.0 0.02 + rotation 1 0 0 0 + llimit [-100] + ulimit [ 100] + lvlimit [-17.4444] + uvlimit [ 17.4444] + climit[50.55] + torqueConst 0.1 + gearRatio 1000 + rotorInertia 1.0e-6 + children [ + DEF RLEG_LINK7_MOV CylinderSensorY { + maxAngle 100 + minAngle -100 + children [ + DEF RLEG_LINK7 Segment { + centerOfMass 0.0 -0.0387 0.0 + mass 0.5072 + momentsOfInertia [0.0011 0.0 0.0 0.0 0.0001 0.0 0.0 0.0 0.0011] + children [ + DEF WHEEL_REAR_R WheelFront {} + ] + } + ] + } + ] + } # End of RLEG_JOINT7 ] } # End of RLEG_BUSH_PITCH ] @@ -1832,12 +2140,16 @@ DEF JAXON_JVRC Humanoid { USE LLEG_JOINT3, USE LLEG_JOINT4, USE LLEG_JOINT5, + USE LLEG_JOINT6, + USE LLEG_JOINT7, USE RLEG_JOINT0, USE RLEG_JOINT1, USE RLEG_JOINT2, USE RLEG_JOINT3, USE RLEG_JOINT4, USE RLEG_JOINT5, + USE RLEG_JOINT6, + USE RLEG_JOINT7, USE LARM_F_JOINT0, USE LARM_F_JOINT1, USE RARM_F_JOINT0, @@ -1874,6 +2186,8 @@ DEF JAXON_JVRC Humanoid { USE LLEG_LINK4, USE LLEG_LINK5_UPPER, USE LLEG_LINK5_LOWER, + USE LLEG_LINK6, + USE LLEG_LINK7, USE RLEG_LINK0, USE RLEG_LINK1, USE RLEG_LINK2, @@ -1881,6 +2195,8 @@ DEF JAXON_JVRC Humanoid { USE RLEG_LINK4, USE RLEG_LINK5_UPPER, USE RLEG_LINK5_LOWER, + USE RLEG_LINK6, + USE RLEG_LINK7, USE LARM_FINGER0, USE LARM_FINGER1, USE RARM_FINGER0, From 88090fb40ef59fdfa829d812bd4bb53a9ab72e56 Mon Sep 17 00:00:00 2001 From: kindsenior Date: Wed, 30 Mar 2022 03:35:42 +0900 Subject: [PATCH 7/9] [JAXON_JVRCmain/_hrpsys.wrl] modify wheel height to be same with the bush model --- jvrc_models/JAXON_JVRC/JAXON_JVRCmain.wrl | 8 ++++---- jvrc_models/JAXON_JVRC/JAXON_JVRCmain_hrpsys.wrl | 8 ++++---- 2 files changed, 8 insertions(+), 8 deletions(-) diff --git a/jvrc_models/JAXON_JVRC/JAXON_JVRCmain.wrl b/jvrc_models/JAXON_JVRC/JAXON_JVRCmain.wrl index 3ba62bc5..995f706d 100644 --- a/jvrc_models/JAXON_JVRC/JAXON_JVRCmain.wrl +++ b/jvrc_models/JAXON_JVRC/JAXON_JVRCmain.wrl @@ -1541,7 +1541,7 @@ DEF JAXON_JVRC Humanoid { jointType "rotate" jointId 38 jointAxis 0 1 0 - translation 0.12 0.0 0.02 + translation 0.12 0.0 -0.0825 # 0.02 - 0.1025 rotation 1 0 0 0 llimit [-100] ulimit [ 100] @@ -1576,7 +1576,7 @@ DEF JAXON_JVRC Humanoid { jointType "rotate" jointId 40 jointAxis 0 1 0 - translation -0.11 0.0 0.02 + translation -0.11 0.0 -0.0825 # 0.02 - 0.1025 rotation 1 0 0 0 llimit [-100] ulimit [ 100] @@ -1798,7 +1798,7 @@ DEF JAXON_JVRC Humanoid { jointType "rotate" jointId 37 jointAxis 0 1 0 - translation 0.12 0.0 0.02 + translation 0.12 0.0 -0.0825 # 0.02 - 0.1025 rotation 1 0 0 0 llimit [-100] ulimit [ 100] @@ -1833,7 +1833,7 @@ DEF JAXON_JVRC Humanoid { jointType "rotate" jointId 39 jointAxis 0 1 0 - translation -0.11 0.0 0.02 + translation -0.11 0.0 -0.0825 # 0.02 - 0.1025 rotation 1 0 0 0 llimit [-100] ulimit [ 100] diff --git a/jvrc_models/JAXON_JVRC/JAXON_JVRCmain_hrpsys.wrl b/jvrc_models/JAXON_JVRC/JAXON_JVRCmain_hrpsys.wrl index edfffdd6..16c041c7 100644 --- a/jvrc_models/JAXON_JVRC/JAXON_JVRCmain_hrpsys.wrl +++ b/jvrc_models/JAXON_JVRC/JAXON_JVRCmain_hrpsys.wrl @@ -1542,7 +1542,7 @@ DEF JAXON_JVRC Humanoid { jointType "rotate" jointId 38 jointAxis 0 1 0 - translation 0.12 0.0 0.02 + translation 0.12 0.0 -0.0825 # 0.02 - 0.1025 rotation 1 0 0 0 llimit [-100] ulimit [ 100] @@ -1577,7 +1577,7 @@ DEF JAXON_JVRC Humanoid { jointType "rotate" jointId 40 jointAxis 0 1 0 - translation -0.11 0.0 0.02 + translation -0.11 0.0 -0.0825 # 0.02 - 0.1025 rotation 1 0 0 0 llimit [-100] ulimit [ 100] @@ -1799,7 +1799,7 @@ DEF JAXON_JVRC Humanoid { jointType "rotate" jointId 37 jointAxis 0 1 0 - translation 0.12 0.0 0.02 + translation 0.12 0.0 -0.0825 # 0.02 - 0.1025 rotation 1 0 0 0 llimit [-100] ulimit [ 100] @@ -1834,7 +1834,7 @@ DEF JAXON_JVRC Humanoid { jointType "rotate" jointId 39 jointAxis 0 1 0 - translation -0.11 0.0 0.02 + translation -0.11 0.0 -0.0825 # 0.02 - 0.1025 rotation 1 0 0 0 llimit [-100] ulimit [ 100] From 5a29cfd8395c274579eb9204d6befaf9ee2376fc Mon Sep 17 00:00:00 2001 From: kindsenior Date: Wed, 30 Mar 2022 03:36:52 +0900 Subject: [PATCH 8/9] [JAXON_JVRCmain/_hrpsys/_bush.wrl] reduce whel weight to 1 kg --- jvrc_models/JAXON_JVRC/JAXON_JVRCmain.wrl | 10 ++++------ jvrc_models/JAXON_JVRC/JAXON_JVRCmain_hrpsys.wrl | 10 ++++------ jvrc_models/JAXON_JVRC/JAXON_JVRCmain_hrpsys_bush.wrl | 10 ++++------ 3 files changed, 12 insertions(+), 18 deletions(-) diff --git a/jvrc_models/JAXON_JVRC/JAXON_JVRCmain.wrl b/jvrc_models/JAXON_JVRC/JAXON_JVRCmain.wrl index 995f706d..a843d23e 100644 --- a/jvrc_models/JAXON_JVRC/JAXON_JVRCmain.wrl +++ b/jvrc_models/JAXON_JVRC/JAXON_JVRCmain.wrl @@ -1561,8 +1561,7 @@ DEF JAXON_JVRC Humanoid { children [ DEF LLEG_LINK6 Segment { centerOfMass 0.0 -0.0455 0.0 - #mass 1.6718 - mass 2.4689 # 1.6718 + 0.5072 + 0.2899 + mass 1.0 momentsOfInertia [0.0038 0.0 0.0 0.0 0.0041 0.0 0.0 0.0 0.0038] children [ DEF WHEEL_FRONT_L WheelFront {} @@ -1593,7 +1592,7 @@ DEF JAXON_JVRC Humanoid { children [ DEF LLEG_LINK7 Segment { centerOfMass 0.0 -0.0387 0.0 - mass 0.5072 + mass 1.0 momentsOfInertia [0.0011 0.0 0.0 0.0 0.0001 0.0 0.0 0.0 0.0011] children [ DEF WHEEL_REAR_L WheelFront {} @@ -1818,8 +1817,7 @@ DEF JAXON_JVRC Humanoid { children [ DEF RLEG_LINK6 Segment { centerOfMass 0.0 0.0455 0.0 - #mass 1.6718 - mass 2.4689 # 1.6718 + 0.5072 + 0.2899 + mass 1.0 momentsOfInertia [0.0038 0.0 0.0 0.0 0.0041 0.0 0.0 0.0 0.0038] children [ DEF WHEEL_FRONT_R WheelFront {} @@ -1850,7 +1848,7 @@ DEF JAXON_JVRC Humanoid { children [ DEF RLEG_LINK7 Segment { centerOfMass 0.0 -0.0387 0.0 - mass 0.5072 + mass 1.0 momentsOfInertia [0.0011 0.0 0.0 0.0 0.0001 0.0 0.0 0.0 0.0011] children [ DEF WHEEL_REAR_R WheelFront {} diff --git a/jvrc_models/JAXON_JVRC/JAXON_JVRCmain_hrpsys.wrl b/jvrc_models/JAXON_JVRC/JAXON_JVRCmain_hrpsys.wrl index 16c041c7..1454b477 100644 --- a/jvrc_models/JAXON_JVRC/JAXON_JVRCmain_hrpsys.wrl +++ b/jvrc_models/JAXON_JVRC/JAXON_JVRCmain_hrpsys.wrl @@ -1562,8 +1562,7 @@ DEF JAXON_JVRC Humanoid { children [ DEF LLEG_LINK6 Segment { centerOfMass 0.0 -0.0455 0.0 - #mass 1.6718 - mass 2.4689 # 1.6718 + 0.5072 + 0.2899 + mass 1.0 momentsOfInertia [0.0038 0.0 0.0 0.0 0.0041 0.0 0.0 0.0 0.0038] children [ DEF WHEEL_FRONT_L WheelFront {} @@ -1594,7 +1593,7 @@ DEF JAXON_JVRC Humanoid { children [ DEF LLEG_LINK7 Segment { centerOfMass 0.0 -0.0387 0.0 - mass 0.5072 + mass 1.0 momentsOfInertia [0.0011 0.0 0.0 0.0 0.0001 0.0 0.0 0.0 0.0011] children [ DEF WHEEL_REAR_L WheelFront {} @@ -1819,8 +1818,7 @@ DEF JAXON_JVRC Humanoid { children [ DEF RLEG_LINK6 Segment { centerOfMass 0.0 0.0455 0.0 - #mass 1.6718 - mass 2.4689 # 1.6718 + 0.5072 + 0.2899 + mass 1.0 momentsOfInertia [0.0038 0.0 0.0 0.0 0.0041 0.0 0.0 0.0 0.0038] children [ DEF WHEEL_FRONT_R WheelFront {} @@ -1851,7 +1849,7 @@ DEF JAXON_JVRC Humanoid { children [ DEF RLEG_LINK7 Segment { centerOfMass 0.0 -0.0387 0.0 - mass 0.5072 + mass 1.0 momentsOfInertia [0.0011 0.0 0.0 0.0 0.0001 0.0 0.0 0.0 0.0011] children [ DEF WHEEL_REAR_R WheelFront {} diff --git a/jvrc_models/JAXON_JVRC/JAXON_JVRCmain_hrpsys_bush.wrl b/jvrc_models/JAXON_JVRC/JAXON_JVRCmain_hrpsys_bush.wrl index d9491348..a9d6d664 100644 --- a/jvrc_models/JAXON_JVRC/JAXON_JVRCmain_hrpsys_bush.wrl +++ b/jvrc_models/JAXON_JVRC/JAXON_JVRCmain_hrpsys_bush.wrl @@ -1734,8 +1734,7 @@ DEF JAXON_JVRC Humanoid { children [ DEF LLEG_LINK6 Segment { centerOfMass 0.0 -0.0455 0.0 - #mass 1.6718 - mass 2.4689 # 1.6718 + 0.5072 + 0.2899 + mass 1.0 momentsOfInertia [0.0038 0.0 0.0 0.0 0.0041 0.0 0.0 0.0 0.0038] children [ DEF WHEEL_FRONT_L WheelFront {} @@ -1766,7 +1765,7 @@ DEF JAXON_JVRC Humanoid { children [ DEF LLEG_LINK7 Segment { centerOfMass 0.0 -0.0387 0.0 - mass 0.5072 + mass 1.0 momentsOfInertia [0.0011 0.0 0.0 0.0 0.0001 0.0 0.0 0.0 0.0011] children [ DEF WHEEL_REAR_L WheelFront {} @@ -2048,8 +2047,7 @@ DEF JAXON_JVRC Humanoid { children [ DEF RLEG_LINK6 Segment { centerOfMass 0.0 0.0455 0.0 - #mass 1.6718 - mass 2.4689 # 1.6718 + 0.5072 + 0.2899 + mass 1.0 momentsOfInertia [0.0038 0.0 0.0 0.0 0.0041 0.0 0.0 0.0 0.0038] children [ DEF WHEEL_FRONT_R WheelFront {} @@ -2080,7 +2078,7 @@ DEF JAXON_JVRC Humanoid { children [ DEF RLEG_LINK7 Segment { centerOfMass 0.0 -0.0387 0.0 - mass 0.5072 + mass 1.0 momentsOfInertia [0.0011 0.0 0.0 0.0 0.0001 0.0 0.0 0.0 0.0011] children [ DEF WHEEL_REAR_R WheelFront {} From 220ca14bcf20b78ef1407f1a41e813d9e594ea1b Mon Sep 17 00:00:00 2001 From: kindsenior Date: Sat, 11 Jun 2022 22:57:43 +0900 Subject: [PATCH 9/9] [jaxon_red_setup.py][jaxon_red_rh_setup.py] do not set error limit to motor_joint (multisense) --- hrpsys_choreonoid_tutorials/scripts/jaxon_blue_rh_setup.py | 1 - hrpsys_choreonoid_tutorials/scripts/jaxon_red_rh_setup.py | 1 - hrpsys_choreonoid_tutorials/scripts/jaxon_red_setup.py | 1 - 3 files changed, 3 deletions(-) diff --git a/hrpsys_choreonoid_tutorials/scripts/jaxon_blue_rh_setup.py b/hrpsys_choreonoid_tutorials/scripts/jaxon_blue_rh_setup.py index dc40b4d4..4ed7903c 100755 --- a/hrpsys_choreonoid_tutorials/scripts/jaxon_blue_rh_setup.py +++ b/hrpsys_choreonoid_tutorials/scripts/jaxon_blue_rh_setup.py @@ -42,7 +42,6 @@ def defJointGroups (self): def startABSTIMP (self): ### not used on hrpsys - # self.el_svc.setServoErrorLimit("motor_joint", sys.float_info.max) # self.el_svc.setServoErrorLimit("RARM_F_JOINT0", sys.float_info.max) # self.el_svc.setServoErrorLimit("RARM_F_JOINT1", sys.float_info.max) # self.el_svc.setServoErrorLimit("LARM_F_JOINT0", sys.float_info.max) diff --git a/hrpsys_choreonoid_tutorials/scripts/jaxon_red_rh_setup.py b/hrpsys_choreonoid_tutorials/scripts/jaxon_red_rh_setup.py index c8071cd3..d3815f5f 100755 --- a/hrpsys_choreonoid_tutorials/scripts/jaxon_red_rh_setup.py +++ b/hrpsys_choreonoid_tutorials/scripts/jaxon_red_rh_setup.py @@ -42,7 +42,6 @@ def defJointGroups (self): def startABSTIMP (self): ### not used on hrpsys - self.el_svc.setServoErrorLimit("motor_joint", sys.float_info.max) self.el_svc.setServoErrorLimit("RARM_F_JOINT0", sys.float_info.max) self.el_svc.setServoErrorLimit("RARM_F_JOINT1", sys.float_info.max) self.el_svc.setServoErrorLimit("LARM_F_JOINT0", sys.float_info.max) diff --git a/hrpsys_choreonoid_tutorials/scripts/jaxon_red_setup.py b/hrpsys_choreonoid_tutorials/scripts/jaxon_red_setup.py index 1c3301fc..25570d5f 100755 --- a/hrpsys_choreonoid_tutorials/scripts/jaxon_red_setup.py +++ b/hrpsys_choreonoid_tutorials/scripts/jaxon_red_setup.py @@ -42,7 +42,6 @@ def defJointGroups (self): def startABSTIMP (self): ### not used on hrpsys - self.el_svc.setServoErrorLimit("motor_joint", sys.float_info.max) self.el_svc.setServoErrorLimit("RARM_F_JOINT0", sys.float_info.max) self.el_svc.setServoErrorLimit("RARM_F_JOINT1", sys.float_info.max) self.el_svc.setServoErrorLimit("LARM_F_JOINT0", sys.float_info.max)