Commit 2c4f5854 authored by Steve Tonneau's avatar Steve Tonneau
Browse files

standing up scenario

parent bf371717
<?xml version="1.0"?>
<robot name="stair_bauzil">
<handle name="handle">
<position> 0 0 0 1 0 0 0 </position>
<link name="base_link"/>
</handle>
<handle name="handle2">
<position> 0 0 0
0 0 0.7071067811865476 0.7071067811865476 </position>
<link name="base_link"/>
</handle>
<contact name="box_surface">
<link name="base_link"/>
<point>-0.025 -0.025 -0.025 -0.025 0.025 -0.025 -0.025 -0.025 0.025 -0.025 0.025 0.025
+0.025 -0.025 -0.025 +0.025 0.025 -0.025 +0.025 -0.025 0.025 +0.025 0.025 0.025 </point>
<triangle> 0 2 1 4 5 6</triangle>
</contact>
</robot>
<robot name="stair_bauzil">
<link name="base_link">
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="package:///../hpp-rbprm-corba/meshes/stair_bauzil.stl"/>
</geometry>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="package://hpp-rbprm-corba/meshes/stair_bauzil.stl"/>
</geometry>
</collision>
</link>
</robot>
......@@ -18,7 +18,7 @@ srdfSuffix = ""
fullBody = FullBody ()
fullBody.loadFullBodyModel(urdfName, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
fullBody.setJointBounds ("base_joint_xyz", [-1,1, -4, -1, 1, 2.2])
fullBody.setJointBounds ("base_joint_xyz", [-2,1, -1, 1, 0, 2.2])
ps = tp.ProblemSolver( fullBody )
......@@ -26,19 +26,19 @@ r = tp.Viewer (ps)
r.loadObstacleModel ('hpp-rbprm-corba', "scene_wall", "contact")
#~ AFTER loading obstacles
rLegId = '7rLeg'
rLegId = '0rLeg'
rLeg = 'RLEG_JOINT0'
rLegOffset = [0,-0.105,0,]
rLegNormal = [0,1,0]
rLegx = 0.09; rLegy = 0.05
fullBody.addLimb(rLegId,rLeg,'',rLegOffset,rLegNormal, rLegx, rLegy, 20000, 0.01)
fullBody.addLimb(rLegId,rLeg,'',rLegOffset,rLegNormal, rLegx, rLegy, 10000, 0.1)
lLegId = '8lLeg'
lLegId = '1lLeg'
lLeg = 'LLEG_JOINT0'
lLegOffset = [0,-0.105,0]
lLegNormal = [0,1,0]
lLegx = 0.09; lLegy = 0.05
fullBody.addLimb(lLegId,lLeg,'',lLegOffset,rLegNormal, lLegx, lLegy, 20000, 0.01)
fullBody.addLimb(lLegId,lLeg,'',lLegOffset,rLegNormal, lLegx, lLegy, 10000, 0.1)
rarmId = '3Rarm'
rarm = 'RARM_JOINT0'
......@@ -46,7 +46,7 @@ rHand = 'RARM_JOINT5'
rArmOffset = [-0.05,-0.050,-0.050]
rArmNormal = [1,0,0]
rArmx = 0.024; rArmy = 0.024
fullBody.addLimb(rarmId,rarm,rHand,rArmOffset,rArmNormal, rArmx, rArmy, 20000, 0.01)
fullBody.addLimb(rarmId,rarm,rHand,rArmOffset,rArmNormal, rArmx, rArmy, 10000, 0.01)
#~ AFTER loading obstacles
......@@ -56,25 +56,49 @@ lHand = 'LARM_JOINT5'
lArmOffset = [-0.05,-0.050,-0.050]
lArmNormal = [1,0,0]
lArmx = 0.024; lArmy = 0.024
fullBody.addLimb(larmId,larm,lHand,lArmOffset,lArmNormal, lArmx, lArmy, 20000, 0.01)
fullBody.addLimb(larmId,larm,lHand,lArmOffset,lArmNormal, lArmx, lArmy, 10000, 0.01)
rKneeId = '0RKnee'
rLeg = 'RLEG_JOINT0'
rKnee = 'RLEG_JOINT3'
rLegOffset = [0.105,0.055,0.017]
rLegNormal = [-1,0,0]
rLegx = 0.05; rLegy = 0.05
#~ fullBody.addLimb(rKneeId, rLeg,rKnee,rLegOffset,rLegNormal, rLegx, rLegy, 10000, 0.01)
#~
lKneeId = '1LKnee'
lLeg = 'LLEG_JOINT0'
lKnee = 'LLEG_JOINT3'
lLegOffset = [0.105,0.055,0.017]
lLegNormal = [-1,0,0]
lLegx = 0.05; lLegy = 0.05
#~ fullBody.addLimb(lKneeId,lLeg,lKnee,lLegOffset,lLegNormal, lLegx, lLegy, 10000, 0.01)
#~
fullBody.client.basic.robot.setJointConfig('LARM_JOINT0',[1])
fullBody.client.basic.robot.setJointConfig('RARM_JOINT0',[-1])
q_0 = fullBody.getCurrentConfig();
q_init = fullBody.getCurrentConfig(); q_init[0:7] = tp.q_init[0:7]
q_goal = fullBody.getCurrentConfig(); q_goal[0:7] = tp.q_goal[0:7]
fullBody.setCurrentConfig (q_init)
q_init = fullBody.generateContacts(q_init, [0,0,1])
r (q_init)
q_0 = fullBody.getCurrentConfig();
q_init = fullBody.generateContacts(q_init, [0,0,-1]); r (q_init)
fullBody.setCurrentConfig (q_goal)
#~ q_goal = fullBody.generateContacts(q_goal, [0,0,1])
r(q_goal)
q_goal = fullBody.generateContacts(q_goal, [0,0,1])
r(q_goal)
#~ fullBody.setStartState(q_init,[rLegId,lLegId,rarmId,larmId])
#~ fullBody.setEndState(q_goal,[rLegId,lLegId,rarmId,larmId])
fullBody.setStartState(q_init,[rLegId,lLegId]) #,rarmId,larmId])
fullBody.setEndState(q_goal,[rLegId,lLegId])#,rarmId,larmId])
#~
#~ configs = fullBody.interpolate(0.1)
#~ configs2 = fullBody.interpolate(0.05)
configs = fullBody.interpolate(0.09)
#~ configs = fullBody.interpolate(0.08)
i = 0;
r (configs[i]); i=i+1; i-1
#~ q_init = fullBody.generateContacts(q_init, [0,0,-1]); r (q_init)
from hpp.corbaserver.rbprm.rbprmbuilder import Builder
from hpp.corbaserver.rbprm.rbprmfullbody import FullBody
from hpp.gepetto import Viewer
import stair_path_bauzil_hrp2 as tp
packageName = "hrp2_14_description"
meshPackageName = "hrp2_14_description"
rootJointType = "freeflyer"
##
# Information to retrieve urdf and srdf files.
urdfName = "hrp2_14"
urdfSuffix = "_reduced"
srdfSuffix = ""
fullBody = FullBody ()
fullBody.loadFullBodyModel(urdfName, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
fullBody.setJointBounds ("base_joint_xyz", [0,2, -1, 1, 0, 2.2])
ps = tp.ProblemSolver( fullBody )
r = tp.Viewer (ps)
r.loadObstacleModel ('hpp-rbprm-corba', "stair_bauzil", "contact")
#~ AFTER loading obstacles
rLegId = '0rLeg'
rLeg = 'RLEG_JOINT0'
rLegOffset = [0,-0.105,0,]
rLegNormal = [0,1,0]
rLegx = 0.09; rLegy = 0.05
fullBody.addLimb(rLegId,rLeg,'',rLegOffset,rLegNormal, rLegx, rLegy, 20000, 0.01)
lLegId = '1lLeg'
lLeg = 'LLEG_JOINT0'
lLegOffset = [0,-0.105,0]
lLegNormal = [0,1,0]
lLegx = 0.09; lLegy = 0.05
fullBody.addLimb(lLegId,lLeg,'',lLegOffset,rLegNormal, lLegx, lLegy, 10000, 0.01)
rarmId = '3Rarm'
rarm = 'RARM_JOINT0'
rHand = 'RARM_JOINT5'
rArmOffset = [-0.05,-0.050,-0.050]
rArmNormal = [1,0,0]
rArmx = 0.024; rArmy = 0.024
fullBody.addLimb(rarmId,rarm,rHand,rArmOffset,rArmNormal, rArmx, rArmy, 30000, 0.01)
#~ AFTER loading obstacles
larmId = '4Larm'
larm = 'LARM_JOINT0'
lHand = 'LARM_JOINT5'
lArmOffset = [-0.05,-0.050,-0.050]
lArmNormal = [1,0,0]
lArmx = 0.024; lArmy = 0.024
#~ fullBody.addLimb(larmId,larm,lHand,lArmOffset,lArmNormal, lArmx, lArmy, 10000, 0.05)
rKneeId = '0RKnee'
rLeg = 'RLEG_JOINT0'
rKnee = 'RLEG_JOINT3'
rLegOffset = [0.105,0.055,0.017]
rLegNormal = [-1,0,0]
rLegx = 0.05; rLegy = 0.05
#~ fullBody.addLimb(rKneeId, rLeg,rKnee,rLegOffset,rLegNormal, rLegx, rLegy, 10000, 0.01)
#~
lKneeId = '1LKnee'
lLeg = 'LLEG_JOINT0'
lKnee = 'LLEG_JOINT3'
lLegOffset = [0.105,0.055,0.017]
lLegNormal = [-1,0,0]
lLegx = 0.05; lLegy = 0.05
#~ fullBody.addLimb(lKneeId,lLeg,lKnee,lLegOffset,lLegNormal, lLegx, lLegy, 10000, 0.01)
#~
#~ fullBody.client.basic.robot.setJointConfig('LARM_JOINT0',[1])
#~ fullBody.client.basic.robot.setJointConfig('RARM_JOINT0',[-1])
q_0 = fullBody.getCurrentConfig();
q_init = fullBody.getCurrentConfig(); q_init[0:7] = tp.q_init[0:7]
q_goal = fullBody.getCurrentConfig(); q_goal[0:7] = tp.q_goal[0:7]
fullBody.setCurrentConfig (q_init)
q_0 = fullBody.getCurrentConfig();
q_init = fullBody.generateContacts(q_init, [0,0,1]); r (q_init)
fullBody.setCurrentConfig (q_goal)
r(q_goal)
q_goal = fullBody.generateContacts(q_goal, [0,0,1])
r(q_goal)
fullBody.setStartState(q_init,[rLegId,lLegId]) #,rarmId,larmId])
fullBody.setEndState(q_goal,[rLegId,lLegId])#,rarmId,larmId])
#~
#~ configs = fullBody.interpolate(0.1)
configs = fullBody.interpolate(0.1)
i = 0;
r (configs[i]); i=i+1; i-1
#~ q_init = fullBody.generateContacts(q_init, [0,0,-1]); r (q_init)
[[-0.8, 0.05, 0.5, 0.7316888688738209, 0.0, 0.6816387600233341, 0.0, 0.0, 0.0, 0.0, 0.0, -0.9616307102093044, -0.11956018256358465, 0.1552003985475618, -0.9018901200100683, -0.06346342848538598, -1.2180511186061531, 0.0, -2.880178272793078, -0.8455340171728246, 0.6465412052375454, -1.0326167015333017, -0.5643387510658422, -1.3749233150793347, 0.0, 0.4952147840421835, -0.11899403129025972, -2.076378632939565, 1.373290164626756, -0.724533450239729, -0.48047623092675124, 0.35420751827208485, -0.5332345907192056, -1.9185725819607906, 1.637171896673827, -1.041576071944718, -0.2651178185492338], [-0.7295081066851622, 0.05139307726099404, 0.5228785104451054, 0.770654967449951, 0.0013499408178099875, 0.6372511262914482, 0.0003175606773991385, 0.0, 0.0, 0.0, 0.0, -1.5524750047414118, 0.5872082208934596, -1.027419867845647, -1.5776001855618162, 0.4809236595394866, -0.38522609699279364, 0.0, -1.5155488372131927, -1.1331915238126773, 1.307951309416334, -1.7043550994076124, -0.5995506197057995, -0.8750283779826754, 0.0, -0.2885623464906358, -0.1539013092609718, -2.02414922410715, 1.5290051011439285, -0.924899678391509, 0.30745487591267945, -0.2133378772981828, -0.22847129193866927, -1.8955389225917296, 1.428752270765925, -0.9628751173103699, 0.24514587813277203], [-0.7295081066851622, 0.05139307726099404, 0.5228785104451054, 0.770654967449951, 0.0013499408178099875, 0.6372511262914482, 0.0003175606773991385, 0.0, 0.0, 0.0, 0.0, -1.5524750047414118, 0.5872082208934596, -1.027419867845647, -1.5776001855618162, 0.4809236595394866, -0.38522609699279364, 0.0, -1.5155488372131927, -1.1331915238126773, 1.307951309416334, -1.7043550994076124, -0.5995506197057995, -0.8750283779826754, 0.0, -0.2885623464906358, -0.1539013092609718, -2.02414922410715, 1.5290051011439285, -0.924899678391509, 0.30745487591267945, -0.2133378524632916, -0.22847135085173945, -1.89553891204994, 1.4287522707658313, -0.9628751376702371, 0.24514586247045206], [-0.6590162133703243, 0.05278615452198808, 0.5457570208902107, 0.8069310624549565, 0.0026951696104282967, 0.5906391407058407, 0.0006340128958998946, 0.0, 0.0, 0.0, 0.0, -1.2637406246322005, 0.4440944185022869, -1.0271505651521229, -1.2507929392255563, 0.2953978435900077, -0.6894792158756579, 0.0, -1.1371180526643174, -0.8722958667804132, 1.1743876674729996, -1.3142505118760086, -0.38648591762824563, -1.086286128360579, 0.0, -0.2716552631385899, -0.11860524563343498, -2.0063302277123665, 1.4468827473550907, -0.7252816644226199, 0.2888550825732252, -0.15018581261741953, -0.24378644911960404, -1.534424575986502, 1.2186566656624584, -0.985873983187673, 0.20794133620184524], [-0.5885243200554865, 0.054179231782982125, 0.568635531335316, 0.8403905306537559, 0.00403099080018533, 0.5419655043623044, 0.0009482520657262834, 0.0, 0.0, 0.0, 0.0, -0.8346789685285324, -0.1209223283727815, 0.10734523126803834, -0.7013317838685373, -0.19699878718739647, -1.1847139743355233, 0.0, -1.0280526799046852, -0.5200516774662453, 1.2492221135994797, -0.6494272108064829, -0.3251957955340264, -1.437181564366608, 0.0, -0.24402578989024423, -0.14498453803683153, -1.6682909686978409, 1.3006670152948594, -0.8003267807790493, 0.27326385122778474, -0.08964330342126667, -0.24824624947943186, -1.1467390028507707, 0.9157698042401929, -0.9417659534911521, 0.17371541828442655], [-0.5885243200554865, 0.054179231782982125, 0.568635531335316, 0.8403905306537559, 0.00403099080018533, 0.5419655043623044, 0.0009482520657262834, 0.0, 0.0, 0.0, 0.0, -0.8346789685285324, -0.1209223283727815, 0.10734523126803834, -0.7013317838685373, -0.19699878718739647, -1.1847139743355233, 0.0, -1.1577499302366454, 0.13262891662869067, -1.328778129203227, -0.653923410329511, 0.09107315196571622, -1.0485920819857026, 0.0, -0.24402578989024423, -0.14498453803683153, -1.6682909686978409, 1.3006670152948594, -0.8003267807790493, 0.27326385122778474, -0.08964330342126667, -0.24824624947943186, -1.1467390028507707, 0.9157698042401929, -0.9417659534911521, 0.17371541828442655], [-0.5885243200554865, 0.054179231782982125, 0.568635531335316, 0.8403905306537559, 0.00403099080018533, 0.5419655043623044, 0.0009482520657262834, 0.0, 0.0, 0.0, 0.0, -0.8346789685285324, -0.1209223283727815, 0.10734523126803834, -0.7013317838685373, -0.19699878718739647, -1.1847139743355233, 0.0, -1.1577499302366454, 0.13262891662869067, -1.328778129203227, -0.653923410329511, 0.09107315196571622, -1.0485920819857026, 0.0, -0.24402578989024423, -0.14498453803683153, -1.6682909686978409, 1.3006670152948594, -0.8003267807790493, 0.27326385122778474, -0.016485179114389764, 0.18118002488082235, -2.1440179413703127, 1.105173621023176, -0.11165590128907413, -0.0672298877137812], [-0.5885243200554865, 0.054179231782982125, 0.568635531335316, 0.8403905306537559, 0.00403099080018533, 0.5419655043623044, 0.0009482520657262834, 0.0, 0.0, 0.0, 0.0, -0.8346789685285324, -0.1209223283727815, 0.10734523126803834, -0.7013317838685373, -0.19699878718739647, -1.1847139743355233, 0.0, -1.1577499302366454, 0.13262891662869067, -1.328778129203227, -0.653923410329511, 0.09107315196571622, -1.0485920819857026, 0.0, 0.2901066798540861, 0.09330382718018007, -1.9028805576174472, 1.1265243729226184, -0.377307029354628, -0.3101982123942892, -0.016485179114389764, 0.18118002488082235, -2.1440179413703127, 1.105173621023176, -0.11165590128907413, -0.0672298877137812], [-0.5885243200554865, 0.054179231782982125, 0.568635531335316, 0.8403905306537559, 0.00403099080018533, 0.5419655043623044, 0.0009482520657262834, 0.0, 0.0, 0.0, 0.0, -0.8346789685285324, -0.1209223283727815, 0.10734523126803834, -0.7013317838685373, -0.19699878718739647, -1.1847139743355233, 0.0, -1.1577499302366454, 0.13262891662869067, -1.328778129203227, -0.653923410329511, 0.09107315196571622, -1.0485920819857026, 0.0, 0.2901066798540861, 0.09330382718018007, -1.9028805576174472, 1.1265243729226184, -0.377307029354628, -0.3101982123942892, -0.016485179114389764, 0.18118002488082235, -2.1440179413703127, 1.105173621023176, -0.11165590128907413, -0.0672298877137812], [-0.5885243200554865, 0.054179231782982125, 0.568635531335316, 0.8403905306537559, 0.00403099080018533, 0.5419655043623044, 0.0009482520657262834, 0.0, 0.0, 0.0, 0.0, -0.8346789685285324, -0.1209223283727815, 0.10734523126803834, -0.7013317838685373, -0.19699878718739647, -1.1847139743355233, 0.0, -0.832157188258598, -0.4576692322577549, 0.9282066370111607, -0.5434049667116027, -0.470518411693578, -1.568897089466711, 0.0, 0.2901066798540861, 0.09330382718018007, -1.9028805576174472, 1.1265243729226184, -0.377307029354628, -0.3101982123942892, -0.016485179114389764, 0.18118002488082235, -2.1440179413703127, 1.105173621023176, -0.11165590128907413, -0.0672298877137812], [-0.5180324267406485, 0.05557230904397617, 0.5915140417804214, 0.870916580365579, 0.005352741647044882, 0.49140011463529853, 0.0012591813218417796, 0.0, 0.0, 0.0, 0.0, -1.8995241080631167, 0.1224588473334237, -0.6509632327582102, -1.0617885385797055, -0.6801035657557954, -1.0539947615458036, 0.0, -1.8087411752769587, -0.1989029870124375, 0.03645991185139464, -1.1257919216099375, -0.17879035297964546, -1.2329170660027147, 0.0, 0.2649016300493709, 0.11872251232586166, -1.6018690560404434, 1.0386123776640246, -0.474591400270663, -0.2982019454597803, -0.0672781180991026, 0.1601595752650518, -1.8872082499793255, 1.126922962323155, -0.26571673718028244, -0.03599446338981499], [-0.4475405334258107, 0.05696538630497021, 0.6143925522255269, 0.8984026591302385, 0.006655808524094307, 0.43911947214401875, 0.0015657153526028532, 0.0, 0.0, 0.0, 0.0, -1.4386548731050626, -0.12420980068201623, -0.19362487698307929, -1.1149986138770156, -1.21458782015375, -1.3320288686855113, 0.0, -1.711190721052617, -0.504294132744302, 0.18117663459741687, -1.387926828184747, -0.6205508858403247, -1.0145391140807836, 0.0, 0.2383643064530336, 0.14052042738346746, -1.2681258044879546, 0.8681896622544392, -0.520936939540468, -0.28717851947561523, -0.1105738808682885, 0.131879366559214, -1.599408905745296, 1.0805629485813222, -0.38489100370545887, -0.007544522000420243], [-0.3770486401109729, 0.058358463565964254, 0.6372710626706323, 0.9227528256339741, 0.007935643021603277, 0.38530606466956313, 0.0018667841880848992, 0.0, 0.0, 0.0, 0.0, -1.655765121730606, 0.8036554806594781, -1.2318731471931592, -1.3999285535178727, -0.7577243390542939, -0.8586262278450467, 0.0, -1.3057554981612338, -0.527916209799681, 0.13348685633956467, -1.505482691845332, -0.6137592993093114, -1.1778637574774655, 0.0, 0.21080760473000487, 0.15873231601245522, -0.8783626740750136, 0.5621832141995358, -0.4867010404489856, -0.27702061454087945, -0.14597720784105045, 0.09790944746909758, -1.2835412515316762, 0.9603918093485072, -0.46057898941325015, 0.0184415774661541], [-0.306556746796135, 0.0597515408269583, 0.6601495731157377, 0.9438820845966421, 0.009187777823439202, 0.3301477301736588, 0.0021613369348572552, 0.0, 0.0, 0.0, 0.0, -1.0218470393721077, 0.5195443358660536, -0.5527018495571979, -1.3016346291217835, -1.3157570199576007, -1.1242123369817023, 0.0, -0.9799563407381132, -0.5530182483334865, 0.15375559244533138, -1.5063226947418429, -0.6185215882288929, -1.3969967610374103, 0.0, -0.06351410653932016, 0.21290376482867815, -2.150351717688086, 1.386590322408599, 0.08247091824974162, -0.14535554339884327, -0.17334743213463008, 0.0597246166377583, -0.9317746785347067, 0.7427640476249237, -0.47670200987193895, 0.04224536951895135], [-0.306556746796135, 0.0597515408269583, 0.6601495731157377, 0.9438820845966421, 0.009187777823439202, 0.3301477301736588, 0.0021613369348572552, 0.0, 0.0, 0.0, 0.0, -1.0218470393721077, 0.5195443358660536, -0.5527018495571979, -1.3016346291217835, -1.3157570199576007, -1.1242123369817023, 0.0, -0.9799563407381132, -0.5530182483334865, 0.15375559244533138, -1.5063226947418429, -0.6185215882288929, -1.3969967610374103, 0.0, -0.06351410653932016, 0.21290376482867815, -2.150351717688086, 1.386590322408599, 0.08247091824974162, -0.14535554339884327, -0.016712542512932194, 0.15339410849517, -0.8595446415770951, 0.7092443022204818, -0.5294352983020696, -0.12806502073050652], [-0.306556746796135, 0.0597515408269583, 0.6601495731157377, 0.9438820845966421, 0.009187777823439202, 0.3301477301736588, 0.0021613369348572552, 0.0, 0.0, 0.0, 0.0, -1.0218470393721077, 0.5195443358660536, -0.5527018495571979, -1.3016346291217835, -1.3157570199576007, -1.1242123369817023, 0.0, -1.0219345119654972, 0.042480553067106894, -0.31405024831014205, -1.5771308211694333, -0.4731533266333664, -1.1890817540868248, 0.0, -0.06351410653932016, 0.21290376482867815, -2.150351717688086, 1.386590322408599, 0.08247091824974162, -0.14535554339884327, -0.016712542512932194, 0.15339410849517, -0.8595446415770951, 0.7092443022204818, -0.5294352983020696, -0.12806502073050652], [-0.306556746796135, 0.0597515408269583, 0.6601495731157377, 0.9438820845966421, 0.009187777823439202, 0.3301477301736588, 0.0021613369348572552, 0.0, 0.0, 0.0, 0.0, -1.3089054617401412, 0.5647020899220243, -0.9924549011870739, -1.5173706703668328, -0.8057275740772372, -0.7047771346664801, 0.0, -1.0219345119654972, 0.042480553067106894, -0.31405024831014205, -1.5771308211694333, -0.4731533266333664, -1.1890817540868248, 0.0, -0.06351410653932016, 0.21290376482867815, -2.150351717688086, 1.386590322408599, 0.08247091824974162, -0.14535554339884327, -0.016712542512932194, 0.15339410849517, -0.8595446415770951, 0.7092443022204818, -0.5294352983020696, -0.12806502073050652], [-0.23606485348129685, 0.061144618087952345, 0.6830280835608433, 0.9617166834513147, 0.010407842300422058, 0.27383700114235704, 0.0024483454441709108, 0.0, 0.0, 0.0, 0.0, -0.7595149032067405, 0.17031112472733265, -0.36899511020133896, -1.3955985087870098, -1.422317814404017, -1.126930171272042, 0.0, -0.7280965076615536, -0.0950001844419838, -0.1747069115622878, -1.496358397382098, -0.6522192230569642, -1.4472207547105183, 0.0, -0.11763030377500411, 0.15976436955741802, -1.953781090982263, 1.5357584883794009, -0.13815930281052333, -0.095249523646763, -0.3532025831307208, -0.24587666012070772, -1.8308145997417962, 0.9912790160098935, 0.27084283554478317, 0.3727325746529942], [-0.23606485348129685, 0.061144618087952345, 0.6830280835608433, 0.9617166834513147, 0.010407842300422058, 0.27383700114235704, 0.0024483454441709108, 0.0, 0.0, 0.0, 0.0, -0.7595149032067405, 0.17031112472733265, -0.36899511020133896, -1.3955985087870098, -1.422317814404017, -1.126930171272042, 0.0, -0.7280965076615536, -0.0950001844419838, -0.1747069115622878, -1.496358397382098, -0.6522192230569642, -1.4472207547105183, 0.0, -0.5162168009847073, -0.046586448138763656, -1.9188604355093848, 1.365325658194254, 0.04350767246613533, 0.28450086929710167, -0.3532025831307208, -0.24587666012070772, -1.8308145997417962, 0.9912790160098935, 0.27084283554478317, 0.3727325746529942], [-0.23606485348129685, 0.061144618087952345, 0.6830280835608433, 0.9617166834513147, 0.010407842300422058, 0.27383700114235704, 0.0024483454441709108, 0.0, 0.0, 0.0, 0.0, -0.7595149032067405, 0.17031112472733265, -0.36899511020133896, -1.3955985087870098, -1.422317814404017, -1.126930171272042, 0.0, -0.718863130804274, -0.03666148330152049, -0.14029815173030014, -1.4954454109302493, -1.2289339856044215, -1.3182546245732822, 0.0, -0.5162168009847073, -0.046586448138763656, -1.9188604355093848, 1.365325658194254, 0.04350767246613533, 0.28450086929710167, -0.3532025831307208, -0.24587666012070772, -1.8308145997417962, 0.9912790160098935, 0.27084283554478317, 0.3727325746529942], [-0.23606485348129685, 0.061144618087952345, 0.6830280835608433, 0.9617166834513147, 0.010407842300422058, 0.27383700114235704, 0.0024483454441709108, 0.0, 0.0, 0.0, 0.0, -1.9900729423225652, 0.18818642468047633, -1.4248820153916097, -1.6004606689594543, -0.11242581868070751, -0.13861344709342704, 0.0, -0.718863130804274, -0.03666148330152049, -0.14029815173030014, -1.4954454109302493, -1.2289339856044215, -1.3182546245732822, 0.0, -0.5162168009847073, -0.046586448138763656, -1.9188604355093848, 1.365325658194254, 0.04350767246613533, 0.28450086929710167, -0.3532025831307208, -0.24587666012070772, -1.8308145997417962, 0.9912790160098935, 0.27084283554478317, 0.3727325746529942], [-0.23606485348129685, 0.061144618087952345, 0.6830280835608433, 0.9617166834513147, 0.010407842300422058, 0.27383700114235704, 0.0024483454441709108, 0.0, 0.0, 0.0, 0.0, -1.9900729423225652, 0.18818642468047633, -1.4248820153916097, -1.6004606689594543, -0.11242581868070751, -0.13861344709342704, 0.0, -0.718863130804274, -0.03666148330152049, -0.14029815173030014, -1.4954454109302493, -1.2289339856044215, -1.3182546245732822, 0.0, -0.5162168009847073, -0.046586448138763656, -1.9188604355093848, 1.365325658194254, 0.04350767246613533, 0.28450086929710167, -0.3532025831307208, -0.24587666012070772, -1.8308145997417962, 0.9912790160098935, 0.27084283554478317, 0.3727325746529942], [-0.23606485348129685, 0.061144618087952345, 0.6830280835608433, 0.9617166834513147, 0.010407842300422058, 0.27383700114235704, 0.0024483454441709108, 0.0, 0.0, 0.0, 0.0, -1.9900729423225652, 0.18818642468047633, -1.4248820153916097, -1.6004606689594543, -0.11242581868070751, -0.13861344709342704, 0.0, -0.718863130804274, -0.03666148330152049, -0.14029815173030014, -1.4954454109302493, -1.2289339856044215, -1.3182546245732822, 0.0, -0.2237912761632093, -0.002985341336740911, -1.9719556941404806, 1.3178683991494795, 0.10630490881818302, 0.09873977811190479, -0.3532025831307208, -0.24587666012070772, -1.8308145997417962, 0.9912790160098935, 0.27084283554478317, 0.3727325746529942], [-0.16557296016645895, 0.06253769534894639, 0.7059065940059488, 0.9761943697807176, 0.011591577766189326, 0.21657043254329625, 0.002726807900754951, 0.0, 0.0, 0.0, 0.0, -1.6293503837249654, 0.18298401282182603, -1.392257176099705, -1.644799218251426, -0.3483362833530161, -0.1294563515046155, 0.0, -1.2315890325922119, -0.8628036439831522, 1.5957287859245317, -1.3517967778110496, 1.0264698328289215, -0.5733333128417168, 0.0, -0.23831401299294297, -0.061325583958369276, -1.7781266733906873, 1.4534879823563562, -0.10894320295586489, 0.13241789742588275, -0.3299047392403371, -0.2958536621432998, -1.661001165729629, 1.1507738096730187, 0.05512680354921336, 0.38293452696694125], [-0.09508106685162104, 0.06393077260994043, 0.728785104451054, 0.9872646086119082, 0.012734852342319823, 0.1585479157423193, 0.0029957523196948903, 0.0, 0.0, 0.0, 0.0, -1.2780169735140658, 0.1656023314627183, -1.341471711019729, -1.6286902284863187, -0.5749912787913568, -0.18292918055139487, 0.0, -0.33947782836768226, -0.28819613138859174, -0.023449016955686262, -1.5383613490815267, -0.02153406480275572, -1.574989693136068, 0.0, -0.2412143612577695, -0.11838595491460409, -1.5384039522011277, 1.5259518029047183, -0.30723664282802776, 0.16191262901052642, -0.2988694386042204, -0.3421807429016502, -1.439839301083552, 1.2355045075487394, -0.13408783114352232, 0.39189611294760457], [-0.022076121021559602, 0.06094842561092166, 0.7520371042641201, 0.9944272378898142, 0.00997955785804397, 0.10492552483828413, 0.0023475956217737445, 0.0, 0.0, 0.0, 0.0, -0.9539883145301508, 0.15059887897688515, -1.2805713370912533, -1.6139344980064718, -0.7846120279113545, -0.24971493204561437, 0.0, -0.4264049901222203, -0.732960935103925, -0.29557326621463037, -1.9204529919655577, -0.6422161379858715, -0.8950332527378759, 0.0, -0.23025964840994495, -0.15452324877137164, -1.264325906052013, 1.5439021872738488, -0.49232497922294666, 0.17891607700409057, -0.2587771730661798, -0.36585758749079694, -1.1843971688852113, 1.26531767514972, -0.3066858699340906, 0.39125988985622007], [0.0522909487332522, 0.05559451254657312, 0.7754915426655788, 0.9985439039503329, 0.00510644676068063, 0.053689413303143634, 0.0012012428034106411, 0.0, 0.0, 0.0, 0.0, -1.0475696829626735, 0.42867778070670703, -1.4266737161990224, -1.9329768179320583, -0.5690357628818655, -0.15024362039871061, 0.0, -0.10019994510944001, -0.6965827236558311, -0.35950605713185074, -1.9705204805908925, -0.6392395949743666, -1.042074891944533, 0.0, -0.2124646571578319, -0.17797933663111182, -0.9603342383348086, 1.4990232084137902, -0.6478374545317998, 0.18944873654292735, -0.21578842096783865, -0.37561113983077865, -0.8967171788145887, 1.2306255047574024, -0.449656361043847, 0.38626938781671905], [0.12665801848806413, 0.05024059948222456, 0.7989459810670376, 0.9999973062324, 0.00021971601710993194, 0.0023101041888371307, 5.16860934261529e-05, 0.0, 0.0, 0.0, 0.0, -0.7847141619871257, 0.4351470735948945, -1.4124789287545003, -1.9425271193014, -0.7227364213059206, -0.16458351576519872, 0.0, 0.18161322306854452, -0.6623493830449768, -0.38745748111385514, -1.9927500398364735, -0.6325539847994472, -1.1886242390999329, 0.0, 0.5784717082109263, 0.2784483752326761, -0.7444144403355267, 1.2209151080776357, -0.4802878636521755, -0.28133901277924617, -0.1722885072002241, -0.3813495846856474, -0.5788036040289395, 1.1211722615569775, -0.5474070444777651, 0.3817659594345889], [0.13, 0.05, 0.8, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -0.24149111208387858, 0.24703437474614728, -0.86131916964458, -1.9395959468147612, -0.985558803559561, -0.5849836044151596, 0.0, -0.13334199981803405, -0.14162921652172455, -0.8345246830091457, -1.5965348589713615, -1.4892834076096082, -0.4470753815799931, 0.0, 0.7852183979307628, -0.13343186876423385, -1.0075074171842457, 0.9195067050906879, 0.08798442638841043, 0.13342213164610411, 5.636679232201488e-06, 0.030602982722709008, -0.14136180137089008, 1.1427318587147743, -1.001380774386747, -0.030566875241395154]]
\ No newline at end of file
[[-0.8, 0.05, 0.5, 0.7316888688738209, 0.0, 0.6816387600233341, 0.0, 0.0, 0.0, 0.0, 0.0, -1.1163647090994449, 0.5493023745972837, -0.904420293908483, -0.6498650164930848, -0.5302334619619172, -1.5807880201890472, 0.0, -2.2777760908681195, -0.6858148698155213, 0.8946771000621203, -0.8510205491879024, -1.0535030751142127, -0.3025428090025787, 0.0, 0.4667904030670675, 0.03191373247877354, -2.066015371200067, 1.3732916707055027, -0.8149054940688741, -0.46780242680696055, -0.02042503996889731, -0.610865, -1.7659744990847654, 1.4499071615906545, -1.2083655732660663, 0.05729793801468941], [-0.7295974426563886, 0.05213708418546642, 0.5237080680304489, 0.7703555380746054, 0.004182853429609858, 0.637600722503239, -0.00040909451110191555, 0.0, 0.0, 0.0, 0.0, -3.073513444023129, 0.6120233737834382, -0.2748851990318788, -0.8571424864273405, -1.1361353950120674, -0.7494859396614069, 0.0, -1.4754233550209788, -0.5142080233152907, 1.1114331440327245, -1.6560239380892317, -0.31944727048222465, -0.3234805081936857, 0.0, 0.2934311686058271, 0.5533452169592136, -1.9430213380162205, 1.3040471311959263, -0.9247167863635447, -0.3519506246968745, -0.3187150543888991, 0.040543367702623706, -1.928585447319408, 1.428256454404051, -0.8607341013304802, 0.2988075499083354], [-0.7295974426563886, 0.05213708418546642, 0.5237080680304489, 0.7703555380746054, 0.004182853429609858, 0.637600722503239, -0.00040909451110191555, 0.0, 0.0, 0.0, 0.0, -3.073513444023129, 0.6120233737834382, -0.2748851990318788, -0.8571424864273405, -1.1361353950120674, -0.7494859396614069, 0.0, -1.4754233550209788, -0.5142080233152907, 1.1114331440327245, -1.6560239380892317, -0.31944727048222465, -0.3234805081936857, 0.0, 0.2934311686058271, 0.5533452169592136, -1.9430213380162205, 1.3040471311959263, -0.9247167863635447, -0.3519506246968745, -0.3187501700175984, 0.040523212667214356, -1.928560173383701, 1.428272458255596, -0.8607427417171523, 0.29883365349099095], [-0.6591948853127771, 0.05427416837093284, 0.5474161360608978, 0.8063628446828204, 0.008351267131858622, 0.5913616084326223, -0.0008167767773559921, 0.0, 0.0, 0.0, 0.0, -0.7511972449520329, -0.0891582721088528, 0.2788579420317372, -1.3367208765550485, 0.09944317244322734, -0.7461797120856111, 0.0, -1.179207564083635, -0.3793643864516901, 1.08570982929358, -1.3001556298983863, -0.11222668419471057, -0.6335406162329605, 0.0, 0.43609446783114647, -0.3123367067621129, -1.9834122653689141, 1.224389161939863, -0.3635899913737496, -0.3065940244857892, -0.3308457645425284, -0.004616433191050585, -1.579889640392654, 1.2134876412748057, -0.8855779944423595, 0.3040294861632739], [-0.5887923279691656, 0.056411252556399255, 0.5711242040913467, 0.8395864870227842, 0.012490851227109908, 0.5430810409519629, -0.0012216394291582678, 0.0, 0.0, 0.0, 0.0, -0.8178736962976092, 0.04809254295071857, 0.09705291716416692, -0.6445296305030894, 0.2975090581393556, -1.2363561648196408, 0.0, -2.1699838608717963, -0.011967012384928344, -0.1598746681493262, -0.7476334562715682, 0.8905333617361985, -1.3412270066578689, 0.0, 0.49675928450157986, -0.24684877388199286, -1.6582433984990603, 1.0957983985597466, -0.43849854002249755, -0.34484380049758695, -0.3373212189792196, -0.05113026173174915, -1.2058022785530749, 0.9044446867823962, -0.8421588385454878, 0.30873688778559566], [-0.5183897706255541, 0.05854833674186568, 0.5948322721217956, 0.8699117729721444, 0.01658731535909624, 0.492925690551392, -0.0016222848305625884, 0.0, 0.0, 0.0, 0.0, -2.442729135269389, 0.8667282656085277, -1.389358814163808, -1.1608766630194167, -0.07886470788114358, -1.1629501150410082, 0.0, -1.7767234497481201, 0.08565071288999389, -0.3299539458619733, -0.9558800437298061, 1.0356813337284096, -1.4886892745238614, 0.0, 0.5420178575362558, -0.17468417852944218, -1.3110990510345848, 0.8798213321615801, -0.4563722720289507, -0.3784156701505828, -0.33812860187321425, -0.09831012634171754, -0.7158252400654556, 0.32636732793417683, -0.6456600550288089, 0.313025344491317], [-0.5183897706255541, 0.05854833674186568, 0.5948322721217956, 0.8699117729721444, 0.01658731535909624, 0.492925690551392, -0.0016222848305625884, 0.0, 0.0, 0.0, 0.0, -2.442729135269389, 0.8667282656085277, -1.389358814163808, -1.1608766630194167, -0.07886470788114358, -1.1629501150410082, 0.0, -1.7767234497481201, 0.08565071288999389, -0.3299539458619733, -0.9558800437298061, 1.0356813337284096, -1.4886892745238614, 0.0, 0.5420178575362558, -0.17468417852944218, -1.3110990510345848, 0.8798213321615801, -0.4563722720289507, -0.3784156701505828, -0.24902041307228362, 0.13060685568271357, -1.8253191910597106, 1.063278457709883, -0.2420476908215692, 0.11677818776486944], [-0.4479872132819427, 0.0606854209273321, 0.6185403401522447, 0.8972340158932683, 0.020626518026996277, 0.4410686997035477, -0.002017329904092751, 0.0, 0.0, 0.0, 0.0, -1.7570195227477632, 0.8720531003918952, -0.8391657986501926, -1.3338052751504141, -0.41348291556305494, -1.1546151550268309, 0.0, -2.0013585991597127, -0.950185678734252, 0.9479851741492399, -1.1317517097799707, 0.23633526969297705, -1.2347640823520847, 0.0, 0.5732705736733553, -0.09831923398790433, -0.9018355772709584, 0.4966122307784555, -0.37543866091525174, -0.40805600406427195, -0.2926501474698329, 0.07333587633910542, -1.5348212450859278, 0.9852237697376557, -0.3399880462826347, 0.14997553226700988], [-0.37758465593833135, 0.06282250511279852, 0.6422484081826937, 0.9214588960244638, 0.02459451540364462, 0.38768908515434497, -0.0024054109052970038, 0.0, 0.0, 0.0, 0.0, -1.2031668019000943, 0.6551275293929948, -0.35751505944401046, -1.349028469893585, -0.7897356320193991, -1.3053943276767417, 0.0, -1.4100910523641708, -0.8452282610817778, 0.43408488488667274, -1.2271748198123265, 0.5435783215666592, -1.308550793354974, 0.0, -0.009549435968557098, 0.19481750672105372, -1.3566302640642987, 0.6759684918471549, -0.13137945024813827, -0.17194861336180883, -0.32470442450725506, 0.011106113888036426, -1.213929072986893, 0.8228084152949083, -0.38776705194096095, 0.1800054961340421], [-0.30718209859472045, 0.06495958929826492, 0.6659564762131425, 0.9425027860834612, 0.028477609471347677, 0.33297111993503026, -0.002785188130562556, 0.0, 0.0, 0.0, 0.0, -0.9112068212007491, -0.1410588409715958, 0.08930972563276479, -1.5403322685031307, 0.036920288878362205, -1.3770668752090556, 0.0, -1.0369498964247745, -0.5825614193795363, -9.947519355394969e-05, -1.2032541427945385, 0.9234703014022468, -1.5205541191011447, 0.0, -0.45693202899151764, 0.48286113628877414, -2.0611140659828924, 1.241279084584017, 0.18436753225227323, -0.15712262937134003, -0.3455808314097426, -0.054242499447074974, -0.8359491515480728, 0.5183966755764028, -0.35319441150557507, 0.20726376132178517], [-0.30718209859472045, 0.06495958929826492, 0.6659564762131425, 0.9425027860834612, 0.028477609471347677, 0.33297111993503026, -0.002785188130562556, 0.0, 0.0, 0.0, 0.0, -0.9112068212007491, -0.1410588409715958, 0.08930972563276479, -1.5403322685031307, 0.036920288878362205, -1.3770668752090556, 0.0, -1.4358815075980789, -0.30343037722085126, -0.06720246806525838, -1.7198440057158741, -0.3449847570962414, -0.650907883521344, 0.0, -0.45693202899151764, 0.48286113628877414, -2.0611140659828924, 1.241279084584017, 0.18436753225227323, -0.15712262937134003, -0.3455808314097426, -0.054242499447074974, -0.8359491515480728, 0.5183966755764028, -0.35319441150557507, 0.20726376132178517], [-0.30718209859472045, 0.06495958929826492, 0.6659564762131425, 0.9425027860834612, 0.028477609471347677, 0.33297111993503026, -0.002785188130562556, 0.0, 0.0, 0.0, 0.0, -0.9112068212007491, -0.1410588409715958, 0.08930972563276479, -1.5403322685031307, 0.036920288878362205, -1.3770668752090556, 0.0, -1.4358815075980789, -0.30343037722085126, -0.06720246806525838, -1.7198440057158741, -0.3449847570962414, -0.650907883521344, 0.0, -0.45693202899151764, 0.48286113628877414, -2.0611140659828924, 1.241279084584017, 0.18436753225227323, -0.15712262937134003, -0.1002601689523714, 0.07909601136648703, -1.1186127251633964, 0.7549751921309786, -0.31866681686548987, -0.05011775480617375], [-0.30718209859472045, 0.06495958929826492, 0.6659564762131425, 0.9425027860834612, 0.028477609471347677, 0.33297111993503026, -0.002785188130562556, 0.0, 0.0, 0.0, 0.0, -0.9112068212007491, -0.1410588409715958, 0.08930972563276479, -1.5403322685031307, 0.036920288878362205, -1.3770668752090556, 0.0, -1.4358815075980789, -0.30343037722085126, -0.06720246806525838, -1.7198440057158741, -0.3449847570962414, -0.650907883521344, 0.0, -0.45693202899151764, 0.48286113628877414, -2.0611140659828924, 1.241279084584017, 0.18436753225227323, -0.15712262937134003, -0.1002601689523714, 0.07909601136648703, -1.1186127251633964, 0.7549751921309786, -0.31866681686548987, -0.05011775480617375], [-0.30718209859472045, 0.06495958929826492, 0.6659564762131425, 0.9425027860834612, 0.028477609471347677, 0.33297111993503026, -0.002785188130562556, 0.0, 0.0, 0.0, 0.0, -0.9112068212007491, -0.1410588409715958, 0.08930972563276479, -1.5403322685031307, 0.036920288878362205, -1.3770668752090556, 0.0, -1.4358815075980789, -0.30343037722085126, -0.06720246806525838, -1.7198440057158741, -0.3449847570962414, -0.650907883521344, 0.0, -0.45693202899151764, 0.48286113628877414, -2.0611140659828924, 1.241279084584017, 0.18436753225227323, -0.15712262937134003, -0.1002601689523714, 0.07909601136648703, -1.1186127251633964, 0.7549751921309786, -0.31866681686548987, -0.05011775480617375], [-0.30718209859472045, 0.06495958929826492, 0.6659564762131425, 0.9425027860834612, 0.028477609471347677, 0.33297111993503026, -0.002785188130562556, 0.0, 0.0, 0.0, 0.0, -0.9112068212007491, -0.1410588409715958, 0.08930972563276479, -1.5403322685031307, 0.036920288878362205, -1.3770668752090556, 0.0, -1.4358815075980789, -0.30343037722085126, -0.06720246806525838, -1.7198440057158741, -0.3449847570962414, -0.650907883521344, 0.0, -0.45693202899151764, 0.48286113628877414, -2.0611140659828924, 1.241279084584017, 0.18436753225227323, -0.15712262937134003, -0.1002601689523714, 0.07909601136648703, -1.1186127251633964, 0.7549751921309786, -0.31866681686548987, -0.05011775480617375], [-0.23677954125110903, 0.06709667348373134, 0.6896645442435914, 0.960293039959135, 0.032262395309135015, 0.2771036972288845, -0.003155350541938152, 0.0, 0.0, 0.0, 0.0, -0.7570930790792472, 0.2497949070425831, -0.2991032567555727, -1.6665905974982003, 0.24784823012968035, -1.3740909990228205, 0.0, -1.056885486136329, -0.323388583085768, -0.09525488987853191, -1.8237640989119466, -0.36781293787206376, -0.8052961858231836, 0.0, -0.37811741889898715, -0.1601432761390806, -1.9333008296941327, 1.1996434362022557, 0.1661603696389341, 0.27695745632606156, -0.126360907341076, 0.03962231679019061, -0.7518848099701321, 0.4982271871090915, -0.31189452657358624, -0.026060018772654625], [-0.1663769839074976, 0.06923375766919777, 0.7133726122740404, 0.9747682434948646, 0.035935807368204224, 0.22027967828859585, -0.0035146202929992026, 0.0, 0.0, 0.0, 0.0, -0.5414532873518318, 0.2298595639571765, -0.32722285297295484, -1.5360423504032281, 0.20561407683960087, -1.5977858208247988, 0.0, -0.7232854086815342, -0.34546922564200705, -0.10119385711800344, -1.8494030602419964, -0.38391083997034736, -0.9957045887189765, 0.0, -0.37818843052964485, -0.23821186073560455, -1.7457612617989835, 1.3230612780844022, -0.039439728537068765, 0.3096977821553648, -0.6244332310329213, -0.06200400738157701, -1.677798764623983, 1.3069368370958494, -0.042327546554283876, 0.2537580454199773], [-0.0947628962339503, 0.06798468863319135, 0.7358908802023121, 0.9857730259970441, 0.033437576031491695, 0.1646899357957848, -0.0032702864322721676, 0.0, 0.0, 0.0, 0.0, -0.6168343536027421, -0.07607028474654721, -0.5017407954293123, -1.8767877104441149, -0.753705549439799, -0.7828357173965506, 0.0, -0.4276177268127, -0.3681293465876509, -0.10161803130372232, -1.8290566136542676, -0.40644466992913714, -1.1969917400154126, 0.0, -0.3616493266343177, -0.29525438870705345, -1.517823665611836, 1.3966805129440054, -0.2319678947212897, 0.33260882350950655, -0.6419465697626956, -0.19214530868461302, -1.4406225081315125, 1.3140210455197119, -0.19459075211959348, 0.32589840122238367], [-0.021499344347688472, 0.062122412470560856, 0.7567878427498655, 0.9935278547074914, 0.022596906965421076, 0.11129643948513512, -0.002210039333913891, 0.0, 0.0, 0.0, 0.0, -1.8640515273447136, 0.9549607702609791, -1.4218983276169335, -2.166097067628427, 0.21715236184427109, -0.48542543378581254, 0.0, -0.9000167178293005, -0.40142606559916677, -0.6309606243791376, -1.7714964125861485, -0.7561630223947831, -0.3836600773161627, 0.0, -0.33305350660051203, -0.32434572848223414, -1.256111930930374, 1.4284142182765545, -0.4123398969686531, 0.3461219076988469, -0.6348674217410063, -0.2927230715662376, -1.1667160079437777, 1.27225841750463, -0.32772757574878825, 0.38171896401769234], [-0.021499344347688472, 0.062122412470560856, 0.7567878427498655, 0.9935278547074914, 0.022596906965421076, 0.11129643948513512, -0.002210039333913891, 0.0, 0.0, 0.0, 0.0, -1.8640515273447136, 0.9549607702609791, -1.4218983276169335, -2.166097067628427, 0.21715236184427109, -0.48542543378581254, 0.0, -0.9000167178293005, -0.40142606559916677, -0.6309606243791376, -1.7714964125861485, -0.7561630223947831, -0.3836600773161627, 0.0, 0.47115765233709456, 0.05145232151405976, -1.1994131482508832, 0.6165302013205525, 0.4016098798508789, -0.19106902553468566, -0.6348674217410063, -0.2927230715662376, -1.1667160079437777, 1.27225841750463, -0.32772757574878825, 0.38171896401769234], [0.051764207538579236, 0.0562601363079299, 0.7776848052974206, 0.9982726446874941, 0.011687777246905975, 0.05756575424532976, -0.0011430965964152999, 0.0, 0.0, 0.0, 0.0, -0.5028045651161872, 0.07110361531693896, -0.4490162510289742, -2.190049917312687, -0.4352254488652096, -0.49229864638075355, 0.0, -0.5232722257593475, -0.4497909965162876, -0.6811704441830794, -1.8176271909971344, -0.9302580650484942, -0.5402241366487556, 0.0, 0.46828875725214086, 0.1890154322813124, -1.0643334352114104, 0.7605724146778021, 0.20825622358922283, -0.2608409702558627, -0.6102075135881752, -0.3852010231250946, -0.8601758116494324, 1.158460840100921, -0.4168460729506719, 0.42967653412634466], [0.12502775942484481, 0.0503978601452991, 0.7985817678449749, 0.9999930208981037, 0.0007432376885771302, 0.0036606650882077857, -7.269067968975871e-05, 0.0, 0.0, 0.0, 0.0, -0.24265605088371406, 0.0439347976419759, -0.4753072284118933, -2.220918417562428, -0.5386233506854109, -0.5884090137571935, 0.0, -0.13144666626668364, -0.5288918699923585, -0.684532753467345, -1.830234426207876, -1.1210715419308126, -0.7643011932388951, 0.0, 0.4467990609188953, 0.31796280254388465, -0.8675740066056226, 0.8003697712915557, 0.060894055004102804, -0.3224575011653343, -0.3515355857988573, -0.5287221287525438, -0.5405389714716688, 0.7507150156896749, -0.21871809066067036, 0.5298124864060489], [0.13, 0.05, 0.8, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.13136051588667466, 0.0361435808622901, -0.18102508524504507, -2.090809657757992, -0.49247716690662824, -1.1379178910311205, 0.0, 0.3483849729803635, -0.8871121064173628, -0.2830759903161151, -1.8846679315794725, -0.038024417025027235, -1.4662562654809195, 0.0, 0.54961340899285, 0.10441022353536134, -0.9847989104117943, 1.0114678249166136, -0.02669117802226524, -0.1044013135020783, 8.432019765520705e-06, 0.030612840833134928, -0.14140017481683392, 1.1427289161380547, -1.0013019097690736, -0.030612702597525562]]
\ No newline at end of file
This diff is collapsed.
This diff is collapsed.
from hpp.corbaserver.rbprm.rbprmbuilder import Builder
from hpp.gepetto import Viewer
rootJointType = 'freeflyer'
packageName = 'hpp-rbprm-corba'
meshPackageName = 'hpp-rbprm-corba'
urdfName = 'hrp2_trunk'
urdfNameRom = 'hrp2_rom'
urdfSuffix = ""
srdfSuffix = ""
rbprmBuilder = Builder ()
rbprmBuilder.loadModel(urdfName, urdfNameRom, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
rbprmBuilder.setJointBounds ("base_joint_xyz", [0,2, -1, 1, 0, 2.2])
#~ from hpp.corbaserver.rbprm. import ProblemSolver
from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
ps = ProblemSolver( rbprmBuilder )
r = Viewer (ps)
q_init = rbprmBuilder.getCurrentConfig (); r (q_init)
q_goal = q_init [::]
q_init = rbprmBuilder.getCurrentConfig ();
q_init [0:3] = [0, -0.6, 0.6]; rbprmBuilder.setCurrentConfig (q_init); r (q_init)
q_goal [0:3] = [1.49, -0.6, 1.25]; r (q_goal)
#~ ps.addPathOptimizer("GradientBased")
ps.addPathOptimizer("RandomShortcut")
ps.setInitialConfig (q_init)
ps.addGoalConfig (q_goal)
ps.client.problem.selectConFigurationShooter("RbprmShooter")
ps.client.problem.selectPathValidation("RbprmPathValidation",0.05)
r.loadObstacleModel (packageName, "stair_bauzil", "planning")
ps.solve ()
from hpp.gepetto import PathPlayer
pp = PathPlayer (rbprmBuilder.client.basic, r)
#~ pp.fromFile("/home/stonneau/dev/hpp/src/hpp-rbprm-corba/script/paths/stair.path")
#~
#~ pp (2)
#~ pp (0)
pp (1)
#~ pp.toFile(1, "/home/stonneau/dev/hpp/src/hpp-rbprm-corba/script/paths/stair.path")
r (q_init)
......@@ -22,37 +22,40 @@ ps = ProblemSolver( rbprmBuilder )
r = Viewer (ps)
q_init = rbprmBuilder.getCurrentConfig (); r (q_init)
q_goal = q_init [::]
q_goal [0:3] = [0.19, 0.05, 0.9]; r (q_goal)
#~ fullBody.client.basic.robot.setJointConfig('base_joint_SO3',[0.7316888688738209, 0, -0.6816387600233341, 0]); q_init = rbprmBuilder.getCurrentConfig (); r (q_init)
rbprmBuilder.client.basic.robot.setJointConfig('base_joint_SO3',[0.7316888688738209, 0, 0.6816387600233341, 0]); q_init = rbprmBuilder.getCurrentConfig (); r (q_init)
q_init = rbprmBuilder.getCurrentConfig ();
q_init [0:3] = [-0.3, 0, 0.7]; rbprmBuilder.setCurrentConfig (q_init); r (q_init)
q_init [0:3] = [-0.8, 0.05, 0.5]; rbprmBuilder.setCurrentConfig (q_init); r (q_init)
#~ q_0 [0:3] = [-0.2, 0, 0.3]; r (q_0)
q_goal = q_init [::]
q_goal [0:3] = [0.1, 0, 1.7]; r (q_goal)
q_goal [0:3] = [0.13, 0.05, 0.8]; r (q_goal)
#~ q_init [0:6] = [0.0, -2.2, 2.0, 0.7316888688738209, 0.0, 0.6816387600233341];
#~ rbprmBuilder.setCurrentConfig (q_init); r (q_init)
#~ ps.addPathOptimizer("GradientBased")
ps.addPathOptimizer("RandomShortcut")
ps.addPathOptimizer("GradientBased")
ps.setInitialConfig (q_init)
ps.addGoalConfig (q_goal)
ps.client.problem.selectConFigurationShooter("RbprmShooter")
#~ ps.client.problem.selectPathValidation("RbprmPathValidation",0.05)
ps.client.problem.selectPathValidation("RbprmPathValidation",0.05)
r.loadObstacleModel (packageName, "scene_wall", "planning")
ps.solve ()
from hpp.gepetto import PathPlayer
pp = PathPlayer (rbprmBuilder.client.basic, r)
#~ pp.fromFile("/home/stonneau/dev/hpp/src/hpp-rbprm-corba/script/paths/stair.path")
#~
pp (2)
#~ pp (1)
#~ pp.fromFile("/home/stonneau/dev/hpp/src/hpp-rbprm-corba/script/paths/truck.path")
#~ pp (1)
#~ pp.toFile(1, "/home/stonneau/dev/hpp/src/hpp-rbprm-corba/script/paths/truck.path")
#~ pp (2)
#~ pp (0)
pp (1)
#~ pp.toFile(1, "/home/stonneau/dev/hpp/src/hpp-rbprm-corba/script/paths/stair.path")
r (q_init)
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment