def generateContactSequence():
fb, v = display_tools.initScene(cfg.Robot, cfg.ENV_NAME)
cs = ContactSequenceHumanoid(0)
filename = cfg.CONTACT_SEQUENCE_PATH + "/" + cfg.DEMO_NAME + ".cs"
print "Import contact sequence binary file : ", filename
cs.loadFromBinary(filename)
display_tools.displayWBconfig(
v, cs.contact_phases[0].reference_configurations[0])
return cs, fb, v
def loadMotionFromFiles(gui, path, npzFilename, csFilename):
# load cs from file :
cs = ContactSequenceHumanoid(0)
cs.loadFromBinary(path + csFilename)
displaySteppingStones(cs, gui, sceneName, Robot)
colors = [[0., 0., 1., 1.], [0., 1., 0., 1.]]
displayCOMTrajectory(cs, gui, sceneName, colors)
#extract data from npz archive :
res = wb_res.loadFromNPZ(path + npzFilename)
displayFeetTrajFromResult(gui, sceneName, res, Robot)
plot.plotALLFromWB(cs, res)
return res, cs
def addInitAndGoalShift(cs_in):
cs = cs_com = ContactSequenceHumanoid(cs_in.size() + 2)
# copy all phases but leave one phase at the beginning and at the end :
for k in range(cs_in.size()):
phase = cs_in.contact_phases[k].copy()
# shift times to take in account the new phase :
for i in range(len(phase.time_trajectory)):
phase.time_trajectory[i] += cfg.TIME_SHIFT_COM
cs.contact_phases[k + 1] = phase
# now add new first phase :
phase = cs.contact_phases[0]
s_final = cs.contact_phases[1].init_state
s_init = s_final.copy()
s_init[2] -= cfg.COM_SHIFT_Z
phase.init_state = s_init
phase.final_state = s_final
generateLinearInterpTraj(phase, cfg.TIME_SHIFT_COM, 0)
copyPhaseContacts(cs.contact_phases[1], phase)
phase.reference_configurations = cs.contact_phases[
1].reference_configurations
# add the new final phase
phase = cs.contact_phases[-1]
s_init = cs.contact_phases[-2].final_state
s_final = s_init.copy()
s_final[2] -= cfg.COM_SHIFT_Z
phase.init_state = s_init
phase.final_state = s_final
generateLinearInterpTraj(phase, cfg.TIME_SHIFT_COM,
cs.contact_phases[-2].time_trajectory[-1])
copyPhaseContacts(cs.contact_phases[-2], phase)
phase.reference_configurations = cs.contact_phases[
-2].reference_configurations
return cs
def generateCentroidalTrajectory(cs, cs_initGuess=None, fb=None, viewer=None):
if cs_initGuess:
print "WARNING : in centroidal.croc, initial guess is ignored."
if not fb:
raise ValueError("CROC called without fullBody object.")
beginId, endId = createFullbodyStatesFromCS(cs, fb)
print "beginid = ", beginId
print "endId = ", endId
cs_result = ContactSequenceHumanoid(cs)
if (2 * (endId - beginId) + 1) != cs.size():
raise NotImplemented(
"Current implementation of CROC require to have 2 contact phases per States in fullBody"
)
# for each phase in the cs, create a corresponding FullBody State and call CROC,
# then discretize the solution and fill the cs struct
# Make the assumption that the CS was created with the generateContactSequence method from the same fb object
id_phase = 0
for id_state in range(beginId, endId):
print "id_state = ", str(id_state)
print "id_phase = ", str(id_phase)
# First, compute the bezier curves between the states :
pid = fb.isDynamicallyReachableFromState(id_state,
id_state + 1,
True,
numPointsPerPhases=0)
if len(pid) != 4:
print "# ERROR : Cannot compute CROC between states " + str(
id_state) + " , " + str(id_state + 1)
return cs
c = fb.getPathAsBezier(int(pid[0]))
# Now split and write the results in the correct contactPhases
if id_phase == 0:
append = False
t = 0.
else:
append = True
t = cs_result.contact_phases[id_phase].time_trajectory[-1]
assert c.min() == 0, "bezier curve should start at t=0."
# First phase (second half of the trajectory of this phase if it's not the initial phase of the CS)
start = 0
end = fb.client.problem.pathLength(int(pid[1]))
writeTrajInPhase(cs_result.contact_phases[id_phase], c, t, start, end,
append)
#Second phase
id_phase += 1
start = end
end += fb.client.problem.pathLength(int(pid[2]))
t = cs_result.contact_phases[id_phase - 1].time_trajectory[-1]
writeTrajInPhase(cs_result.contact_phases[id_phase], c, t, start, end)
# Third phase (only the first half of the trajectory, exept if it's the final contact phase of the CS)
id_phase += 1
start = end
end += fb.client.problem.pathLength(int(pid[3]))
t = cs_result.contact_phases[id_phase - 1].time_trajectory[-1]
assert abs(end -
c.max()) < 1e-10, "Error in computation of time interval"
writeTrajInPhase(cs_result.contact_phases[id_phase], c, t, start, end)
return cs_result
import numpy as np
import pinocchio
from crocoddyl import (ActionModelImpact, CallbackDDPVerbose,
CallbackSolverDisplay, ShootingProblem, SolverDDP,
StatePinocchio, a2m, m2a)
from crocoddyl.locomotion import ContactSequenceWrapper, createMultiphaseShootingProblem
from multicontact_api import ContactSequenceHumanoid, CubicHermiteSpline
np.set_printoptions(linewidth=400, suppress=True)
robot = conf.robot
rmodel = robot.model
rdata = robot.data
rmodel.defaultState = np.concatenate([m2a(robot.q0), np.zeros(rmodel.nv)])
# ----------------Load Contact Phases and PostProcess-----------------------
cs = ContactSequenceHumanoid(0)
cs.loadFromXML(conf.MUSCOD_CS_OUTPUT_FILENAME, conf.CONTACT_SEQUENCE_XML_TAG)
csw = ContactSequenceWrapper(cs, conf.contact_patches)
csw.createCentroidalPhi(rmodel, rdata)
csw.createEESplines(rmodel, rdata, conf.X_init, 0.005)
# ----------------Define Problem----------------------------
models = createMultiphaseShootingProblem(rmodel, rdata, csw, conf.DT)
# disp = lambda xs: disptraj(robot, xs)
problem = ShootingProblem(m2a(conf.X_init[0]), models[:-1], models[-1])
# Set contacts in the data elements. Ugly.
# This is defined for IAMEuler. If using IAMRK4, differential is a list. so we need to change.
for d in problem.runningDatas:
from multicontact_api import ContactSequenceHumanoid, ContactPhaseHumanoid
import mlp.viewer.display_tools as display_tools
from mlp.utils.cs_tools import *
import mlp.config as cfg
from pinocchio import SE3
from mlp.utils.util import rotatePlacement
from hpp.corbaserver.rbprm.talos import Robot # change robot here
from mlp.utils.cs_tools import moveEffectorToPlacement
import numpy as np
from hpp.corbaserver.rbprm.rbprmstate import State, StateHelper
ENV_NAME = "multicontact/stairsAirbus_noRamp"
fb, v = display_tools.initScene(Robot, ENV_NAME, False)
cs = ContactSequenceHumanoid(0)
#Create an initial contact phase :
q_ref = [
0,
0,
0.9832773,
0,
0.0,
0.0,
1.0, #Free flyer
1.45,
0.0,
-0.611354,
1.059395,
-0.448041,
-0.001708, #Left Leg
def fillCSFromTimeopt(cs, cs_initGuess, tp):
cs_com = ContactSequenceHumanoid(cs)
# extract infos from tp to fill cs.contact_phases struct
u = [0] * 6
x = [0] * 9
MASS = tp.getMass()
p_id = 0 # phase id in cs
k_id = 1 # id in the current phase
# tp.getTime(0) == dt !! not 0
p0 = cs_com.contact_phases[0]
init_state = p0.init_state
init_state[0:3] = tp.getInitialCOM()
p0.init_state = init_state
state = p0.init_state
appendOrReplace(p0.time_trajectory, 0, 0.)
appendOrReplace(p0.state_trajectory, 0, state)
appendOrReplace(p0.control_trajectory, 0, np.matrix(np.zeros(6)).T)
for k in range(tp.getTrajectorySize()):
appendOrReplace(cs_com.contact_phases[p_id].time_trajectory, k_id,
tp.getTime(k))
#extract x and u from tp :
if k == 0:
u[0:3] = ((tp.getLMOM(k) / MASS) /
tp.getTime(k)).tolist() # acceleration
u[3:6] = ((tp.getAMOM(k)) /
(tp.getTime(k))).tolist() # angular momentum variation
else:
u[0:3] = (
((tp.getLMOM(k) / MASS) - (tp.getLMOM(k - 1) / MASS)) /
(tp.getTime(k) - tp.getTime(k - 1))).tolist() #acceleration
u[3:6] = ((tp.getAMOM(k) - tp.getAMOM(k - 1)) /
(tp.getTime(k) - tp.getTime(k - 1))
).tolist() # angular momentum variation
#print "control = ",np.matrix(u)
x[0:3] = tp.getCOM(k).tolist() # position
x[3:6] = (tp.getLMOM(k) / MASS).tolist() # velocity
x[6:9] = tp.getAMOM(k).tolist() # angular momentum
appendOrReplace(cs_com.contact_phases[p_id].control_trajectory, k_id,
np.matrix(u))
appendOrReplace(cs_com.contact_phases[p_id].state_trajectory, k_id,
np.matrix(x))
if k > 0 and isNewPhase(tp, k - 1, k, cs_com, cs_initGuess,
p_id) and p_id < cs_com.size() - 1:
#last k of current phase, first k of next one (same state_traj and time)
# set final state of current phase :
cs_com.contact_phases[p_id].final_state = np.matrix(x)
# first k of the current phase
p_id += 1
k_id = 0
cs_com.contact_phases[p_id].init_state = np.matrix(x)
appendOrReplace(cs_com.contact_phases[p_id].time_trajectory, k_id,
tp.getTime(k))
appendOrReplace(cs_com.contact_phases[p_id].control_trajectory,
k_id, np.matrix(u))
appendOrReplace(cs_com.contact_phases[p_id].state_trajectory, k_id,
np.matrix(x))
k_id += 1
# timeopt solution is not guarantee to end at the desired final state.
# so we add a final phase here, with a smooth motion from the final state atteined by timeopt to the desired one
connectFinalPhase(cs_com.contact_phases[-1])
return cs_com
def generateCentroidalTrajectory(cs,
cs_initGuess=None,
fullBody=None,
viewer=None):
if cs_initGuess:
print "WARNING : in centroidal.geometric, initial guess is ignored."
cs_result = ContactSequenceHumanoid(cs)
p0 = cs_result.contact_phases[0]
com_z = p0.init_state[2, 0]
previous_phase = None
# first, compute the new init/final position for each state : in the center of the support polygon
for pid in range(1, cs_result.size() - 1):
phase = cs_result.contact_phases[pid]
state = phase.init_state
com_x = 0.
com_y = 0.
if phase.LF_patch.active:
com_x += phase.LF_patch.placement.translation[0, 0]
com_y += phase.LF_patch.placement.translation[1, 0]
if phase.RF_patch.active:
com_x += phase.RF_patch.placement.translation[0, 0]
com_y += phase.RF_patch.placement.translation[1, 0]
if phase.LH_patch.active:
com_x += phase.LH_patch.placement.translation[0, 0]
com_y += phase.LH_patch.placement.translation[1, 0]
if phase.RH_patch.active:
com_x += phase.RH_patch.placement.translation[0, 0]
com_y += phase.RH_patch.placement.translation[1, 0]
com_x /= phase.numActivePatches()
com_y /= phase.numActivePatches()
# test : take com height from config found from planning :
com_z = cs_result.contact_phases[pid].init_state[2, 0]
state[0] = com_x
state[1] = com_y
state[2] = com_z
phase.init_state = state
#print "phase : "+str(pid)+" com Init = "+str(com_x)+","+str(com_y)+","+str(com_z)
if previous_phase != None:
previous_phase.final_state = phase.init_state.copy()
previous_phase = cs_result.contact_phases[pid]
# then, generate a straight line from init_state to final_state for each phase :
t_total = 0.
for pid in range(cs_result.size()):
phase = cs_result.contact_phases[pid]
duration = computePhaseDuration(cs_result, pid)
com0 = phase.init_state[0:3]
com1 = phase.final_state[0:3]
vel = (com1 - com0) / duration
am = np.matrix(np.zeros(3)).T
t = 0.
while t < duration - 0.0001:
u = t / duration
state = np.matrix(np.zeros(9)).T
com = com0 * (1. - u) + com1 * (u)
state[0:3] = com
state[3:6] = vel
state[6:9] = am
phase.state_trajectory.append(state)
phase.time_trajectory.append(t_total)
t += cfg.SOLVER_DT
t_total += cfg.SOLVER_DT
state[0:3] = com1
state[3:6] = vel
state[6:9] = am
phase.state_trajectory.append(state)
phase.time_trajectory.append(t_total)
return cs_result
def contactSequenceFromRBPRMConfigs(fb, beginId, endId):
print "generate contact sequence from planning : "
n_states = endId - beginId + 1
# There could be either contact break, creation or repositionning between each adjacent states.
# But there should be only contacts break or creation between each adjacent contactPhases
cs = ContactSequenceHumanoid(0)
prev_phase = None
phaseId = 0
# for each contact state we must create 2 phase (one with all the contact and one with the next replaced contact(s) broken)
for stateId in range(beginId, endId + 1):
if VERBOSE:
print "current state id = ", stateId
# %%%%%%%%% add phase with all the contacts in the rbprm State: %%%%%%%%%%%%%
phase = ContactPhaseHumanoid()
current_config = fb.getConfigAtState(stateId)
fb.setCurrentConfig(current_config)
# Determine the contact changes which are going to occur :
if stateId < endId:
next_variations = fb.getContactsVariations(stateId, stateId + 1)
else:
next_variations = []
if VERBOSE:
print "variations : ", next_variations
assert len(
next_variations
) <= 1, "Several changes of contacts in adjacent states, not implemented yet !"
if len(next_variations) == 0:
if VERBOSE:
print "no variations !"
movingLimb = None
contact_break = False
contact_create = False
else:
movingLimb = next_variations[0]
contact_break = fb.isLimbInContact(movingLimb, stateId)
contact_create = fb.isLimbInContact(movingLimb, stateId + 1)
if contact_break and contact_create:
contact_reposition = True
else:
contact_reposition = False
if VERBOSE:
print "movingLimb = ", movingLimb
print "break = " + str(contact_break) + " ; create = " + str(
contact_create) + " ; reposition = " + str(contact_reposition)
if movingLimb or stateId == endId: # add a ContactPhase corresponding to the current state
# Build the phase contact patches (placement and activity)
setContactActivityFromRBRMState(phase, fb, stateId)
if not prev_phase:
setContactPlacementFromRBPRMState(phase, fb, stateId)
else:
copyPhaseContactPlacements(prev_phase, phase)
# now we change the contacts that have moved :
previous_variations = fb.getContactsVariations(
stateId - 1, stateId)
setContactPlacementFromRBPRMState(phase, fb, stateId,
previous_variations)
# assign current wholebody config as reference in phase :
phase.reference_configurations.append(
np.matrix((current_config)).T)
# retrieve the COM position for init and final state
init_state = np.matrix(np.zeros(9)).T
init_state[0:3] = np.matrix(fb.getCenterOfMass()).transpose()
init_state[3:6] = np.matrix(current_config[-6:-3]).transpose()
phase.init_state = init_state
final_state = init_state.copy()
if not contact_reposition and stateId < endId: # in the case of contact reposition, the CoM motion will take place in the next intermediate phase. For the current phase the init and final state are equals
current_config = fb.getConfigAtState(stateId + 1)
fb.setCurrentConfig(current_config)
final_state[0:3] = np.matrix(fb.getCenterOfMass()).transpose()
final_state[3:6] = np.matrix(current_config[-6:-3]).transpose()
phase.final_state = final_state
# add phase to contactSequence :
cs.contact_phases.append(phase)
phaseId += 1
prev_phase = phase
if VERBOSE:
print "add a phase at id : ", phaseId - 1
if contact_reposition:
# %%%%%% create intermediate state, by removing the contact repositionned betwen stateId and stateId+1 %%%%%%%%
phase = ContactPhaseHumanoid()
# copy previous placement :
copyPhaseContacts(prev_phase, phase)
# find the contact to break :
patch = contactPatchForEffector(phase,
fb.dict_limb_joint[movingLimb])
patch.active = False
# assign current wholebody config as reference in phase :
phase.reference_configurations.append(
np.matrix((current_config)).T)
# retrieve the COM position for init and final state
phase.init_state = prev_phase.final_state.copy()
final_state = phase.init_state.copy()
current_config = fb.getConfigAtState(stateId + 1)
fb.setCurrentConfig(current_config)
final_state[0:3] = np.matrix(fb.getCenterOfMass()).transpose()
final_state[3:6] = np.matrix(current_config[-6:-3]).transpose()
phase.final_state = final_state.copy()
# add phase to contactSequence :
cs.contact_phases.append(phase)
phaseId += 1
prev_phase = phase
if VERBOSE:
print "add an intermediate contact phase at id : ", phaseId - 1
# end adding intermediate contact phase
# end if movingLimb or stateId == endId
# end for each stateId
# assign contact models :
# only used by muscod ?? But we need to fill it otherwise the serialization fail
for k, phase in enumerate(cs.contact_phases):
RF_patch = phase.RF_patch
cm = RF_patch.contactModel
cm.mu = cfg.MU
cm.ZMP_radius = 0.01
RF_patch.contactModelPlacement = SE3.Identity()
LF_patch = phase.LF_patch
cm = LF_patch.contactModel
cm.mu = cfg.MU
cm.ZMP_radius = 0.01
LF_patch.contactModelPlacement = SE3.Identity()
LH_patch = phase.LH_patch
cm = LH_patch.contactModel
cm.mu = cfg.MU
cm.ZMP_radius = 0.01
LH_patch.contactModelPlacement = SE3.Identity()
RH_patch = phase.RH_patch
cm = RH_patch.contactModel
cm.mu = cfg.MU
cm.ZMP_radius = 0.01
RH_patch.contactModelPlacement = SE3.Identity()
return cs
def generateContactSequence():
RF, root_init, pb, coms, footpos, allfeetpos, res = runLPScript()
# load scene and robot
fb, v = initScene(cfg.Robot, cfg.ENV_NAME, False)
q_init = fb.referenceConfig[::] + [0] * 6
q_init[0:7] = root_init
#q_init[2] += fb.referenceConfig[2] - 0.98 # 0.98 is in the _path script
v(q_init)
# init contact sequence with first phase : q_ref move at the right root pose and with both feet in contact
# FIXME : allow to customize that first phase
cs = ContactSequenceHumanoid(0)
addPhaseFromConfig(fb, v, cs, q_init, [fb.rLegId, fb.lLegId])
# loop over all phases of pb and add them to the cs :
for pId in range(
2, len(pb["phaseData"])
): # start at 2 because the first two ones are already done in the q_init
prev_contactPhase = cs.contact_phases[-1]
#n = normal(pb["phaseData"][pId])
phase = pb["phaseData"][pId]
moving = phase["moving"]
movingID = fb.lfoot
if moving == RF:
movingID = fb.rfoot
pos = allfeetpos[pId] # array, desired position for the feet movingID
pos[2] += 0.005 # FIXME it shouldn't be required !!
# compute desired foot rotation :
if USE_ORIENTATION:
if pId < len(pb["phaseData"]) - 1:
quat0 = Quaternion(pb["phaseData"][pId]["rootOrientation"])
quat1 = Quaternion(pb["phaseData"][pId + 1]["rootOrientation"])
rot = quat0.slerp(0.5, quat1)
# check if feets do not cross :
if moving == RF:
qr = rot
ql = Quaternion(
prev_contactPhase.LF_patch.placement.rotation)
else:
ql = rot
qr = Quaternion(
prev_contactPhase.RF_patch.placement.rotation)
rpy = matrixToRpy((qr * (ql.inverse())).matrix(
)) # rotation from the left foot pose to the right one
if rpy[2, 0] > 0: # yaw positive, feet are crossing
rot = Quaternion(phase["rootOrientation"]
) # rotation of the root, from the guide
else:
rot = Quaternion(phase["rootOrientation"]
) # rotation of the root, from the guide
else:
rot = Quaternion.Identity()
placement = SE3()
placement.translation = np.matrix(pos).T
placement.rotation = rot.matrix()
moveEffectorToPlacement(fb,
v,
cs,
movingID,
placement,
initStateCenterSupportPolygon=True)
# final phase :
# fixme : assume root is in the middle of the last 2 feet pos ...
q_end = fb.referenceConfig[::] + [0] * 6
p_end = (allfeetpos[-1] + allfeetpos[-2]) / 2.
for i in range(3):
q_end[i] += p_end[i]
setFinalState(cs, q=q_end)
displaySteppingStones(cs, v.client.gui, v.sceneName, fb)
return cs, fb, v
def contactSequenceFromRBPRMConfigs(fb, configs, beginId, endId):
print "generate contact sequence from planning : "
global i_sphere
i_sphere = 0
#print "MR offset",MRsole_offset
#print "ML offset",MLsole_offset
n_double_support = len(configs)
# config only contains the double support stance
n_steps = n_double_support * 2 - 1
# Notice : what we call double support / simple support are in fact the state with all the contacts and the state without the next moving contact
cs = ContactSequenceHumanoid(n_steps)
unusedPatch = cs.contact_phases[0].LF_patch.copy()
unusedPatch.placement = SE3.Identity()
unusedPatch.active = False
# for each contact state we must create 2 phase (one with all the contact and one with the next replaced contact(s) broken)
for stateId in range(beginId, endId + 1):
# %%%%%%%%% all the contacts : %%%%%%%%%%%%%
cs_id = (stateId - beginId) * 2
config_id = stateId - beginId
phase_d = cs.contact_phases[cs_id]
fb.setCurrentConfig(configs[config_id])
"""
init_guess_for_phase = init_guess_provided
if init_guess_for_phase:
c_init_guess = curves_initGuess[config_id]
t_init_guess = timings_initGuess[config_id]
init_guess_for_phase = isinstance(c_init_guess,bezier)
if init_guess_for_phase:
print "bezier curve provided for config id : "+str(config_id)
"""
# compute MRF and MLF : the position of the contacts
q_rl = fb.getJointPosition(fb.rfoot)
q_ll = fb.getJointPosition(fb.lfoot)
q_rh = fb.getJointPosition(fb.rhand)
q_lh = fb.getJointPosition(fb.lhand)
# feets
MRF = SE3.Identity()
MLF = SE3.Identity()
MRF.translation = np.matrix(q_rl[0:3]).T
MLF.translation = np.matrix(q_ll[0:3]).T
if not cfg.FORCE_STRAIGHT_LINE:
rot_rl = quatFromConfig(q_rl)
rot_ll = quatFromConfig(q_ll)
MRF.rotation = rot_rl.matrix()
MLF.rotation = rot_ll.matrix()
# apply the transform ankle -> center of contact
MRF *= fb.MRsole_offset
MLF *= fb.MLsole_offset
# hands
MRH = SE3()
MLH = SE3()
MRH.translation = np.matrix(q_rh[0:3]).T
MLH.translation = np.matrix(q_lh[0:3]).T
rot_rh = quatFromConfig(q_rh)
rot_lh = quatFromConfig(q_lh)
MRH.rotation = rot_rh.matrix()
MLH.rotation = rot_lh.matrix()
MRH *= fb.MRhand_offset
MLH *= fb.MLhand_offset
phase_d.RF_patch.placement = MRF
phase_d.LF_patch.placement = MLF
phase_d.RH_patch.placement = MRH
phase_d.LH_patch.placement = MLH
# initial state : Set all new contacts patch (either with placement computed below or unused)
if stateId == beginId:
# FIXME : for loop ? how ?
if fb.isLimbInContact(fb.rLegId, stateId):
phase_d.RF_patch.active = True
else:
phase_d.RF_patch.active = False
if fb.isLimbInContact(fb.lLegId, stateId):
phase_d.LF_patch.active = True
else:
phase_d.LF_patch.active = False
if fb.isLimbInContact(fb.rArmId, stateId):
phase_d.RH_patch.active = True
else:
phase_d.RH_patch.active = False
if fb.isLimbInContact(fb.lArmId, stateId):
phase_d.LH_patch.active = True
else:
phase_d.LH_patch.active = False
else:
# we need to copy the unchanged patch from the last simple support phase (and not create a new one with the same placement)
phase_d.RF_patch = phase_s.RF_patch
phase_d.RF_patch.active = fb.isLimbInContact(fb.rLegId, stateId)
phase_d.LF_patch = phase_s.LF_patch
phase_d.LF_patch.active = fb.isLimbInContact(fb.lLegId, stateId)
phase_d.RH_patch = phase_s.RH_patch
phase_d.RH_patch.active = fb.isLimbInContact(fb.rArmId, stateId)
phase_d.LH_patch = phase_s.LH_patch
phase_d.LH_patch.active = fb.isLimbInContact(fb.lArmId, stateId)
# now we change the contacts that have moved :
variations = fb.getContactsVariations(stateId - 1, stateId)
if len(variations) != 1:
print "Several contact changes between states " + str(
stateId -
1) + " and " + str(stateId) + " : " + str(variations)
assert len(
variations
) == 1, "Several changes of contacts in adjacent states, not implemented yet !"
for var in variations:
# FIXME : for loop in variation ? how ?
if var == fb.lLegId:
phase_d.LF_patch.placement = MLF
if var == fb.rLegId:
phase_d.RF_patch.placement = MRF
if var == fb.lArmId:
phase_d.LH_patch.placement = MLH
if var == fb.rArmId:
phase_d.RH_patch.placement = MRH
# retrieve the COM position for init and final state (equal for double support phases)
init_state = np.matrix(np.zeros(9)).T
init_state[0:3] = np.matrix(fb.getCenterOfMass()).transpose()
init_state[3:6] = np.matrix(configs[config_id][-6:-3]).transpose()
final_state = init_state.copy()
#phase_d.time_trajectory.append((fb.getDurationForState(stateId))*cfg.DURATION_n_CONTACTS/cfg.SPEED)
phase_d.init_state = init_state
phase_d.final_state = final_state
phase_d.reference_configurations.append(
np.matrix((configs[config_id])).T)
#print "done for double support"
if stateId < endId:
# %%%%%% simple support : %%%%%%%%
phase_s = cs.contact_phases[cs_id + 1]
# copy previous placement :
phase_s.RF_patch = phase_d.RF_patch
phase_s.LF_patch = phase_d.LF_patch
phase_s.RH_patch = phase_d.RH_patch
phase_s.LH_patch = phase_d.LH_patch
# find the contact to break :
variations = fb.getContactsVariations(stateId, stateId + 1)
if len(variations) != 1:
print "Several contact changes between states " + str(
stateId) + " and " + str(stateId +
1) + " : " + str(variations)
assert len(
variations
) == 1, "Several changes of contacts in adjacent states, not implemented yet !"
for var in variations:
if var == fb.lLegId:
phase_s.LF_patch.active = False
if var == fb.rLegId:
phase_s.RF_patch.active = False
if var == fb.lArmId:
phase_s.LH_patch.active = False
if var == fb.rArmId:
phase_s.RH_patch.active = False
# retrieve the COM position for init and final state
phase_s.reference_configurations.append(
np.matrix((configs[config_id])).T)
init_state = phase_d.init_state.copy()
final_state = phase_d.final_state.copy()
fb.setCurrentConfig(configs[config_id + 1])
final_state[0:3] = np.matrix(fb.getCenterOfMass()).transpose()
final_state[3:6] = np.matrix(configs[config_id +
1][-6:-3]).transpose()
#phase_s.time_trajectory.append((fb.getDurationForState(stateId))*(1-cfg.DURATION_n_CONTACTS)/cfg.SPEED)
phase_s.init_state = init_state.copy()
phase_s.final_state = final_state.copy()
#print "done for single support"
# assign contact models :
# only used by muscod ?? But we need to fill it otherwise the serialization fail
for k, phase in enumerate(cs.contact_phases):
RF_patch = phase.RF_patch
cm = RF_patch.contactModel
cm.mu = cfg.MU
cm.ZMP_radius = 0.01
RF_patch.contactModelPlacement = SE3.Identity()
LF_patch = phase.LF_patch
cm = LF_patch.contactModel
cm.mu = cfg.MU
cm.ZMP_radius = 0.01
LF_patch.contactModelPlacement = SE3.Identity()
LH_patch = phase.LH_patch
cm = LH_patch.contactModel
cm.mu = cfg.MU
cm.ZMP_radius = 0.01
LH_patch.contactModelPlacement = SE3.Identity()
RH_patch = phase.RH_patch
cm = RH_patch.contactModel
cm.mu = cfg.MU
cm.ZMP_radius = 0.01
RH_patch.contactModelPlacement = SE3.Identity()
return cs
def generateCentroidalTrajectory(cs,
cs_initGuess=None,
fullBody=None,
viewer=None):
if cs_initGuess:
print "WARNING : in centroidal.quasiStatic, initial guess is ignored."
if not fullBody:
raise ValueError("quasiStatic called without fullBody object.")
beginId, endId = createFullbodyStatesFromCS(cs, fullBody)
print "beginid = ", beginId
print "endId = ", endId
cs_result = ContactSequenceHumanoid(cs)
def getPhaseDuration(sid, pid):
if sid == endId:
duration = cfg.DURATION_FINAL
if pid == 0:
duration = cfg.DURATION_INIT
elif cs.contact_phases[pid].numActivePatches() == 1:
duration = cfg.DURATION_SS
elif cs.contact_phases[pid].numActivePatches() == 2:
duration = cfg.DURATION_DS
elif cs.contact_phases[pid].numActivePatches() == 3:
duration = cfg.DURATION_TS
elif cs.contact_phases[pid].numActivePatches() == 4:
duration = cfg.DURATION_QS
return duration
# for each phase in the cs, create a corresponding FullBody State and call CROC,
# then discretize the solution and fill the cs struct
# Make the assumption that the CS was created with the generateContactSequence method from the same fb object
id_phase = 0
for id_state in range(beginId, endId + 1):
print "id_state = ", str(id_state)
print "id_phase = ", str(id_phase)
phase_fixed = cs_result.contact_phases[
id_phase] # phase where the CoM move and the contacts are fixed
# set initial state to be the final one of the previous phase :
if id_phase > 1:
phase_fixed.init_state = phase_swing.final_state.copy()
current_t = phase_swing.time_trajectory[-1]
else:
current_t = 0.
# compute 'optimal' position of the COM to go before switching phase:
if id_state == endId:
c = phase_fixed.final_state[0:3]
else:
c = getTargetCOMPosition(fullBody, id_state)
# fill phase trajectories to connect to 'c'
moveToCOMPosition(phase_fixed, c, current_t,
getPhaseDuration(id_state, id_phase))
id_phase += 1
if id_state < endId:
current_t = phase_fixed.time_trajectory[-1]
phase_swing = cs_result.contact_phases[
id_phase] # phase where the CoM is fixed and an effector move
# in swing phase, com do not move :
phase_swing.init_state = phase_fixed.final_state.copy()
phase_swing.final_state = phase_fixed.final_state.copy()
fillPhaseTrajWithZeros(phase_swing, current_t,
getPhaseDuration(id_state, id_phase))
id_phase += 1
return cs_result
def generateCentroidalTrajectory(cs,cs_initGuess = None, fullBody = None, viewer = None):
cs_res = ContactSequenceHumanoid(0)
filename = cfg.CONTACT_SEQUENCE_PATH + "/"+cfg.DEMO_NAME+"_COM.cs"
cs_res.loadFromBinary(filename)
print "Import contact sequence binary file : ",filename
return cs_res