From 3cd3da75571cf5cd3e9d819727937241696ad467 Mon Sep 17 00:00:00 2001 From: Naoki-Hiraoka Date: Sun, 13 Jun 2021 12:33:23 +0900 Subject: [PATCH] [hrpsys_choreonoid] add SampleRobot --- README.md | 28 +- hrpsys_choreonoid/CMakeLists.txt | 10 + .../config/BodyRTC_SampleRobot.RH.conf | 35 ++ .../config/SampleRobotCustomizer.yaml | 5 + .../config/SampleRobot_RH_LOAD_OBJ.cnoid.in | 368 ++++++++++++++++++ hrpsys_choreonoid/config/flat.yaml | 5 + .../launch/samplerobot_choreonoid.launch.in | 41 ++ hrpsys_choreonoid/package.xml | 1 + .../scripts/samplerobot_rh_setup.py | 77 ++++ 9 files changed, 568 insertions(+), 2 deletions(-) create mode 100644 hrpsys_choreonoid/config/BodyRTC_SampleRobot.RH.conf create mode 100644 hrpsys_choreonoid/config/SampleRobotCustomizer.yaml create mode 100644 hrpsys_choreonoid/config/SampleRobot_RH_LOAD_OBJ.cnoid.in create mode 100644 hrpsys_choreonoid/config/flat.yaml create mode 100644 hrpsys_choreonoid/launch/samplerobot_choreonoid.launch.in create mode 100755 hrpsys_choreonoid/scripts/samplerobot_rh_setup.py diff --git a/README.md b/README.md index bf273922..dbdbcb9a 100644 --- a/README.md +++ b/README.md @@ -43,9 +43,35 @@ source devel/setup.bash ``` ### run test +If you get error, try `export ORBgiopMaxMsgSize=2097152000`. + +#### SampleRobot +``` +rtmlaunch hrpsys_choreonoid samplerobot_choreonoid.launch +``` +Launch another terminal and send command to robot. (python) +``` +ipython -i `rospack find hrpsys_choreonoid`/scripts/samplerobot_rh_setup.py "SampleRobot(Robot)0" +hcf.abc_svc.goPos(1,0,0) +``` +Launch another terminal and send command to robot. (euslisp) +``` +roseus `rospack find hrpsys_ros_bridge`/euslisp/samplerobot-interface.l +(samplerobot-init) +(send *ri* :go-pos 1 0 0) +(setq *robot* *sr*) +(objects *robot*) +(send *robot* :reset-manip-pose) +(send *ri* :angle-vector (send *robot* :angle-vector) 5000) +``` + +#### JAXON +Build hrpsys_choreonoid_tutorials ``` catkin build hrpsys_choreonoid_tutorials source devel/setup.bash +``` +``` rtmlaunch hrpsys_choreonoid_tutorials jaxon_jvrc_choreonoid.launch ``` Launch another terminal and send command to robot. (python) @@ -66,8 +92,6 @@ roseus `rospack find hrpsys_choreonoid_tutorials`/euslisp/jaxon_jvrc-interface.l (send *ri* :angle-vector (send *robot* :angle-vector) 5000) ``` -If you get error, try `export ORBgiopMaxMsgSize=2097152000`. - See also [hrpsys_choreonoid_tutorials/README.md](/hrpsys_choreonoid_tutorials/README.md) --- diff --git a/hrpsys_choreonoid/CMakeLists.txt b/hrpsys_choreonoid/CMakeLists.txt index 2571f847..ea696636 100644 --- a/hrpsys_choreonoid/CMakeLists.txt +++ b/hrpsys_choreonoid/CMakeLists.txt @@ -96,3 +96,13 @@ endif() add_subdirectory(iob) add_custom_target(hrpsys_choreonoid_iob ALL DEPENDS RobotHardware_choreonoid) + + +set(HRPSYS_CHOREONOID_DIRECTORY ${PROJECT_SOURCE_DIR}) +set(OPENHRP_SAMPLE_DIR ${openhrp3_PREFIX}/share/OpenHRP-3.1/sample) + +### +#SampleRobot conid +### +configure_file(${PROJECT_SOURCE_DIR}/config/SampleRobot_RH_LOAD_OBJ.cnoid.in ${PROJECT_SOURCE_DIR}/config/SampleRobot_RH_LOAD_OBJ.cnoid @ONLY) +configure_file(${PROJECT_SOURCE_DIR}/launch/samplerobot_choreonoid.launch.in ${PROJECT_SOURCE_DIR}/launch/samplerobot_choreonoid.launch @ONLY) #OPENHRP_SAMPLE_DIR is difficult to find in roslaunch api diff --git a/hrpsys_choreonoid/config/BodyRTC_SampleRobot.RH.conf b/hrpsys_choreonoid/config/BodyRTC_SampleRobot.RH.conf new file mode 100644 index 00000000..240e4e57 --- /dev/null +++ b/hrpsys_choreonoid/config/BodyRTC_SampleRobot.RH.conf @@ -0,0 +1,35 @@ +## +name-server = localhost:15005 +## +## PD Controller +## in: angleRef, angle +## out: torque +## +in-port = tauIn:JOINT_TORQUE +out-port = angleOut:JOINT_VALUE +out-port = qvel:JOINT_VELOCITY +out-port = torque:JOINT_TORQUE +# out-port = ddq:JOINT_ACCELERATION +connection = tauIn:RobotHardware_choreonoid0:torqueOut +connection = angleOut:RobotHardware_choreonoid0:angleIn +connection = qvel:RobotHardware_choreonoid0:qvel_sim +connection = torque:RobotHardware_choreonoid0:torque_sim +### +# debug ## ground truth robot potition +### +out-port = WAIST:WAIST:ABS_TRANSFORM +#### +# sensors +#### +out-port = rfsensor_sim:rfsensor:FORCE_SENSOR +out-port = lfsensor_sim:lfsensor:FORCE_SENSOR +out-port = rhsensor_sim:rhsensor:FORCE_SENSOR +out-port = lhsensor_sim:lhsensor:FORCE_SENSOR +out-port = gsensor_sim:gsensor:ACCELERATION_SENSOR2 +out-port = gyrometer_sim:gyrometer:RATE_GYRO_SENSOR2 +connection = rfsensor_sim:RobotHardware_choreonoid0:rfsensor_sim +connection = lfsensor_sim:RobotHardware_choreonoid0:lfsensor_sim +connection = rhsensor_sim:RobotHardware_choreonoid0:rhsensor_sim +connection = lhsensor_sim:RobotHardware_choreonoid0:lhsensor_sim +connection = gsensor_sim:RobotHardware_choreonoid0:gsensor_sim +connection = gyrometer_sim:RobotHardware_choreonoid0:gyrometer_sim diff --git a/hrpsys_choreonoid/config/SampleRobotCustomizer.yaml b/hrpsys_choreonoid/config/SampleRobotCustomizer.yaml new file mode 100644 index 00000000..8437b048 --- /dev/null +++ b/hrpsys_choreonoid/config/SampleRobotCustomizer.yaml @@ -0,0 +1,5 @@ +bush: + springT: 3.3e5 + dampingT: 3.3e2 + springR: 2.5e3 + dampingR: 2.5 diff --git a/hrpsys_choreonoid/config/SampleRobot_RH_LOAD_OBJ.cnoid.in b/hrpsys_choreonoid/config/SampleRobot_RH_LOAD_OBJ.cnoid.in new file mode 100644 index 00000000..41d3bbde --- /dev/null +++ b/hrpsys_choreonoid/config/SampleRobot_RH_LOAD_OBJ.cnoid.in @@ -0,0 +1,368 @@ +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: "SampleRobot" + plugin: Body + class: BodyItem + data: + modelFile: "@OPENHRP_SAMPLE_DIR@/model/sample1_bush.wrl" + currentBaseLink: "WAIST" + rootPosition: [ 0.0, 0.0, 0.680 ] + rootAttitude: [ + 1, 0, 0, + 0, 1, 0, + 0, 0, 1 ] + jointPositions: [ + -7.778932e-05, -0.378613, -0.00021, 0.832039, -0.452564, 0.000245, + 0.31129, -0.159481, -0.115399, -0.636277, 0.0, 0.0, 0.0, + -7.778932e-05, -0.378613, -0.00021, 0.832039, -0.452564, 0.000245, + 0.31129, 0.159481, 0.115399, -0.636277, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0] + initialRootPosition: [ 0.0, 0.0, 0.680 ] + initialRootAttitude: [ + 1, 0, 0, + 0, 1, 0, + 0, 0, 1 ] + initialJointPositions: [ + -7.778932e-05, -0.378613, -0.00021, 0.832039, -0.452564, 0.000245, + 0.31129, -0.159481, -0.115399, -0.636277, 0.0, 0.0, 0.0, + -7.778932e-05, -0.378613, -0.00021, 0.832039, -0.452564, 0.000245, + 0.31129, 0.159481, 0.115399, -0.636277, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0] + zmp: [ 0, 0, 0 ] + collisionDetection: true + selfCollisionDetection: false + isEditable: true + children: + - + id: 3 + name: "BodyRTC" + plugin: OpenRTM + class: BodyRTCItem + data: + isImmediateMode: true + moduleName: "@HRPSYS_CHOREONOID_DIRECTORY@/lib/RobotHardware_choreonoid" + confFileName: "@HRPSYS_CHOREONOID_DIRECTORY@/config/BodyRTC_SampleRobot.RH.conf" + configurationMode: Use Configuration File + AutoConnect: false + InstanceName: SampleRobot(Robot)0 + bodyPeriodicRate: 0.002 + - + id: 4 + name: "add_objects.py" + plugin: Python + class: PythonScriptItem + data: + file: "@HRPSYS_CHOREONOID_DIRECTORY@/launch/add_objects.py" + executionOnLoading: true + backgroundExecution: false + - + id: 5 + 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: 9 + name: "ros_service_server.py" + plugin: Python + class: PythonScriptItem + data: + file: "@JVRC_RTC_DIRECTORY@/scripts/ros_service_server.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/config/flat.yaml b/hrpsys_choreonoid/config/flat.yaml new file mode 100644 index 00000000..9ba61a95 --- /dev/null +++ b/hrpsys_choreonoid/config/flat.yaml @@ -0,0 +1,5 @@ +obj1: + name: 'VISIBLE_FLOOR' + file: '$(find choreonoid)/share/model/misc/floor.body' + translation: [0, 0, -0.1] + rotation: [[1, 0, 0], [0, 1, 0], [0, 0, 1]] diff --git a/hrpsys_choreonoid/launch/samplerobot_choreonoid.launch.in b/hrpsys_choreonoid/launch/samplerobot_choreonoid.launch.in new file mode 100644 index 00000000..ed643fff --- /dev/null +++ b/hrpsys_choreonoid/launch/samplerobot_choreonoid.launch.in @@ -0,0 +1,41 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/hrpsys_choreonoid/package.xml b/hrpsys_choreonoid/package.xml index 9fc908cc..2012d558 100644 --- a/hrpsys_choreonoid/package.xml +++ b/hrpsys_choreonoid/package.xml @@ -24,6 +24,7 @@ hrpsys openhrp3 choreonoid + hrpsys_ros_bridge roscpp tf openrtm_aist diff --git a/hrpsys_choreonoid/scripts/samplerobot_rh_setup.py b/hrpsys_choreonoid/scripts/samplerobot_rh_setup.py new file mode 100755 index 00000000..e1bd02ba --- /dev/null +++ b/hrpsys_choreonoid/scripts/samplerobot_rh_setup.py @@ -0,0 +1,77 @@ +#!/usr/bin/env python + +import sys +import rospkg +sys.path.append(rospkg.RosPack().get_path("hrpsys_ros_bridge") + "/test") +from samplerobot_hrpsys_config import * + +class SampleRobotChoreonoidHrpsysConfigurator(SampleRobotHrpsysConfigurator): + def waitForRobotHardware(self, robotname="Robot"): + '''!@brief + Wait for RobotHardware is exists and activated. + + @param robotname str: name of RobotHardware component. + ''' + self.rh = None + timeout_count = 0 + # wait for simulator or RobotHardware setup which sometime takes a long time + while self.rh == None and timeout_count < 10: # <- time out limit + if timeout_count > 0: # do not sleep initial loop + time.sleep(1); + self.rh = rtm.findRTC("RobotHardware_choreonoid0") + print(self.configurator_name + "wait for %s : %s ( timeout %d < 10)" % ( robotname, self.rh, timeout_count)) + if self.rh and self.rh.isActive() == None: # just in case rh is not ready... + self.rh = None + timeout_count += 1 + + if not self.rh: + print(self.configurator_name + "Could not find RobotHardware_choreonoid0") + if self.ms: + print(self.configurator_name + "Candidates are .... " + str([x.name() for x in self.ms.get_components()])) + print(self.configurator_name + "Exitting.... " + robotname) + exit(1) + + print(self.configurator_name + "findComps -> RobotHardware_choreonoid0 : %s isActive? = %s " % (self.rh, self.rh.isActive())) + + # wait for simulator or RobotHardware setup which sometime takes a long time + self.rh_choreonoid = None + while self.rh_choreonoid == None and timeout_count < 10: # <- time out limit + if timeout_count > 0: # do not sleep initial loop + time.sleep(1); + self.rh_choreonoid = rtm.findRTC(robotname) + print(self.configurator_name + "wait for %s : %s ( timeout %d < 10)" % ( robotname, self.rh_choreonoid, timeout_count)) + if self.rh_choreonoid and self.rh_choreonoid.isActive() == None: # just in case rh is not ready... + self.rh_choreonoid = None + timeout_count += 1 + + if not self.rh_choreonoid: + print(self.configurator_name + "Could not find " + robotname) + if self.ms: + print(self.configurator_name + "Candidates are .... " + str([x.name() for x in self.ms.get_components()])) + print(self.configurator_name + "Exitting.... " + robotname) + exit(1) + + print(self.configurator_name + "findComps -> %s : %s isActive? = %s " % (robotname, self.rh_choreonoid, self.rh_choreonoid.isActive())) + + def activateComps(self): + stash_rh = self.rh + self.rh = self.rh_choreonoid + HrpsysConfigurator.activateComps(self) + self.rh = stash_rh + + def startABSTIMP (self): + self.startAutoBalancer() + self.ic_svc.startImpedanceController("larm") + self.ic_svc.startImpedanceController("rarm") + self.startStabilizer() + +if __name__ == '__main__': + hcf = SampleRobotChoreonoidHrpsysConfigurator("SampleRobot") + if len(sys.argv) > 2 : + hcf.init(sys.argv[1], sys.argv[2]) + hcf.startABSTIMP() + elif len(sys.argv) > 1 : + hcf.init(sys.argv[1]) + hcf.startABSTIMP() + else : + hcf.init()