Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Add terrain environment and walk example #313

Merged
merged 2 commits into from
Aug 29, 2014
Merged

Add terrain environment and walk example #313

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
cb9f002
(environments) : Add environments directory and terrain floor VRML
snozawa Aug 29, 2014
5ff03cc
(SampleRobot) : Add example for stair up, stair down, and slope updow…
snozawa Aug 29, 2014
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
1 change: 1 addition & 0 deletions sample/CMakeLists.txt
View file
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,4 @@
install(FILES
TerrainFloor.wrl
DESTINATION share/hrpsys/samples/environments)

Loading