diff --git a/sample/CMakeLists.txt b/sample/CMakeLists.txt index 7b1e4b17349..a068ec004a0 100644 --- a/sample/CMakeLists.txt +++ b/sample/CMakeLists.txt @@ -7,6 +7,7 @@ endif() add_subdirectory(PA10) add_subdirectory(HRP4C) +add_subdirectory(SampleRobot) install(PROGRAMS simTest1.py simTest2.py simTest3.py simTest4.py simTest5.py diff --git a/sample/SampleRobot/CMakeLists.txt b/sample/SampleRobot/CMakeLists.txt new file mode 100644 index 00000000000..1663907ad1c --- /dev/null +++ b/sample/SampleRobot/CMakeLists.txt @@ -0,0 +1,35 @@ +# generate .conf and .xml files for two controller period +set(controller_time_list 0.002 0.005) # [s] +set(controller_period_list 500 200) # [Hz] +foreach(_idx 0 1) + list(GET controller_time_list ${_idx} _tmp_controller_time) + list(GET controller_period_list ${_idx} _tmp_controller_period) + set(CONTROLLER_TIME ${_tmp_controller_time}) + set(CONTROLLER_PERIOD ${_tmp_controller_period}) + configure_file(SampleRobot.RobotHardware.conf.in ${CMAKE_CURRENT_BINARY_DIR}/SampleRobot.RobotHardware.${_tmp_controller_period}.conf) + configure_file(SampleRobot.xml.in ${CMAKE_CURRENT_BINARY_DIR}/SampleRobot.${_tmp_controller_period}.xml) + configure_file(SampleRobot.conf.in ${CMAKE_CURRENT_BINARY_DIR}/SampleRobot.${_tmp_controller_period}.conf) +endforeach() + +configure_file(samplerobot_walk.py.in ${CMAKE_CURRENT_BINARY_DIR}/samplerobot_walk.py) + +install(PROGRAMS + samplerobot_data_logger.py + samplerobot_auto_balancer.py + samplerobot_stabilizer.py + samplerobot_remove_force_offset.py + samplerobot_impedance_controller.py + ${CMAKE_CURRENT_BINARY_DIR}/samplerobot_walk.py + DESTINATION share/hrpsys/samples/SampleRobot) + +install(FILES + # files for 500[Hz] = 0.002[s] + ${CMAKE_CURRENT_BINARY_DIR}/SampleRobot.500.conf + ${CMAKE_CURRENT_BINARY_DIR}/SampleRobot.500.xml + ${CMAKE_CURRENT_BINARY_DIR}/SampleRobot.RobotHardware.500.conf + # files for 200[Hz] = 0.005[s] + ${CMAKE_CURRENT_BINARY_DIR}/SampleRobot.200.conf + ${CMAKE_CURRENT_BINARY_DIR}/SampleRobot.200.xml + ${CMAKE_CURRENT_BINARY_DIR}/SampleRobot.RobotHardware.200.conf + DESTINATION share/hrpsys/samples/SampleRobot) + diff --git a/sample/SampleRobot/SampleRobot.RobotHardware.conf.in b/sample/SampleRobot/SampleRobot.RobotHardware.conf.in new file mode 100644 index 00000000000..c7a804517b4 --- /dev/null +++ b/sample/SampleRobot/SampleRobot.RobotHardware.conf.in @@ -0,0 +1,5 @@ +model: file://@OPENHRP_DIR@/share/OpenHRP-3.1/sample/model/sample1.wrl +exec_cxt.periodic.type: hrpExecutionContext +exec_cxt.periodic.rate: @CONTROLLER_PERIOD@ + + diff --git a/sample/SampleRobot/SampleRobot.conf.in b/sample/SampleRobot/SampleRobot.conf.in new file mode 100644 index 00000000000..7bf6082e2a1 --- /dev/null +++ b/sample/SampleRobot/SampleRobot.conf.in @@ -0,0 +1,6 @@ +model: file://@OPENHRP_DIR@/share/OpenHRP-3.1/sample/model/sample1.wrl +dt: @CONTROLLER_TIME@ + +abc_leg_offset: 0,0.09,0 +abc_stride_parameter: 0.15,0.05,10 +end_effectors: :rarm,RARM_WRIST_P,CHEST,0.0,-5.684342e-17,-0.12,9.813078e-18,1.0,0.0,1.5708, :larm,LARM_WRIST_P,CHEST,0.0,5.684342e-17,-0.12,-9.813078e-18,1.0,0.0,1.5708, :rleg,RLEG_ANKLE_R,WAIST,0.0,0.0,-0.07,0.0,0.0,0.0,0.0, :lleg,LLEG_ANKLE_R,WAIST,0.0,0.0,-0.07,0.0,0.0,0.0,0.0, diff --git a/sample/SampleRobot/SampleRobot.xml.in b/sample/SampleRobot/SampleRobot.xml.in new file mode 100644 index 00000000000..aece726febb --- /dev/null +++ b/sample/SampleRobot/SampleRobot.xml.in @@ -0,0 +1,192 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/sample/SampleRobot/samplerobot_auto_balancer.py b/sample/SampleRobot/samplerobot_auto_balancer.py new file mode 100755 index 00000000000..10e4ad75af0 --- /dev/null +++ b/sample/SampleRobot/samplerobot_auto_balancer.py @@ -0,0 +1,102 @@ +#!/usr/bin/env python + +try: + from hrpsys.hrpsys_config import * + import OpenHRP +except: + print "import without hrpsys" + import rtm + from rtm import * + from OpenHRP import * + import waitInput + from waitInput import * + import socket + import time + +def getRTCList (): + return [ + ['seq', "SequencePlayer"], + ['sh', "StateHolder"], + ['fk', "ForwardKinematics"], + ['abc', "AutoBalancer"], + ] + +def init (): + global hcf + hcf = HrpsysConfigurator() + hcf.getRTCList = getRTCList + hcf.init ("SampleRobot(Robot)0") + +def demo(): + init() + # set initial pose from sample/controller/SampleController/etc/Sample.pos + initial_pose = [-7.779e-005, -0.378613, -0.000209793, 0.832038, -0.452564, 0.000244781, 0.31129, -0.159481, -0.115399, -0.636277, 0, 0, 0, -7.77902e-005, -0.378613, -0.000209794, 0.832038, -0.452564, 0.000244781, 0.31129, 0.159481, 0.115399, -0.636277, 0, 0, 0, 0, 0, 0] + arm_front_pose = [-7.779e-005, -0.378613, -0.000209793, 0.832038, -0.452564, 0.000244781, -0.8, -0.159481, -0.115399, -0.636277, 0, 0, 0, -7.77902e-005, -0.378613, -0.000209794, 0.832038, -0.452564, 0.000244781, -0.8, 0.159481, 0.115399, -0.636277, 0, 0, 0, 0.2, 0, 0] + hcf.seq_svc.setJointAngles(initial_pose, 2.0) + hcf.seq_svc.waitInterpolation() + + # sample for AutoBalancer mode + # 1. AutoBalancer mode by fixing feet + hcf.abc_svc.startAutoBalancer([":rleg", ":lleg"]); + hcf.seq_svc.setJointAngles(arm_front_pose, 1.0) + hcf.seq_svc.waitInterpolation() + hcf.seq_svc.setJointAngles(initial_pose, 1.0) + hcf.seq_svc.waitInterpolation() + hcf.abc_svc.stopAutoBalancer(); + # 2. AutoBalancer mode by fixing hands and feet + hcf.abc_svc.startAutoBalancer([":rleg", ":lleg", ":rarm", ":larm"]) + hcf.seq_svc.setJointAngles(arm_front_pose, 1.0) + hcf.seq_svc.waitInterpolation() + hcf.seq_svc.setJointAngles(initial_pose, 1.0) + hcf.seq_svc.waitInterpolation() + hcf.abc_svc.stopAutoBalancer(); + # 3. getAutoBalancerParam + ret = hcf.abc_svc.getAutoBalancerParam() + if ret[0]: + print "getAutoBalancerParam() => OK" + # 4. setAutoBalancerParam + ret[1].default_zmp_offsets = [[0.1,0,0], [0.1,0,0]] + hcf.abc_svc.setAutoBalancerParam(ret[1]) + if ret[0] and ret[1].default_zmp_offsets == [[0.1,0,0], [0.1,0,0]]: + print "setAutoBalancerParam() => OK" + hcf.abc_svc.startAutoBalancer([":rleg", ":lleg"]); + hcf.abc_svc.stopAutoBalancer(); + ret[1].default_zmp_offsets = [[0,0,0], [0,0,0]] + hcf.abc_svc.setAutoBalancerParam(ret[1]) + + # sample for walk pattern generation by AutoBalancer RTC + # 1. goPos + hcf.abc_svc.goPos(0.1, 0.05, 20) + hcf.abc_svc.waitFootSteps() + # 2. goVelocity and goStop + hcf.abc_svc.goVelocity(-0.1, -0.05, -20) + time.sleep(1) + hcf.abc_svc.goStop() + # 3. setFootSteps + hcf.abc_svc.setFootSteps([OpenHRP.AutoBalancerService.Footstep([0,-0.09,0], [1,0,0,0], ":rleg"), OpenHRP.AutoBalancerService.Footstep([0,0.09,0], [1,0,0,0], ":lleg")]) + hcf.abc_svc.waitFootSteps() + hcf.abc_svc.setFootSteps([OpenHRP.AutoBalancerService.Footstep([0,-0.09,0], [1,0,0,0], ":rleg"), OpenHRP.AutoBalancerService.Footstep([0.15,0.09,0], [1,0,0,0], ":lleg"), OpenHRP.AutoBalancerService.Footstep([0.3,-0.09,0], [1,0,0,0], ":rleg"), OpenHRP.AutoBalancerService.Footstep([0.3,0.09,0], [1,0,0,0], ":lleg")]) + hcf.abc_svc.waitFootSteps() + # 4. getGaitGeneratorParam + ret = hcf.abc_svc.getGaitGeneratorParam() + if ret[0]: + print "getGaitGeneratorParam() => OK" + # 5. setGaitGeneratorParam + ret[1].default_step_time = 0.7 + ret[1].default_step_height = 0.15 + ret[1].default_double_support_ratio = 0.4 + hcf.abc_svc.setGaitGeneratorParam(ret[1]) + ret = hcf.abc_svc.getGaitGeneratorParam() + if ret[0] and ret[1].default_step_time == 0.7 and ret[1].default_step_height == 0.15 and ret[1].default_double_support_ratio == 0.4: + print "setGaitGeneratorParam() => OK" + hcf.abc_svc.goVelocity(0,0,0) + time.sleep(1) + hcf.abc_svc.goStop() + # 6. walking by fixing + # abc_svc.startAutoBalancer([AutoBalancerService.AutoBalancerLimbParam(":rleg", [0,0,0], [0,0,0,0]), + # AutoBalancerService.AutoBalancerLimbParam(":lleg", [0,0,0], [0,0,0,0]), + # AutoBalancerService.AutoBalancerLimbParam(":rarm", [0,0,0], [0,0,0,0]), + # AutoBalancerService.AutoBalancerLimbParam(":larm", [0,0,0], [0,0,0,0])]) + # abc_svc.goPos(0.1, 0.05, 20) + # abc_svc.waitFootSteps() + # abc_svc.stopABC() diff --git a/sample/SampleRobot/samplerobot_data_logger.py b/sample/SampleRobot/samplerobot_data_logger.py new file mode 100755 index 00000000000..19d69fc2307 --- /dev/null +++ b/sample/SampleRobot/samplerobot_data_logger.py @@ -0,0 +1,43 @@ +#!/usr/bin/env python + +try: + from hrpsys.hrpsys_config import * + import OpenHRP +except: + print "import without hrpsys" + import rtm + from rtm import * + from OpenHRP import * + import waitInput + from waitInput import * + import socket + import time + +def getRTCList (): + return [ + ['seq', "SequencePlayer"], + ['sh', "StateHolder"], + ['fk', "ForwardKinematics"], + ['log', "DataLogger"], + ] + +def init (): + global hcf + hcf = HrpsysConfigurator() + hcf.getRTCList = getRTCList + hcf.init ("SampleRobot(Robot)0") + +def demo (): + init() + # set initial pose from sample/controller/SampleController/etc/Sample.pos + initial_pose = [-7.779e-005, -0.378613, -0.000209793, 0.832038, -0.452564, 0.000244781, 0.31129, -0.159481, -0.115399, -0.636277, 0, 0, 0, -7.77902e-005, -0.378613, -0.000209794, 0.832038, -0.452564, 0.000244781, 0.31129, 0.159481, 0.115399, -0.636277, 0, 0, 0, 0, 0, 0] + + # Set max ring-buffer length : 200 [loop] * 0.002 [s] = 0.4 [s] data + hcf.log_svc.maxLength(200) + # Clear buffer + hcf.log_svc.clear() + hcf.seq_svc.setJointAngles(initial_pose, 2.0) + hcf.seq_svc.waitInterpolation() + # Save log files for each ports as /tmp/test-samplerobot-log.* + # file names are /tmp/test-samplerobot-log.[RTCName]_[PortName], c.f., /tmp/test-samplerobot-log.sh_qOut ... etc + hcf.log_svc.save("/tmp/test-samplerobot-log") diff --git a/sample/SampleRobot/samplerobot_impedance_controller.py b/sample/SampleRobot/samplerobot_impedance_controller.py new file mode 100755 index 00000000000..45741176b7a --- /dev/null +++ b/sample/SampleRobot/samplerobot_impedance_controller.py @@ -0,0 +1,55 @@ +#!/usr/bin/env python + +try: + from hrpsys.hrpsys_config import * + import OpenHRP +except: + print "import without hrpsys" + import rtm + from rtm import * + from OpenHRP import * + import waitInput + from waitInput import * + import socket + import time + +def getRTCList (): + return [ + ['seq', "SequencePlayer"], + ['sh', "StateHolder"], + ['fk', "ForwardKinematics"], + ['kf', "KalmanFilter"], + ['rmfo', "RemoveForceSensorLinkOffset"], + ['ic', "ImpedanceController"], + ] + +def init (): + global hcf + hcf = HrpsysConfigurator() + hcf.getRTCList = getRTCList + hcf.init ("SampleRobot(Robot)0") + +def demo(): + init() + # set initial pose from sample/controller/SampleController/etc/Sample.pos + initial_pose = [-7.779e-005, -0.378613, -0.000209793, 0.832038, -0.452564, 0.000244781, 0.31129, -0.159481, -0.115399, -0.636277, 0, 0, 0, -7.77902e-005, -0.378613, -0.000209794, 0.832038, -0.452564, 0.000244781, 0.31129, 0.159481, 0.115399, -0.636277, 0, 0, 0, 0, 0, 0] + hcf.seq_svc.setJointAngles(initial_pose, 2.0) + hcf.seq_svc.waitInterpolation() + + # 1. get + hcf.ic_svc.getImpedanceControllerParam("rhsensor") + ret1=hcf.ic_svc.getImpedanceControllerParam("rhsensor") + if ret1[0]: + print "getImpedanceControllerParam => OK" + # 2. set and start + ret1[1].base_name="CHEST" + ret1[1].target_name="RARM_WRIST_R" + hcf.ic_svc.setImpedanceControllerParam(ret1[1]) + ret2=hcf.ic_svc.setImpedanceControllerParam(ret1[1]) + ret3=hcf.ic_svc.getImpedanceControllerParam("rhsensor") + if ret2 and ret1[1].base_name == ret3[1].base_name and ret1[1].target_name == ret3[1].target_name: + print "setImpedanceControllerParam => OK" + # 3. stop + ret4=hcf.ic_svc.deleteImpedanceController("rhsensor") + if ret4: + print "deleteImpedanceController => OK" diff --git a/sample/SampleRobot/samplerobot_remove_force_offset.py b/sample/SampleRobot/samplerobot_remove_force_offset.py new file mode 100755 index 00000000000..6688d6b5c1e --- /dev/null +++ b/sample/SampleRobot/samplerobot_remove_force_offset.py @@ -0,0 +1,53 @@ +#!/usr/bin/env python + +try: + from hrpsys.hrpsys_config import * + import OpenHRP +except: + print "import without hrpsys" + import rtm + from rtm import * + from OpenHRP import * + import waitInput + from waitInput import * + import socket + import time + +def getRTCList (): + return [ + ['seq', "SequencePlayer"], + ['sh', "StateHolder"], + ['fk', "ForwardKinematics"], + ['kf', "KalmanFilter"], + ['rmfo', "RemoveForceSensorLinkOffset"], + ] + +def init (): + global hcf + hcf = HrpsysConfigurator() + hcf.getRTCList = getRTCList + hcf.init ("SampleRobot(Robot)0") + +def demo(): + import numpy + init() + # set initial pose from sample/controller/SampleController/etc/Sample.pos + initial_pose = [-7.779e-005, -0.378613, -0.000209793, 0.832038, -0.452564, 0.000244781, 0.31129, -0.159481, -0.115399, -0.636277, 0, 0, 0.637045, -7.77902e-005, -0.378613, -0.000209794, 0.832038, -0.452564, 0.000244781, 0.31129, 0.159481, 0.115399, -0.636277, 0, 0, -0.637045, 0, 0, 0] + hcf.seq_svc.setJointAngles(initial_pose, 0.5) + hcf.seq_svc.waitInterpolation() + # 1. force and moment are large because of link offsets + print numpy.linalg.norm(rtm.readDataPort(hcf.rmfo.port("off_rhsensor")).data) > 1e-2 + print numpy.linalg.norm(rtm.readDataPort(hcf.rmfo.port("off_lhsensor")).data) > 1e-2 + # 2. Set link offsets + # link_offset_centroid and link_offset_mass are identified value. + hcf.rmfo_svc.setForceMomentOffsetParam("rhsensor", OpenHRP.RemoveForceSensorLinkOffsetService.forcemomentOffsetParam(force_offset=[0,0,0], moment_offset=[0,0,0], link_offset_centroid=[0,0.0368,-0.076271], link_offset_mass=0.800011)) + hcf.rmfo_svc.setForceMomentOffsetParam("lhsensor", OpenHRP.RemoveForceSensorLinkOffsetService.forcemomentOffsetParam(force_offset=[0,0,0], moment_offset=[0,0,0], link_offset_centroid=[0,-0.0368,-0.076271], link_offset_mass=0.800011)) + ret = hcf.rmfo_svc.getForceMomentOffsetParam("rhsensor") + if ret[0] and ret[1].link_offset_mass == 0.800011: + print "getGaitGeneratorParam() => OK" + ret = hcf.rmfo_svc.getForceMomentOffsetParam("lhsensor") + if ret[0] and ret[1].link_offset_mass == 0.800011: + print "getGaitGeneratorParam() => OK" + # 3. force and moment are reduced + print numpy.linalg.norm(rtm.readDataPort(hcf.rmfo.port("off_lhsensor")).data) < 1e-2 + print numpy.linalg.norm(rtm.readDataPort(hcf.rmfo.port("off_lhsensor")).data) < 1e-2 diff --git a/sample/SampleRobot/samplerobot_stabilizer.py b/sample/SampleRobot/samplerobot_stabilizer.py new file mode 100755 index 00000000000..aeb14d009c7 --- /dev/null +++ b/sample/SampleRobot/samplerobot_stabilizer.py @@ -0,0 +1,45 @@ +#!/usr/bin/env python + +try: + from hrpsys.hrpsys_config import * + import OpenHRP +except: + print "import without hrpsys" + import rtm + from rtm import * + from OpenHRP import * + import waitInput + from waitInput import * + import socket + import time + +def getRTCList (): + return [ + ['seq', "SequencePlayer"], + ['sh', "StateHolder"], + ['fk', "ForwardKinematics"], + ['kf', "KalmanFilter"], + ['rmfo', "RemoveForceSensorLinkOffset"], + ['abc', "AutoBalancer"], + ['st', "Stabilizer"], + ] + +def init (): + global hcf + hcf = HrpsysConfigurator() + hcf.getRTCList = getRTCList + hcf.init ("SampleRobot(Robot)0") + +def demo(): + init() + # set initial pose from sample/controller/SampleController/etc/Sample.pos + initial_pose = [-7.779e-005, -0.378613, -0.000209793, 0.832038, -0.452564, 0.000244781, 0.31129, -0.159481, -0.115399, -0.636277, 0, 0, 0, -7.77902e-005, -0.378613, -0.000209794, 0.832038, -0.452564, 0.000244781, 0.31129, 0.159481, 0.115399, -0.636277, 0, 0, 0, 0, 0, 0] + hcf.seq_svc.setJointAngles(initial_pose, 2.0) + hcf.seq_svc.waitInterpolation() + + # s + hcf.st_svc.setParameter(OpenHRP.StabilizerService.stParam([0.5,0.5], [5.0, 5.0], [0.0,0.0], [0.1, 0.1], [0,0], [0,0], [0,0], [0,0], 0,0,0,0)) + hcf.st_svc.startStabilizer () + hcf.abc_svc.goPos(0.5, 0.1, 10) + hcf.abc_svc.waitFootSteps() + hcf.st_svc.stopStabilizer () diff --git a/sample/SampleRobot/samplerobot_walk.py.in b/sample/SampleRobot/samplerobot_walk.py.in new file mode 100755 index 00000000000..61449c12cc0 --- /dev/null +++ b/sample/SampleRobot/samplerobot_walk.py.in @@ -0,0 +1,35 @@ +#!/usr/bin/env python + +try: + from hrpsys.hrpsys_config import * + import OpenHRP +except: + print "import without hrpsys" + import rtm + from rtm import * + from OpenHRP import * + import waitInput + from waitInput import * + import socket + import time + +def getRTCList (): + return [ + ['seq', "SequencePlayer"], + ['sh', "StateHolder"], + ['fk', "ForwardKinematics"] + ] + +def init (): + global hcf + hcf = HrpsysConfigurator() + hcf.getRTCList = getRTCList + hcf.init ("SampleRobot(Robot)0") + +def loadPattern(basename, tm=1.0): + hcf.seq_svc.loadPattern(basename, tm) + hcf.seq_svc.waitInterpolation() + +def demo(): + init() + loadPattern("@OPENHRP_DIR@/share/OpenHRP-3.1/sample/controller/SampleController/etc/Sample")