Skip to content

Commit

Permalink
Merge pull request #313 from snozawa/add_terrain_environment_and_walk…
Browse files Browse the repository at this point in the history 
…_example

Add terrain environment and walk example
  • Loading branch information
@fkanehiro
fkanehiro committed Aug 29, 2014
2 parents c49814e + 5ff03cc commit 346ed1e
Show file tree
Hide file tree
Showing 6 changed files with 9,168 additions and 0 deletions.
1 change: 1 addition & 0 deletions sample/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -9,6 +9,7 @@ add_subdirectory(PA10)
add_subdirectory(HRP4C)
add_subdirectory(SampleRobot)
add_subdirectory(Sample6dofRobot)
add_subdirectory(environments)

install(PROGRAMS
simTest1.py simTest2.py simTest3.py simTest4.py simTest5.py
Expand Down
15 changes: 15 additions & 0 deletions sample/SampleRobot/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12,9 +12,19 @@ foreach(_idx 0 1)
configure_file(SampleRobot.conf.in ${CMAKE_CURRENT_BINARY_DIR}/SampleRobot.${_tmp_controller_period}.conf)
endforeach()

set(robot_waist_translation "-0.8 0 0" "-4.15 -0.2 0" "-5.83 -0.2 -0.6096")
set(file_suffix "SlopeUpDown" "StairUp" "StairDown")
foreach(_idx 0 1 2)
list(GET robot_waist_translation ${_idx} _tmp_robot_waist_translation)
list(GET file_suffix ${_idx} _tmp_file_suffix)
set(ROBOT_WAIST_TRANSLATION ${_tmp_robot_waist_translation})
configure_file(SampleRobot.TerrainFloor.xml.in ${CMAKE_CURRENT_BINARY_DIR}/SampleRobot.TerrainFloor.${_tmp_file_suffix}.xml)
endforeach()

# Use configure_file to specify openhrp3 directory for sample1.wrl model
configure_file(samplerobot_walk.py.in ${CMAKE_CURRENT_BINARY_DIR}/samplerobot_walk.py)
configure_file(samplerobot_auto_balancer.py.in ${CMAKE_CURRENT_BINARY_DIR}/samplerobot_auto_balancer.py)
configure_file(samplerobot_terrain_walk.py.in ${CMAKE_CURRENT_BINARY_DIR}/samplerobot_terrain_walk.py)
configure_file(samplerobot_data_logger.py.in ${CMAKE_CURRENT_BINARY_DIR}/samplerobot_data_logger.py)
configure_file(samplerobot_impedance_controller.py.in ${CMAKE_CURRENT_BINARY_DIR}/samplerobot_impedance_controller.py)
configure_file(samplerobot_remove_force_offset.py.in ${CMAKE_CURRENT_BINARY_DIR}/samplerobot_remove_force_offset.py)
Expand All @@ -23,6 +33,7 @@ configure_file(samplerobot_stabilizer.py.in ${CMAKE_CURRENT_BINARY_DIR}/samplero
install(PROGRAMS
${CMAKE_CURRENT_BINARY_DIR}/samplerobot_data_logger.py
${CMAKE_CURRENT_BINARY_DIR}/samplerobot_auto_balancer.py
${CMAKE_CURRENT_BINARY_DIR}/samplerobot_terrain_walk.py
${CMAKE_CURRENT_BINARY_DIR}/samplerobot_stabilizer.py
${CMAKE_CURRENT_BINARY_DIR}/samplerobot_remove_force_offset.py
${CMAKE_CURRENT_BINARY_DIR}/samplerobot_impedance_controller.py
Expand All @@ -41,5 +52,9 @@ install(FILES
${CMAKE_CURRENT_BINARY_DIR}/SampleRobot.200.torque.xml
${CMAKE_CURRENT_BINARY_DIR}/SampleRobot.RobotHardware.200.conf
SampleRobot.PDgain.dat
# TerrainFloor
${CMAKE_CURRENT_BINARY_DIR}/SampleRobot.TerrainFloor.SlopeUpDown.xml
${CMAKE_CURRENT_BINARY_DIR}/SampleRobot.TerrainFloor.StairUp.xml
${CMAKE_CURRENT_BINARY_DIR}/SampleRobot.TerrainFloor.StairDown.xml
DESTINATION share/hrpsys/samples/SampleRobot)

198 changes: 198 additions & 0 deletions sample/SampleRobot/SampleRobot.TerrainFloor.xml.in
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,198 @@
<grxui>
<mode name="Simulation">
<item class="com.generalrobotix.ui.item.GrxSimulationItem" name="simulationItem">
<property name="integrate" value="true"/>
<property name="timeStep" value="0.005"/>
<property name="totalTime" value="2000000.0"/>
<property name="method" value="EULER"/>
</item>
<item class="com.generalrobotix.ui.item.GrxRTSItem" name="SampleRobot" select="true">
<property name="SampleRobot(Robot)0.period" value="0.005"/>
<property name="HGcontroller0.period" value="0.005"/>
<property name="HGcontroller0.factory" value="HGcontroller"/>
<property name="connection" value="HGcontroller0.qOut:SampleRobot(Robot)0.qRef"/>
<property name="connection" value="HGcontroller0.dqOut:SampleRobot(Robot)0.dqRef"/>
<property name="connection" value="HGcontroller0.ddqOut:SampleRobot(Robot)0.ddqRef"/>
</item>
<item class="com.generalrobotix.ui.item.GrxModelItem" name="sample1" url="@OPENHRP_DIR@/share/OpenHRP-3.1/sample/model/sample1.wrl">
<property name="rtcName" value="SampleRobot(Robot)0"/>
<property name="inport" value="qRef:JOINT_VALUE"/>
<property name="inport" value="dqRef:JOINT_VELOCITY"/>
<property name="inport" value="ddqRef:JOINT_ACCELERATION"/>
<property name="outport" value="q:JOINT_VALUE"/>
<property name="outport" value="tau:JOINT_TORQUE"/>
<property name="outport" value="lfsensor:lfsensor:FORCE_SENSOR"/>
<property name="outport" value="rfsensor:rfsensor:FORCE_SENSOR"/>
<property name="outport" value="lhsensor:lhsensor:FORCE_SENSOR"/>
<property name="outport" value="rhsensor:rhsensor:FORCE_SENSOR"/>
<property name="outport" value="gyrometer:gyrometer:RATE_GYRO_SENSOR"/>
<property name="outport" value="gsensor:gsensor:ACCELERATION_SENSOR"/>
<property name="WAIST.NumOfAABB" value="1"/>
<property name="WAIST.translation" value="0 0 0.7235"/>
<property name="WAIST.rotation" value="1 0 0 0"/>
<property name="WAIST.mode" value="Torque"/>
<property name="controller" value="SampleRobot"/>
<property name="RLEG_HIP_R.angle" value="0.0"/>
<property name="RLEG_HIP_R.mode" value="HighGain"/>
<property name="RLEG_HIP_R.NumOfAABB" value="1"/>
<property name="RLEG_HIP_P.angle" value="0.0"/>
<property name="RLEG_HIP_P.mode" value="HighGain"/>
<property name="RLEG_HIP_P.NumOfAABB" value="1"/>
<property name="RLEG_HIP_Y.angle" value="0.0"/>
<property name="RLEG_HIP_Y.mode" value="HighGain"/>
<property name="RLEG_HIP_Y.NumOfAABB" value="1"/>
<property name="RLEG_KNEE.angle" value="0.0"/>
<property name="RLEG_KNEE.mode" value="HighGain"/>
<property name="RLEG_KNEE.NumOfAABB" value="1"/>
<property name="RLEG_ANKLE_P.angle" value="0.0"/>
<property name="RLEG_ANKLE_P.mode" value="HighGain"/>
<property name="RLEG_ANKLE_P.NumOfAABB" value="1"/>
<property name="RLEG_ANKLE_R.angle" value="0.0"/>
<property name="RLEG_ANKLE_R.mode" value="HighGain"/>
<property name="RLEG_ANKLE_R.NumOfAABB" value="1"/>
<property name="RARM_SHOULDER_P.angle" value="0.0"/>
<property name="RARM_SHOULDER_P.mode" value="HighGain"/>
<property name="RARM_SHOULDER_P.NumOfAABB" value="1"/>
<property name="RARM_SHOULDER_R.angle" value="0.0"/>
<property name="RARM_SHOULDER_R.mode" value="HighGain"/>
<property name="RARM_SHOULDER_R.NumOfAABB" value="1"/>
<property name="RARM_SHOULDER_Y.angle" value="0.0"/>
<property name="RARM_SHOULDER_Y.mode" value="HighGain"/>
<property name="RARM_SHOULDER_Y.NumOfAABB" value="1"/>
<property name="RARM_ELBOW.angle" value="0.0"/>
<property name="RARM_ELBOW.mode" value="HighGain"/>
<property name="RARM_ELBOW.NumOfAABB" value="1"/>
<property name="RARM_WRIST_Y.angle" value="0.0"/>
<property name="RARM_WRIST_Y.mode" value="HighGain"/>
<property name="RARM_WRIST_Y.NumOfAABB" value="1"/>
<property name="RARM_WRIST_P.angle" value="0.0"/>
<property name="RARM_WRIST_P.mode" value="HighGain"/>
<property name="RARM_WRIST_P.NumOfAABB" value="1"/>
<property name="RARM_WRIST_R.angle" value="0.0"/>
<property name="RARM_WRIST_R.mode" value="HighGain"/>
<property name="RARM_WRIST_R.NumOfAABB" value="1"/>
<property name="LLEG_HIP_R.angle" value="0.0"/>
<property name="LLEG_HIP_R.mode" value="HighGain"/>
<property name="LLEG_HIP_R.NumOfAABB" value="1"/>
<property name="LLEG_HIP_P.angle" value="0.0"/>
<property name="LLEG_HIP_P.mode" value="HighGain"/>
<property name="LLEG_HIP_P.NumOfAABB" value="1"/>
<property name="LLEG_HIP_Y.angle" value="0.0"/>
<property name="LLEG_HIP_Y.mode" value="HighGain"/>
<property name="LLEG_HIP_Y.NumOfAABB" value="1"/>
<property name="LLEG_KNEE.angle" value="0.0"/>
<property name="LLEG_KNEE.mode" value="HighGain"/>
<property name="LLEG_KNEE.NumOfAABB" value="1"/>
<property name="LLEG_ANKLE_P.angle" value="0.0"/>
<property name="LLEG_ANKLE_P.mode" value="HighGain"/>
<property name="LLEG_ANKLE_P.NumOfAABB" value="1"/>
<property name="LLEG_ANKLE_R.angle" value="0.0"/>
<property name="LLEG_ANKLE_R.mode" value="HighGain"/>
<property name="LLEG_ANKLE_R.NumOfAABB" value="1"/>
<property name="LARM_SHOULDER_P.angle" value="0.0"/>
<property name="LARM_SHOULDER_P.mode" value="HighGain"/>
<property name="LARM_SHOULDER_P.NumOfAABB" value="1"/>
<property name="LARM_SHOULDER_R.angle" value="0.0"/>
<property name="LARM_SHOULDER_R.mode" value="HighGain"/>
<property name="LARM_SHOULDER_R.NumOfAABB" value="1"/>
<property name="LARM_SHOULDER_Y.angle" value="0.0"/>
<property name="LARM_SHOULDER_Y.mode" value="HighGain"/>
<property name="LARM_SHOULDER_Y.NumOfAABB" value="1"/>
<property name="LARM_ELBOW.angle" value="0.0"/>
<property name="LARM_ELBOW.mode" value="HighGain"/>
<property name="LARM_ELBOW.NumOfAABB" value="1"/>
<property name="LARM_WRIST_Y.angle" value="0.0"/>
<property name="LARM_WRIST_Y.mode" value="HighGain"/>
<property name="LARM_WRIST_Y.NumOfAABB" value="1"/>
<property name="LARM_WRIST_P.angle" value="0.0"/>
<property name="LARM_WRIST_P.mode" value="HighGain"/>
<property name="LARM_WRIST_P.NumOfAABB" value="1"/>
<property name="LARM_WRIST_R.angle" value="0.0"/>
<property name="LARM_WRIST_R.mode" value="HighGain"/>
<property name="LARM_WRIST_R.NumOfAABB" value="1"/>
<property name="WAIST_P.angle" value="0.0"/>
<property name="WAIST_P.mode" value="HighGain"/>
<property name="WAIST_P.NumOfAABB" value="1"/>
<property name="WAIST_R.angle" value="0.0"/>
<property name="WAIST_R.mode" value="HighGain"/>
<property name="WAIST_R.NumOfAABB" value="1"/>
<property name="CHEST.angle" value="0.0"/>
<property name="CHEST.mode" value="HighGain"/>
<property name="CHEST.NumOfAABB" value="1"/>
</item>
<view class="com.generalrobotix.ui.view.GrxRobotHardwareClientView" name="RobotHardware RTC Client">
<property name="robotHost" value="localhost"/>
<property name="StateHolderRTC" value="StateHolder0"/>
<property name="interval" value="100"/>
<property name="RobotHardwareServiceRTC" value="RobotHardware0"/>
<property name="robotPort" value="2809"/>
<property name="ROBOT" value="SampleRobot"/>
</view>
<view class="com.generalrobotix.ui.view.Grx3DView" name="3DView">
<property name="view.mode" value="Room"/>
<property name="showCoM" value="false"/>
<property name="showCoMonFloor" value="false"/>
<property name="showDistance" value="false"/>
<property name="showIntersection" value="false"/>
<property name="eyeHomePosition" value="-0.70711 -0 0.70711 2 0.70711 -0 0.70711 2 0 1 0 0.8 0 0 0 1 "/>
<property name="showCollision" value="true"/>
<property name="showActualState" value="true"/>
<property name="showScale" value="true"/>
</view>
<item class="com.generalrobotix.ui.item.GrxRTSItem" name="TerrainFloor" select="true">
<property name="TerrainFloor(Robot)0.period" value="0.005"/>
<property name="HGcontroller0.period" value="0.005"/>
<property name="HGcontroller0.factory" value="HGcontroller"/>
<property name="connection" value="HGcontroller0.qOut:TerrainFloor(Robot)0.qRef"/>
<property name="connection" value="HGcontroller0.dqOut:TerrainFloor(Robot)0.dqRef"/>
<property name="connection" value="HGcontroller0.ddqOut:TerrainFloor(Robot)0.ddqRef"/>
</item>
<item class="com.generalrobotix.ui.item.GrxModelItem" name="TerrainFloor" url="@CMAKE_INSTALL_PREFIX@/share/hrpsys/samples/environments/TerrainFloor.wrl">
<property name="rtcName" value="TerrainFloor(Robot)0"/>
<property name="inport" value="qRef:JOINT_VALUE"/>
<property name="inport" value="dqRef:JOINT_VELOCITY"/>
<property name="inport" value="ddqRef:JOINT_ACCELERATION"/>
<property name="outport" value="q:JOINT_VALUE"/>
<property name="outport" value="tau:JOINT_TORQUE"/>
<property name="WAIST.NumOfAABB" value="1"/>
<property name="WAIST.translation" value="@ROBOT_WAIST_TRANSLATION@"/>
<!-- for demoSlopeUp -->
<!-- <property name="WAIST.translation" value="-0.8 0 0"/> -->
<!-- for demoStairUp -->
<!-- <property name="WAIST.translation" value="-4.15 -0.2 0"/> -->
<!-- for demoStairDown -->
<!-- <property name="WAIST.translation" value="-5.83 -0.2 -0.6096"/> -->
<property name="WAIST.rotation" value="1 0 0 0"/>
</item>
<view class="com.generalrobotix.ui.view.GrxRobotHardwareClientView" name="RobotHardware RTC Client">
<property name="robotHost" value="localhost"/>
<property name="StateHolderRTC" value="StateHolder0"/>
<property name="interval" value="100"/>
<property name="RobotHardwareServiceRTC" value="RobotHardware0"/>
<property name="robotPort" value="2809"/>
<property name="ROBOT" value="TerrainFloor"/>
</view>
<view class="com.generalrobotix.ui.view.Grx3DView" name="3DView">
<property name="view.mode" value="Room"/>
<property name="showCoM" value="false"/>
<property name="showCoMonFloor" value="false"/>
<property name="showDistance" value="false"/>
<property name="showIntersection" value="false"/>
<property name="eyeHomePosition" value="-0.70711 -0 0.70711 2 0.70711 -0 0.70711 2 0 1 0 0.8 0 0 0 1 "/>
<property name="showCollision" value="true"/>
<property name="showActualState" value="true"/>
<property name="showScale" value="true"/>
</view>
<item class="com.generalrobotix.ui.item.GrxCollisionPairItem" name="CP#TerrainFloor_#sample1_">
<property name="springConstant" value="0 0 0 0 0 0"/>
<property name="slidingFriction" value="0.5"/>
<property name="jointName2" value=""/>
<property name="jointName1" value=""/>
<property name="damperConstant" value="0 0 0 0 0 0"/>
<property name="objectName2" value="TerrainFloor"/>
<property name="objectName1" value="sample1"/>
<property name="springDamperModel" value="false"/>
<property name="staticFriction" value="0.5"/>
</item>
</mode>
</grxui>
92 changes: 92 additions & 0 deletions sample/SampleRobot/samplerobot_terrain_walk.py.in
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,92 @@
#!/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 init ():
global hcf
hcf = HrpsysConfigurator()
hcf.getRTCList = hcf.getRTCListUnstable
hcf.init ("SampleRobot(Robot)0", "@OPENHRP_DIR@/share/OpenHRP-3.1/sample/model/sample1.wrl")

def initPose():
initial_pose=[1.336179e-17, -0.631511, -6.328912e-18, 1.30679, -0.675275, -1.709808e-17, 0.523599, 0.0, 0.0, -1.0472, 0.15708, -0.113446, 0.637045, -1.132555e-17, -0.631511, -6.328860e-18, 1.30679, -0.675275, 7.588845e-18, 0.523599, 0.0, 0.0, -1.0472, -0.15708, -0.113446, -0.637045, 0.0, 0.0, 0.0]
hcf.seq_svc.setJointAngles(initial_pose, 1.0)
hcf.seq_svc.waitInterpolation()

def demo():
init()
initPose()
print "Please execute 'demoStairUp()', 'demoStairDown()', and 'demoSlopeUpDown()'"

def setupGaitGeneratorParam():
ggp = hcf.abc_svc.getGaitGeneratorParam()
ggp[1].default_step_height = 0.20
ggp[1].default_double_support_ratio = 0.3
ggp[1].default_step_time = 1.2
ggp[1].default_orbit_type = OpenHRP.AutoBalancerService.RECTANGLE;
hcf.abc_svc.setGaitGeneratorParam(ggp[1])

def stairWalk(stair_height = 0.1524):
stair_stride_x = 0.25
floor_stride_x = 0.157
init_step_x = 0
init_step_z = 0
for step_idx in [1,2,3,4]:
hcf.abc_svc.setFootSteps([OpenHRP.AutoBalancerService.Footstep([init_step_x, -0.09, init_step_z], [1,0,0,0], "rleg"),
OpenHRP.AutoBalancerService.Footstep([init_step_x+stair_stride_x, 0.09, init_step_z+stair_height], [1,0,0,0], "lleg")])
hcf.abc_svc.waitFootSteps()
hcf.abc_svc.setFootSteps([OpenHRP.AutoBalancerService.Footstep([init_step_x+stair_stride_x, 0.09, init_step_z+stair_height], [1,0,0,0], "lleg"),
OpenHRP.AutoBalancerService.Footstep([init_step_x+stair_stride_x, -0.09, init_step_z+stair_height], [1,0,0,0], "rleg")])
hcf.abc_svc.waitFootSteps()
hcf.abc_svc.setFootSteps([OpenHRP.AutoBalancerService.Footstep([init_step_x+stair_stride_x, 0.09, init_step_z+stair_height], [1,0,0,0], "lleg"),
OpenHRP.AutoBalancerService.Footstep([init_step_x+stair_stride_x+floor_stride_x, -0.09, init_step_z+stair_height], [1,0,0,0], "rleg"),
OpenHRP.AutoBalancerService.Footstep([init_step_x+stair_stride_x+floor_stride_x, 0.09, init_step_z+stair_height], [1,0,0,0], "lleg")])
hcf.abc_svc.waitFootSteps()
init_step_x = init_step_x + stair_stride_x + floor_stride_x
init_step_z = init_step_z + stair_height

# sample for SampleRobot.TerrainFloor.SlopeUpDown.xml
def demoSlopeUpDown():
print "Start stlop up down"
setupGaitGeneratorParam()
hcf.abc_svc.startAutoBalancer(["rleg", "lleg"]);
fsList=[OpenHRP.AutoBalancerService.Footstep([0.8,-0.09,0.0], [1.0,0.0,2.775558e-17,0.0], "rleg"), OpenHRP.AutoBalancerService.Footstep([1.0953,0.09,0.030712], [0.991445,0.0,-0.130526,0.0], "lleg"), OpenHRP.AutoBalancerService.Footstep([1.28848,-0.09,0.082475], [0.991445,0.0,-0.130526,0.0], "rleg"), OpenHRP.AutoBalancerService.Footstep([1.38508,0.09,0.108357], [0.991445,0.0,-0.130526,0.0], "lleg"), OpenHRP.AutoBalancerService.Footstep([1.38508,-0.09,0.108357], [0.991445,0.0,-0.130526,0.0], "rleg"), OpenHRP.AutoBalancerService.Footstep([1.54959,0.09,0.125863], [0.991445,0.0,0.130526,0.0], "lleg"), OpenHRP.AutoBalancerService.Footstep([1.74277,-0.09,0.074099], [0.991445,0.0,0.130526,0.0], "rleg"), OpenHRP.AutoBalancerService.Footstep([1.79107,0.09,0.061158], [0.991445,0.0,0.130526,0.0], "lleg"), OpenHRP.AutoBalancerService.Footstep([2.05,-0.09,0.0], [1.0,0.0,0.0,0.0], "rleg"), OpenHRP.AutoBalancerService.Footstep([2.05,0.09,0.0], [1.0,0.0,0.0,0.0], "lleg")]
# set st Parameter
# stp1 = hcf.st_svc.getParameter()
# stp1.k_tpcc_p=[0.05, 0.05]
# stp1.k_tpcc_x=[4.0, 4.0]
# stp1.k_brot_p=[0.0, 0.0]
# hcf.st_svc.setParameter(stp1)
# hcf.st_svc.startStabilizer ()
for idx in range(len(fsList)-1):
hcf.abc_svc.setFootSteps([fsList[idx],fsList[idx+1]])
hcf.abc_svc.waitFootSteps()
hcf.abc_svc.stopAutoBalancer();

# sample for SampleRobot.TerrainFloor.StairUp.xml
def demoStairUp():
print "Start stair up"
setupGaitGeneratorParam()
hcf.abc_svc.startAutoBalancer(["rleg", "lleg"]);
stairWalk()
hcf.abc_svc.stopAutoBalancer();

# sample for SampleRobot.TerrainFloor.StairDown.xml
def demoStairDown():
print "Start stair down"
setupGaitGeneratorParam()
hcf.abc_svc.startAutoBalancer(["rleg", "lleg"]);
stairWalk(-0.1524)
hcf.abc_svc.stopAutoBalancer();

4 changes: 4 additions & 0 deletions sample/environments/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,4 @@
install(FILES
TerrainFloor.wrl
DESTINATION share/hrpsys/samples/environments)

Loading

0 comments on commit 346ed1e

Please sign in to comment.