Commit 60e95c5c authored by Steve T's avatar Steve T
Browse files

new scenarios

parent a1cf0f94
......@@ -585,7 +585,7 @@ def plotQPRes(pb, res, linewidth=2, ax = None, plot_constraints = False, show =
for el in weighted:
bFreez = bf.reshape((-1,)) - Af[:,:2].dot(el[:2])
# ~ print ('AAAAAAAAAAAAAAAAAAA',bf.shape)
from qp import solve_lp
from sl1m.qp import solve_lp
z = solve_lp(zeros(1),Afreez, bFreez)
el[-1] = z.x
plotPoints(ax, [el], color = "b")
......@@ -794,9 +794,36 @@ def targetCom(pb, cVars, endCom):
nVarEnd = numVariablesForPhase(pb["phaseData"][-1])
cx_end_diff = cVars[-nVarEnd] - endCom[0]
cy_end_diff = cVars[-nVarEnd+1] - endCom[1]
cz_end_diff = cVars[-nVarEnd+2] - endCom[2]
# ~ cz_end_diff = cVars[-nVarEnd+2] - endCom[2]
#
return cx_end_diff * cx_end_diff + cy_end_diff * cy_end_diff + cz_end_diff * cz_end_diff
# ~ return cx_end_diff * cx_end_diff + cy_end_diff * cy_end_diff + cz_end_diff * cz_end_diff
return cx_end_diff * cx_end_diff + cy_end_diff * cy_end_diff
# gurobi cost functions
def targetLegCenter(pb, cVars, endCom):
startCol = 0;
previousStartCol = 0;
endCol = 0;
#~ fixedFootMatrix = None;
footOffset = 4
cost = 0
fact = 1. / float(N_EFFECTORS)
for phaseId, phaseDataT in enumerate(pb["phaseData"]):
#inequality
endCol = startCol + numVariablesForPhase(phaseDataT)
footCol = startCol + footOffset
# expressing everything in terms of first foot
x= 0; y = 0;
for footId in range(1, N_EFFECTORS):
x += cVars[footCol+footId * 3 ] * fact
y += cVars[footCol+footId * 3 + 1] * fact
startCol = endCol
if phaseId == len(pb["phaseData"]) - 1:
cost = (x - endCom[0]) * (x - endCom[0]) + (y - endCom[1]) * (y - endCom[1])
print ("cost ", cost)
return cost
def posturalCost(pb, cVars, initPos, initCom):
startCol = 0;
......@@ -817,6 +844,44 @@ def posturalCost(pb, cVars, initPos, initCom):
cost += sum([(cVars[footCol+footId * 3 + el] - cVars[footCol + el] - refCost[footId-1][el])* (cVars[footCol+footId * 3 + el] - cVars[footCol + el] - refCost[footId-1][el]) for el in range(3)])
startCol = endCol
return cost
def stepSizeCost(pb, cVars, initPos, initCom):
startCol = 0;
previousStartCol = 0;
endCol = 0;
#~ fixedFootMatrix = None;
footOffset = 4
cost = 0
for phaseId, phaseDataT in enumerate(pb["phaseData"]):
#inequality
endCol = startCol + numVariablesForPhase(phaseDataT)
footCol = startCol + footOffset
# expressing everything in terms of first foot
for footId in range(1, N_EFFECTORS):
cost += sum([(cVars[footCol+footId * 3 + el] - 0.15)* (cVars[footCol+footId * 3 + el] - 0.15) for el in range(2)])
startCol = endCol
return cost
def maxStepSizeCost(pb, cVars, initPos, initCom):
startCol = 0;
previousStartCol = 0;
endCol = 0;
#~ fixedFootMatrix = None;
footOffset = 4
cost = 0
for phaseId, phaseDataT in enumerate(pb["phaseData"]):
#inequality
endCol = startCol + numVariablesForPhase(phaseDataT)
footCol = startCol + footOffset
# expressing everything in terms of first foot
for footId in range(1, N_EFFECTORS):
cost += sum([(cVars[footCol+footId * 3 + el] - 0.15)* (cVars[footCol+footId * 3 + el] - 0.15) for el in range(2)])
startCol = endCol
return cost
def solveMIPGurobi(pb, surfaces, MIP = True, draw_scene = None, plot = True, initGuess = None, initGuessMip = None, l1Contact = False, initPos = None, endPos = None, initCom = None, endCom = None):
......@@ -831,9 +896,9 @@ def solveMIPGurobi(pb, surfaces, MIP = True, draw_scene = None, plot = True, ini
A, b, E, e = convertProblemToLp(pb)
print ("initpos ", initPos)
E,e = addInitEndConstraint(pb, E, e, initPos, endPos, initCom, endCom)
# ~ E,e = addInitEndConstraint(pb, E, e, initPos, endPos, initCom, endCom)
#todo is end constraint desirable ?
# ~ E,e = addInitEndConstraint(pb, E, e, initPos, endPos, initCom, None)
E,e = addInitEndConstraint(pb, E, e, initPos, endPos, initCom, None)
slackMatrix = wSelectionMatrix(pb)
slackIndices = [i for i,el in enumerate (slackMatrix) if el > 0]
numSlackVariables = len([el for el in slackMatrix if el > 0])
......@@ -975,8 +1040,14 @@ def solveMIPGurobi(pb, surfaces, MIP = True, draw_scene = None, plot = True, ini
if initPos is not None:
# ~ obj = targetCom(pb, cVars, endCom)
obj = posturalCost(pb, cVars, initPos, initCom)
obj = 0
print (" initPos is not None !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!")
obj += 0.01 * posturalCost(pb, cVars, initPos, initCom)
# ~ obj += 0.01 * posturalCost(pb, cVars, initPos, initCom)
# ~ obj += stepSizeCost(pb, cVars, initPos, initCom)
# ~ obj += 10. * targetCom(pb, cVars, endCom)
obj += 10. * targetCom(pb, cVars, endCom)
# ~ obj = targetLegCenter(pb, cVars, endCom)
model.setObjective(obj)
# ~ grb.setParam('SOLUTION_LIMIT', 1)
......
......@@ -41,3 +41,4 @@ q_init = q_ref[::]
fullBody.setReferenceConfig(q_ref)
fullBody.setCurrentConfig (q_init)
fullBody.setPostureWeights(fullBody.postureWeights[::])
footOffset = 0.03
......@@ -13,13 +13,14 @@ initContacts, initPos, initCom = initConstraintsFrom_q_init(fullBody, q_init, li
pb, coms, footpos, allfeetpos, res = solve(initCom = initCom, initPos = initPos)
q_init[2] += footOffset
v(q_init)
#create init state
s = rbprmstate.State(fullBody, q = q_init, limbsIncontact = limbNames[:])
states = [s]
#compute whole-body states from contact sequences
run(fullBody, states, allfeetpos, limbNames, coms, pb)
run(fullBody, states, allfeetpos, limbNames, coms, pb, footOffset)
#play motion
play(states,v)
......
from sl1m.planner_scenarios.state_methods import *
from sl1m.planner_scenarios.anymal_constants import *
from sl1m.stand_alone_scenarios.anymal.flat_ground import solve
#load scene
from hpp.corbaserver.affordance.affordance import AffordanceTool
afftool = AffordanceTool ()
afftool.setAffordanceConfig('Support', [0.5, 0.03, 0.05])
afftool.loadObstacleModel ("hpp_environments", "ori/modular_palet_flat", "planning", v,reduceSizes=[0.1,0,0])
afftool.visualiseAffordances('Support', v, v.color.lightBrown)
v.addLandmark(v.sceneName,1)
#retrieve surfaces from scene for sl1m
from hpp.corbaserver.rbprm.tools import surfaces_from_path
#starting position
q_init [0:3] = [-1.2,0,0.6]
initContacts, initPos, initCom = initConstraintsFrom_q_init(fullBody, q_init, limbNames)
initPos = [array([-0.8, 0.2, 0.13]),
array([-0.8, -0.2, 0.13]),
array([-1.6, 0.2, 0.13]),
array([-1.6, -0.2, 0.13])]
endCom = [10., 0., 0.4]
from sl1m.stand_alone_scenarios.anymal.palet import solve, overrideSurfaces
#in case reduceSize changed
overrideSurfaces(surfaces_from_path.getAllSurfaces(afftool))
#compute contact sequence
pb, coms, footpos, allfeetpos, res = solve(initPos = initPos, endCom = endCom)
# ~ pb, coms, footpos, allfeetpos, res = solve(initPos = initPos)
# ~ footOffset = 0.
q_init[2] += footOffset
v(q_init)
#create init state
s = rbprmstate.State(fullBody, q = q_init, limbsIncontact = limbNames[:])
states = [s]
#compute whole-body states from contact sequences
run(fullBody, states, allfeetpos, limbNames, coms, pb, footOffset)
#play motion
#export contact sequence
cs = exportCS(fullBody, q_init, states, pb, allfeetpos, limbNames, effectorNames)
#save file
cs.saveAsBinary("anymal_palet.cs")
play(states,v)
#display footsteps
displaySteppingStones(cs, v.client.gui, v.sceneName, fullBody)
from sl1m.planner_scenarios.state_methods import *
from sl1m.planner_scenarios.anymal_constants import *
from sl1m.stand_alone_scenarios.anymal.flat_ground import solve
#load scene
from hpp.corbaserver.affordance.affordance import AffordanceTool
afftool = AffordanceTool ()
afftool.setAffordanceConfig('Support', [0.5, 0.03, 0.00005])
afftool.loadObstacleModel ("hpp_environments", "multicontact/ori_plinth", "planning", v,reduceSizes=[0.2,0,0])
afftool.visualiseAffordances('Support', v, v.color.lightBrown)
v.addLandmark(v.sceneName,1)
#retrieve surfaces from scene for sl1m
from hpp.corbaserver.rbprm.tools import surfaces_from_path
#starting position
q_init [0:3] = [-1.7,0,0.465]
initContacts, initPos, initCom = initConstraintsFrom_q_init(fullBody, q_init, limbNames)
initPos = [array([-1.3, 0.2, -0.0]), array([-1.3, -0.2, -0.0]), array([-2.1, 0.2, -0.0]), array([-2.1, -0.2, -0.0])]
v(q_init)
from sl1m.stand_alone_scenarios.anymal.plinth import solve, overrideSurfaces
#in case reduceSize changed
overrideSurfaces(surfaces_from_path.getAllSurfaces(afftool))
#compute contact sequence
print ("initPos ", initPos)
pb, coms, footpos, allfeetpos, res = solve(initPos=initPos)
# ~ pb, coms, footpos, allfeetpos, res = solve()
#create init state
s = rbprmstate.State(fullBody, q = q_init, limbsIncontact = limbNames[:])
states = [s]
#compute whole-body states from contact sequences
run(fullBody, states, allfeetpos, limbNames, coms, pb)
#play motion
play(states,v)
#export contact sequence
cs = exportCS(fullBody, q_init, states, pb, allfeetpos, limbNames, effectorNames)
#save file
cs.saveAsBinary("anymal_flatGround.cs")
#display footsteps
displaySteppingStones(cs, v.client.gui, v.sceneName, fullBody)
from sl1m.planner_scenarios.state_methods import *
from sl1m.planner_scenarios.anymal_constants import *
from sl1m.stand_alone_scenarios.anymal.flat_ground import solve
#load scene
from hpp.corbaserver.affordance.affordance import AffordanceTool
afftool = AffordanceTool ()
afftool.setAffordanceConfig('Support', [0.5, 0.03, 0.05])
afftool.loadObstacleModel ("hpp_environments", "ori/race_slopes_container", "planning", v,reduceSizes=[0.1,0,0])
afftool.visualiseAffordances('Support', v, v.color.lightBrown)
v.addLandmark(v.sceneName,1)
#retrieve surfaces from scene for sl1m
from hpp.corbaserver.rbprm.tools import surfaces_from_path
#starting position
q_init [0:3] = [0, 0, 0.61]
initContacts, initPos, initCom = initConstraintsFrom_q_init(fullBody, q_init, limbNames)
initPos = [array([-0.8, 0.2, 0.13]),
array([-0.8, -0.2, 0.13]),
array([-1.6, 0.2, 0.13]),
array([-1.6, -0.2, 0.13])]
endCom = [10., 0., 0.4]
from sl1m.stand_alone_scenarios.anymal.race_slopes_container import solve, overrideSurfaces
#in case reduceSize changed
overrideSurfaces(surfaces_from_path.getAllSurfaces(afftool))
# ~ #compute contact sequence
pb, coms, footpos, allfeetpos, res = solve(initPos = initPos, endCom = endCom)
# ~ q_init[2] += footOffset
# ~ v(q_init)
# ~ #create init state
# ~ s = rbprmstate.State(fullBody, q = q_init, limbsIncontact = limbNames[:])
# ~ states = [s]
# ~ #compute whole-body states from contact sequences
# ~ run(fullBody, states, allfeetpos, limbNames, coms, pb, footOffset)
# ~ #play motion
# ~ #export contact sequence
# ~ cs = exportCS(fullBody, q_init, states, pb, allfeetpos, limbNames, effectorNames)
# ~ #save file
# ~ cs.saveAsBinary("anymal_palet.cs")
# ~ play(states,v)
# ~ #display footsteps
# ~ displaySteppingStones(cs, v.client.gui, v.sceneName, fullBody)
from hpp.corbaserver.rbprm.anymal import Robot
from hpp.gepetto import Viewer
from hpp.corbaserver import Client
from tools.display_tools import *
import time
from hpp.corbaserver.rbprm import rbprmstate
from hpp.corbaserver.rbprm import state_alg
from hpp.corbaserver.affordance.affordance import AffordanceTool
from numpy import array, zeros, ones
#~ rootJointType = 'freeflyer'
packageName = 'hpp-rbprm-corba'
meshPackageName = 'hpp-rbprm-corba'
#~ urdfName = 'hrp2_trunk_flexible'
#~ urdfNameRoms = ['hrp2_larm_rom','hrp2_rarm_rom','hrp2_lleg_rom','hrp2_rleg_rom']
#~ urdfSuffix = ""
#~ srdfSuffix = ""
fullBody = Robot ()
fullBody.setJointBounds ("root_joint", [-20,20, -10, 10, 0, 2.2])
fullBody.client.robot.setDimensionExtraConfigSpace(0)
from hpp.corbaserver.problem_solver import ProblemSolver
ps = ProblemSolver( fullBody )
r = Viewer (ps)
v = Viewer (ps,viewerClient=r.client, displayCoM = True)
afftool = AffordanceTool ()
afftool.setAffordanceConfig('Support', [0.5, 0.03, 0.00005])
afftool.loadObstacleModel (packageName, "stair_bauzil", "planning", r)
afftool.visualiseAffordances('Support', r, [0.25, 0.5, 0.5])
# ~ fullBody.setConstrainedJointsBounds()
fullBody.setJointBounds("LF_KFE",[-1.4,0.])
fullBody.setJointBounds("RF_KFE",[-1.4,0.])
fullBody.setJointBounds("LH_KFE",[0.,1.4])
fullBody.setJointBounds("RH_KFE",[0.,1.4])
fullBody.setJointBounds ("root_joint", [-20,20, -20, 20, -20, 20])
dict_heuristic = {fullBody.rLegId:"static", fullBody.lLegId:"static", fullBody.rArmId:"fixedStep04", fullBody.lArmId:"fixedStep04"}
fullBody.loadAllLimbs(dict_heuristic,"ReferenceConfiguration",nbSamples=12)
q_ref = fullBody.referenceConfig[::]
q_init = q_ref[::]
fullBody.setReferenceConfig(q_ref)
fullBody.setCurrentConfig (q_init)
fullBody.setPostureWeights(fullBody.postureWeights[::])
from sl1m.stand_alone_scenarios.anymal.flat_ground import solve
def getContactsFromConfig(q, limbs = Robot.limbs_names):
s = rbprmstate.State(fullBody, q = q, limbsIncontact = limbs)
res = {}
for limb in limbs:
rLegPn = s.getContactPosAndNormalsForLimb(limb)
res[limb] = (rLegPn[0][0], rLegPn[1][0])
return res
# ~ rLegPn = s.getContactPosAndNormalsForLimb(Robot.rLegId)
# ~ lLegPn = s.getContactPosAndNormalsForLimb(Robot.lLegId)
# ~ return { Robot.rLegId : (rLegPn[0][0], rLegPn[1][0]),Robot.lLegId : (lLegPn[0][0], lLegPn[1][0]) }
#given by limb constraints
from sl1m.stand_alone_scenarios.constraints_anymal import limbNames, effectorNames
from hpp.corbaserver.rbprm.rbprmstate import StateHelper
initContacts = getContactsFromConfig(q_init)
initPos = [array(initContacts[el][0][0]) for el in limbNames]
initCom = array(fullBody.getCenterOfMass())
# ~ pb, coms, footpos, allfeetpos, res = solve(initCom = None, initPos = None)
pb, coms, footpos, allfeetpos, res = solve(initCom = initCom, initPos = initPos)
z = array([0.,0.,1.])
from scipy.optimize import linprog
#static eq is com is convex combination of pos (projected)
def staticEq(positions, com):
sizeX = len(positions)
E = zeros((3,sizeX))
for i, pos in enumerate(positions):
E[:2,i] = pos[:2]
e = array([com[0], com[1], 1.])
E[2,:] = ones(sizeX)
try:
res = linprog(ones(sizeX), A_ub=None, b_ub=None, A_eq=E, b_eq=e, bounds=[(0.,1.) for _ in range(sizeX)], method='interior-point', callback=None, options={'presolve': True})
return res['success']
except:
return False
def gen_state(s, pId, num_max_sample = 1, first = False, normal = z, newContact = True , useCom = False ):
#~ pId = 6
phase = pb["phaseData"][pId]
feetPos = allfeetpos[pId]
sres, succ = StateHelper.cloneState(s)
for limbId, pos in zip(limbNames, feetPos):
sres, succ = state_alg.addNewContact(sres, limbId, pos.tolist(), normal.tolist(), num_max_sample= 200)
# ~ v(sres.q())
if not succ:
print("succes ?", succ)
succCom = False
ite = 0
# ~ #~ if first:
# ~ #~ print "FIRST "
# ~ #~ com[2] -= 0.25
# ~ #~ while(not succCom and ite < 11):
com = coms[pId*2].tolist()
# ~ print("com ", com)
#check whether com is feasible
if staticEq(feetPos, com):
print ("Config equilibrium")
succCom = True
if useCom:
while(not succCom and ite < 30):
succCom = fullBody.projectStateToCOM(sres.sId ,com, num_max_sample)
com[2] -= 0.05
ite += 1
if succCom:
sres2, succ = StateHelper.cloneState(sres)
q = sres2.q()
q[3:7] = [0.,0.,0.,1.]
q[3:] = fullBody.referenceConfig[3:]
sres2.setQ(q)
succCom2 = fullBody.projectStateToCOM(sres2.sId ,com, num_max_sample)
if succCom2:
print ("SUCCESS COM2 ", succCom)
sres = sres2
print ("SUCCESS COM ", succCom)
v(sres.q())
return sres
v(q_init)
s = rbprmstate.State(fullBody, q = q_init, limbsIncontact = limbNames[:])
states = [s]
def run():
for i, _ in enumerate(allfeetpos[1:]):
states.append(gen_state(states[-1],i+1, useCom = True))
def play():
for s in states:
v(s.q())
time.sleep(0.5)
run()
play()
extraDof = [0 for _ in range(6)]
configs = [ st.q() + extraDof for st in states[:]]; i = 0
......@@ -44,7 +44,6 @@ def staticEq(positions, com):
def gen_state(pb, coms, allfeetpos,fullBody, limbNames, s, pId, num_max_sample = 1, first = False, normal = z, newContact = True , useCom = False ):
#~ pId = 6
phase = pb["phaseData"][pId]
feetPos = allfeetpos[pId]
sres, succ = StateHelper.cloneState(s)
for limbId, pos in zip(limbNames, feetPos):
......@@ -67,7 +66,7 @@ def gen_state(pb, coms, allfeetpos,fullBody, limbNames, s, pId, num_max_sample
if useCom:
while(not succCom and ite < 30):
succCom = fullBody.projectStateToCOM(sres.sId ,com, num_max_sample)
com[2] -= 0.05
com[2] += 0.05
ite += 1
if succCom:
sres2, succ = StateHelper.cloneState(sres)
......@@ -84,12 +83,17 @@ def gen_state(pb, coms, allfeetpos,fullBody, limbNames, s, pId, num_max_sample
fullBody.setCurrentConfig(sres.q())
return sres
def applyOffset(allfeetpos, offset):
for i in range(len(allfeetpos)):
for j in range(len(allfeetpos[i])):
allfeetpos[i][j][2]+=offset
#### show results ####
def run(fullBody, states, allfeetpos, limbNames, coms, pb):
def run(fullBody, states, allfeetpos, limbNames, coms, pb, offset, useCom = False):
applyOffset(allfeetpos, offset)
for i, _ in enumerate(allfeetpos[1:]):
states.append(gen_state(pb,coms,allfeetpos,fullBody, limbNames, states[-1],i+1, useCom = True))
states.append(gen_state(pb,coms,allfeetpos,fullBody, limbNames, states[-1],i+1, useCom = useCom))
def play(states, v, dt = 0.5):
for s in states:
......@@ -122,7 +126,7 @@ def exportCS(fullBody, q_init, states, pb, allfeetpos, limbNames, effectorNames)
cs = ContactSequence(0)
addPhaseFromConfig(fullBody, cs, q_init, limbNames[:])
rot = Quaternion.Identity() #todo update
for pId in range(1, len(pb["phaseData"])):
for pId in range(1, len(states)):
print ('phase ', pId)
# ~ rot = Quaternion.Identity()
prevFeetPos = allfeetpos[pId-1]
......
......@@ -59,6 +59,8 @@ def solve(initCom = None, initPos = None, endCom = None):
# ~ endCom = initCom + array([6, 0.0, 0.0])
initGlobals(nEffectors = 4)
pb = gen_flat_pb()
print ("init post ", initPos)
print ("initCom ", initCom)
endPos = None
# ~ print ("initPos", initPos)
pb, res, time = solveMIPGurobi(pb, surfaces, MIP = True, draw_scene = None, plot = True, l1Contact = False, initPos = initPos, endPos = endPos, initCom = initCom, endCom= endCom)
......@@ -103,7 +105,6 @@ if __name__ == '__main__':
Acom = np.hstack([A[:,:2], A[:,3:4]])
for i in range(5):
print ("WTTTTTTTTTTFFFFFFFFFFFFFFf!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!", i)
coms, footpos, allfeetpos = retrieve_points_from_res(pb, res)
pb = gen_flat_pb()
initPos = None
......@@ -114,7 +115,7 @@ if __name__ == '__main__':
pb, res, time = solveMIPGurobi(pb, surfaces, MIP = True, draw_scene = None, plot = True, l1Contact = False, initPos = initPos, endPos = endPos, initCom = initCom, endCom= endCom)
plotQPRes(pb, res, ax=ax, plot_constraints = False, show = False, plotSupport = True)
plt.show(block = False)
# ~ pb, res, time = solveMIPGurobi(pb, surfaces, MIP = True, draw_scene = None, plot = True, l1Contact = False, initPos = None)
pb, res, time = solveMIPGurobi(pb, surfaces, MIP = True, draw_scene = None, plot = True, l1Contact = False, initPos = None)
......
import numpy as np
from numpy import array, asmatrix, matrix, zeros, ones
from numpy import array, dot, stack, vstack, hstack, asmatrix, identity, cross, concatenate
from numpy.linalg import norm
from sl1m.constants_and_tools import *
from sl1m.planner_l1_generic_equalities_as_ineq import *
from hpp.corbaserver.rbprm import rbprmstate
from hpp.corbaserver.rbprm import state_alg
from sl1m.stand_alone_scenarios.constraints_anymal import *
all_surfaces = None
all_surfaces_array = None
default_surfaces = None
surfaces_and_normals = [([[0.39550296660292134, -0.27720715031876186, 0.16499999165534973],
[0.39550296660292134, -0.44479297175155064, 0.16499999165534973],
[0.5644970119394066, -0.44479297175155064, 0.16499999165534973],
[0.5644970119394066, -0.27720715031876186, 0.16499999165534973]],
[0.0, 0.0, 1.0]),
([[0.3955029688071581, 0.20279284117516114, 0.16499999165534973],
[0.3955029688071581, 0.035207045099176625, 0.16499999165534973],
[0.5644970097351698, 0.035207045099176625, 0.16499999165534973],
[0.5644970097351698, 0.20279284117516114, 0.16499999165534973]],
[0.0, 0.0, 1.0]),
([[-0.5644969567065002, -0.27720715472723556, 0.16499999165534973],
[-0.5644969567065002, -0.44479296734307694, 0.16499999165534973],
[-0.39550296223118286, -0.44479296734307694, 0.16499999165534973],
[-0.39550296223118286, -0.27720715472723556, 0.16499999165534973]],
[0.0, 0.0, 1.0]),
([[-0.5644969545022632, 0.20279283676668725, 0.16499999165534973],