Commit 8bc0f1e4 authored by Pierre Fernbach's avatar Pierre Fernbach
Browse files

[tool] add/update tools script

parent ed054fb4
......@@ -139,6 +139,8 @@ install(FILES
data/urdf/flat_step.urdf
data/urdf/flat_hole.urdf
data/urdf/cross_gap.urdf
data/urdf/floor_bauzil.urdf
data/urdf/floor_bauzil_obstacles.urdf
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/urdf
)
install(FILES
......@@ -215,6 +217,8 @@ install(FILES
data/srdf/flat_step.srdf
data/srdf/flat_hole.srdf
data/srdf/cross_gap.srdf
data/srdf/floor_bauzil.srdf
data/srdf/floor_bauzil_obstacles.srdf
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/srdf
)
install(FILES
......@@ -295,6 +299,8 @@ install(FILES
data/meshes/flat_hole.stl
data/meshes/cross_gap.stl
data/meshes/cross_gap2.stl
data/meshes/floor_bauzil.stl
data/meshes/floor_bauzil_obstacles.stl
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/meshes
)
......
......@@ -771,7 +771,7 @@ module hpp
boolean areKinematicsConstraintsVerifiedForState(in unsigned short stateFrom,in floatSeq point)raises (Error);
boolean isReachableFromState(in unsigned short stateFrom, in unsigned short stateTo)raises (Error);
floatSeq isReachableFromState(in unsigned short stateFrom, in unsigned short stateTo)raises (Error);
floatSeq isDynamicallyReachableFromState(in unsigned short stateFrom, in unsigned short stateTo,in boolean addPathPerPhase, in floatSeq timings,in short numPointsPerPhase)raises (Error);
......
......@@ -7,7 +7,7 @@ from hpp.corbaserver.rbprm.rbprmstate import State,StateHelper
#from display_tools import *
import talos.flatGround_pyrene_pathKino as tp
import time
from planning.robot_config.talos import *
from robot_config.talos import *
tPlanning = tp.tPlanning
......@@ -156,7 +156,7 @@ print "number of configs :", len(configsFull)
from planning.configs.talos_flatGround import *
from configs.talos_flatGround import *
from generate_contact_sequence import *
beginState = 0
......
......@@ -4,7 +4,7 @@ from hpp.corbaserver import Client
from hpp.corbaserver.robot import Robot as Parent
from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
import omniORB.any
from planning.configs.straight_walk_config import *
from configs.talos_flatGround import *
import time
class Robot (Parent):
......
from stair_bauzil_hrp2_interp import *
import generate_muscod_problem as mp
from pathlib2 import Path
import muscodSSH as ssh
tryWarmStart = True
print "run bench with feasibility criterion"
com = fullBody.getCenterOfMass()
if com[0] > 1.25 :
success = True
else :
success = False
numConf = len(configs)
if success :
# muscod without warm start :
if Path(CONTACT_SEQUENCE_WHOLEBODY_FILE).is_file():
os.remove(CONTACT_SEQUENCE_WHOLEBODY_FILE)
filename_xml = OUTPUT_DIR + "/" + OUTPUT_SEQUENCE_FILE
mp.generate_muscod_problem(filename_xml,True)
successMuscod,ssh_ok = ssh.call_muscod()
time.sleep(5.) # wait for sync of the ~/home (worst case, usually 0.1 is enough ... )
muscodConverged = successMuscod and Path(CONTACT_SEQUENCE_WHOLEBODY_FILE).is_file()
if tryWarmStart :
if not success :
# generate warm start from planning :
if Path(CONTACT_SEQUENCE_WHOLEBODY_FILE).is_file():
os.remove(CONTACT_SEQUENCE_WHOLEBODY_FILE)
filename_xml = OUTPUT_DIR + "/" + OUTPUT_SEQUENCE_FILE
cs = generateContactSequenceWithInitGuess(ps,fullBody,configs,beginState,endState)
cs.saveAsXML(filename, "ContactSequence")
mp.generate_muscod_problem(filename_xml,True)
successMuscod,ssh_ok = ssh.call_muscod()
time.sleep(5.) # wait for sync of the ~/home (worst case, usually 0.1 is enough ... )
muscodWarmStartConverged = successMuscod and Path(CONTACT_SEQUENCE_WHOLEBODY_FILE).is_file()
else :
muscodWarmStartConverged= True
else :
muscodWarmStartConverged = False
else :
muscodConverged = False
muscodWarmStartConverged = False
tInterpolateConfigs = 0.
numConf = 0.
name = "/local/fernbac/bench_iros18/bench_contactGeneration/stairsMC_croc.log"
f = open(name,"a")
f.write("new\n")
f.write("success "+str(success)+"\n")
f.write("muscodNoWarmStart "+str(muscodConverged)+"\n")
f.write("muscodWarmStart "+str(muscodWarmStartConverged)+"\n")
f.write("time "+str(tInterpolateConfigs)+"\n")
f.write("configs "+str(numConf)+"\n")
f.close()
from stair_bauzil_hrp2_interp import *
import generate_muscod_problem as mp
from pathlib2 import Path
import muscodSSH as ssh
tryWarmStart = False
print "run bench without feasibility criterion"
com = fullBody.getCenterOfMass()
if com[0] > 1.3:
success = True
else :
success = False
#success = False
numConf = len(configs)
if success :
# muscod without warm start :
if Path(CONTACT_SEQUENCE_WHOLEBODY_FILE).is_file():
os.remove(CONTACT_SEQUENCE_WHOLEBODY_FILE)
filename_xml = OUTPUT_DIR + "/" + OUTPUT_SEQUENCE_FILE
mp.generate_muscod_problem(filename_xml,True)
successMuscod,ssh_ok = ssh.call_muscod()
time.sleep(5.) # wait for sync of the ~/home (worst case, usually 0.1 is enough ... )
muscodConverged = successMuscod and Path(CONTACT_SEQUENCE_WHOLEBODY_FILE).is_file()
crocConverged = True
for id_state in range(beginState,endState):
pid = fullBody.isDynamicallyReachableFromState(id_state,id_state+1,numPointsPerPhases=0)
if len(pid) == 0:
print "Croc did not converge for state "+str(id_state)
crocConverged = False
else :
crocConverged = False
muscodConverged = False
tInterpolateConfigs = 0.
numConf = 0.
name = "/local/fernbac/bench_iros18/bench_contactGeneration/stairsMC_noCroc.log"
f = open(name,"a")
f.write("new\n")
f.write("success "+str(success)+"\n")
f.write("muscodNoWarmStart "+str(muscodConverged)+"\n")
f.write("crocConverged "+str(crocConverged)+"\n")
f.write("time "+str(tInterpolateConfigs)+"\n")
f.write("configs "+str(numConf)+"\n")
f.close()
from hpp.corbaserver.rbprm.rbprmstate import State,StateHelper
from disp_bezier import *
max_acc = 1
pointsPerPhase=4
import numpy as np
max_acc = 5.
pointsPerPhase=5
ROUND = 1000.
global curves_initGuess
......@@ -10,6 +11,14 @@ global timings_initGuess
curves_initGuess = []
timings_initGuess = []
def stdVecToMatrix(std_vector):
vec_l = []
for vec in std_vector:
vec_l.append(vec)
res = np.hstack(tuple(vec_l))
return res
def compute_time_array(t_qp,t_c):
......@@ -45,57 +54,70 @@ def check_acceleration_bounds(ddc_qp,t_discretized):
print "acceleration between : "+str(min)+" ; "+str(max)
return True;
def check_projection(s,c,dc,ddc,t):
def check_projection(s,c,t):
return True
success = s.projectToCOM(c(t).transpose().tolist()[0],500)
if not success :
print "Unable to project to com at time : "+str(t)
return False
# check dynamic stability
else:
return True
def check_stability(s,c,dc,ddc,t):
q = s.q()
q[-6:-3] = dc(t).transpose().tolist()[0]
q[-3:] = ddc(t).transpose().tolist()[0]
success = s.fullBody.isConfigBalanced(q,s.getLimbsInContact())
success = s.fullBody.isConfigBalanced(q,s.getLimbsInContact(),CoM = c(t).transpose().tolist()[0])
if not success :
print "UNSTABLE at time : "+str(t)
return False
return True
#print "q = ",q
return False
else :
return True
def check_projection_path(s0,s1,c,dc,ddc,t_qp,t_per_phase):
def check_projection_path(s0,s1,c,dc,ddc,t_per_phase):
kin_valid = True
stab_valid = True
moving_limb = s0.contactsVariations(s1)
s0_orig = State(s0.fullBody,q=s0.q(),limbsIncontact=s0.getLimbsInContact())
s1_orig = State(s1.fullBody,q=s1.q(),limbsIncontact=s1.getLimbsInContact())
if len(moving_limb)>1:
print "Too many contact variation between adjacent states"
return False
return False,False
if len(moving_limb)==0:
print "No contact between adjacent states"
return True
print "test for phase 0 :"
return True,True
#print "test for phase 0 :"
for t in t_per_phase[0]:
if not check_projection(s0,c,dc,ddc,t):
return False
if kin_valid and not check_projection(s0,c,t):
kin_valid = False
if stab_valid and not check_stability(s0_orig,c,dc,ddc,t):
stab_valid = False
smid,success = StateHelper.removeContact(s0,moving_limb[0])
if not success:
smid_orig,success_orig = StateHelper.removeContact(s0_orig,moving_limb[0])
if not (success and success_orig):
print "Error in creation of intermediate state"
return False
print "test for phase 1 :"
return False,False
#print "test for phase 1 :"
for t in t_per_phase[1]:
#if t == t_per_phase[0][-1]:
# t -= EPS
if not check_projection(smid,c,dc,ddc,t):
return False
print "test for phase 2"
if kin_valid and not check_projection(smid,c,t):
kin_valid = False
if stab_valid and not check_stability(smid_orig,c,dc,ddc,t):
stab_valid = False
#print "test for phase 2"
# go in reverse order for p2, it's easier for the projection :
for i in range(1,len(t_per_phase[2])+1):
t = t_per_phase[1][-i]
if not check_projection(s1,c,dc,ddc,t):
return False
t = t_per_phase[1][-1]
if not check_projection(s1,c,dc,ddc,t):
return False
return True
t = t_per_phase[2][-i]
if kin_valid and not check_projection(s1,c,t):
kin_valid = False
if stab_valid and not check_stability(s1_orig,c,dc,ddc,t):
stab_valid = False
return kin_valid,stab_valid
def check_one_transition(ps,fullBody,s0,s1,r=None,pp=None):
global curves_initGuess
......@@ -116,14 +138,96 @@ def check_one_transition(ps,fullBody,s0,s1,r=None,pp=None):
print "### test acceleration bounds ###"
valid = valid and check_acceleration_bounds(ddc_qp,t_discretized)
print "### test kinematic projection ###"
valid = valid and check_projection_path(s0,s1,c_qp,dc_qp,ddc_qp,t_qp,t_per_phase)
valid = valid and check_projection_path(s0,s1,c_qp,dc_qp,ddc_qp,t_per_phase)
curves_initGuess.append(c_qp)
timings_initGuess.append(t_qp)
return valid
def genTimeArray(Ts,dt):
t = 0.
res = []
current_t = 0.
for phase_id in range(len(Ts)):
t_phase = []
while t <= (current_t + Ts[phase_id]):
t_phase += [t]
t += dt
res += [t_phase]
current_t += Ts[phase_id]
return res
def check_traj_valid(ps,fullBody,s0_,s1_,pIds,dt=0.001):
# create copy of original states :
s0 = State(fullBody,q=s0_.q(),limbsIncontact=s0_.getLimbsInContact())
s1 = State(fullBody,q=s1_.q(),limbsIncontact=s1_.getLimbsInContact())
fullBody.setStaticStability(False)
Ts = [ps.pathLength(int(pIds[1])), ps.pathLength(int(pIds[2])), ps.pathLength(int(pIds[3]))]
t_array = genTimeArray(Ts,dt)
#print "Time array : ",t_array
c_qp = fullBody.getPathAsBezier(int(pIds[0]))
dc_qp = c_qp.compute_derivate(1)
ddc_qp = dc_qp.compute_derivate(1)
return check_projection_path(s0,s1,c_qp,dc_qp,ddc_qp,t_array)
def check_muscod_traj(fullBody,cs,s0_,s1_):
if cs.size() != 3 :
print "Contact sequence must be of size 3 (one step)"
return False,False
kin_valid = True
stab_valid = True
stab_valid_discretized = True
# create copy of original states :
s0 = State(fullBody,q=s0_.q(),limbsIncontact=s0_.getLimbsInContact())
s1 = State(fullBody,q=s1_.q(),limbsIncontact=s1_.getLimbsInContact())
moving_limb = s0.contactsVariations(s1)
smid,success = StateHelper.removeContact(s0,moving_limb[0])
if not success:
print "Error in creation of intermediate state"
return False,False
phases = []
c_array = []
dc_array = []
ddc_array = []
t_array = []
states = [s0,smid,s1]
states_copy = [State(fullBody,q=s0_.q(),limbsIncontact=s0_.getLimbsInContact()), State(fullBody,q=smid.q(),limbsIncontact=smid.getLimbsInContact()), State(fullBody,q=s1_.q(),limbsIncontact=s1_.getLimbsInContact()) ]
for k in range(3) :
phases += [cs.contact_phases[k]]
state_traj = stdVecToMatrix(phases[k].state_trajectory).transpose()
control_traj = stdVecToMatrix(phases[k].control_trajectory).transpose()
c_array = state_traj[:,:3]
dc_array = state_traj[:,3:6]
ddc_array = control_traj[:,:3]
t_array = stdVecToMatrix(phases[k].time_trajectory).transpose()
for i in range(len(c_array)):
#if kin_valid and not states[k].projectToCOM(c_array[i].tolist()[0],500) :
# kin_valid = False
q = states_copy[k].q()
q[-6:-3] = dc_array[i].tolist()[0]
q[-3:] = ddc_array[i].tolist()[0]
if stab_valid_discretized and not states_copy[k].fullBody.isConfigBalanced(q,states_copy[k].getLimbsInContact(),CoM = c_array[i].tolist()[0]) :
stab_valid_discretized = False
for i in range(int(len(c_array)/100)):
q = states_copy[k].q()
q[-6:-3] = dc_array[i*100].tolist()[0]
q[-3:] = ddc_array[i*100].tolist()[0]
if stab_valid and not states_copy[k].fullBody.isConfigBalanced(q,states_copy[k].getLimbsInContact(),CoM = c_array[i*100].tolist()[0]) :
stab_valid = False
return kin_valid,stab_valid, stab_valid_discretized
def check_contact_plan(ps,r,pp,fullBody,idBegin,idEnd):
fullBody.client.basic.robot.setExtraConfigSpaceBounds([-0,0,-0,0,-0,0,0,0,0,0,0,0])
......@@ -166,4 +270,12 @@ valid = True
t_qp = [ps.pathLength(int(pid[1])), ps.pathLength(int(pid[2])), ps.pathLength(int(pid[3]))]
t_discretized,t_per_phase = compute_time_array(t_qp)
"""
\ No newline at end of file
"""
"""
cs = ContactSequenceHumanoid(0)
cs.loadFromXML('/home/pfernbac/Documents/muscod/uc-dual/PROBLEM/random/res/contact_sequence_trajectory.xml',CONTACT_SEQUENCE_XML_TAG)
"""
......@@ -5,11 +5,11 @@ from pinocchio.utils import *
import locomote
from locomote import WrenchCone,SOC6,ControlType,IntegratorType,ContactPatch, ContactPhaseHumanoid, ContactSequenceHumanoid
import planning.generate_muscod_problem as mp
import generate_muscod_problem as mp
import muscodSSH as ssh
from planning.config import *
from config import *
statesPerStep=4 # number of double support configs from the planning per call to muscod
statesPerStep=5 # number of double support configs from the planning per call to muscod
stepSize=statesPerStep*2 - 1 # contact_sequence contain double support AND simple support states
def solveMuscodProblem(configsFull,cs):
......
name = 's2'
gui.addSphere(name,0.01,[1,0,0,1])
gui.setVisibility(name,"ALWAYS_ON_TOP")
gui.addToGroup(name,"world")
gui.applyConfiguration(name,p)
gui.refresh()
gui=r.client.gui
name = 's2'
gui.addSphere(name,0.01,[1,0,0,1])
gui.setVisibility(name,"ALWAYS_ON_TOP")
gui.addToGroup(name,r.sceneName)
gui.applyConfiguration(name,p)
gui.refresh()
......@@ -4,7 +4,6 @@ from hpp.gepetto import Viewer
from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
import numpy as np
#from hpp.corbaserver.rbprm.tools.cwc_trajectory import *
from hpp import Error as hpperr
from numpy import array, matrix
from hpp.corbaserver.rbprm.rbprmstate import State,StateHelper
......@@ -21,7 +20,7 @@ srdfSuffix = ""
fullBody = FullBody ()
fullBody.loadFullBodyModel(urdfName, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
fullBody.setJointBounds ("base_joint_xyz", [-0.3,0.3, -0.3, 0.3, 0.6, 0.7])
fullBody.setJointBounds ("base_joint_xyz", [-0.7,0.7, -0.7, 0.7, 0.6, 0.7])
fullBody.setJointBounds ("CHEST_JOINT0", [0.,0.])
fullBody.setJointBounds ("CHEST_JOINT1", [0.,0.])
fullBody.setJointBounds ("HEAD_JOINT0", [0.,0.])
......@@ -61,7 +60,8 @@ rLegLimbOffset=[0,0,-0.035]#0.035
rLegNormal = [0,0,1]
rLegx = 0.09; rLegy = 0.05
#fullBody.addLimbDatabase("./db/hrp2_rleg_db.db",rLegId,"forward")
fullBody.addLimb(rLegId,rLeg,'',rLegOffset,rLegNormal, rLegx, rLegy, 100000, "manipulability", 0.01,"_6_DOF",limbOffset=rLegLimbOffset)
fullBody.addLimb(rLegId,rLeg,'',rLegOffset,rLegNormal, rLegx, rLegy, 100000, "manipulability", 0.01,"_6_DOF",kinematicConstraintsPath = "package://hpp-rbprm-corba/com_inequalities/fullSize/RLEG_JOINT0_com_constraints.obj",limbOffset=rLegLimbOffset,kinematicConstraintsMin=0.2)
#fullBody.addLimb(rLegId,rLeg,'',rLegOffset,rLegNormal, rLegx, rLegy, 100000, "manipulability", 0.01,"_6_DOF",kinematicConstraintsPath = "package://hpp-rbprm-corba/com_inequalities/empty_com_constraints.obj",limbOffset=rLegLimbOffset,kinematicConstraintsMin=0.2)
fullBody.runLimbSampleAnalysis(rLegId, "ReferenceConfiguration", True)
#fullBody.saveLimbDatabase(rLegId, "./db/hrp2_rleg_db.db")
......@@ -71,11 +71,14 @@ lLegLimbOffset=[0,0,0.035]
lLegNormal = [0,0,1]
lLegx = 0.09; lLegy = 0.05
#fullBody.addLimbDatabase("./db/hrp2_lleg_db.db",lLegId,"forward")
fullBody.addLimb(lLegId,lLeg,'',lLegOffset,rLegNormal, lLegx, lLegy, 100000, "manipulability", 0.01,"_6_DOF",limbOffset=lLegLimbOffset)
fullBody.addLimb(lLegId,lLeg,'',lLegOffset,rLegNormal, lLegx, lLegy, 100000, "manipulability", 0.01,"_6_DOF",kinematicConstraintsPath = "package://hpp-rbprm-corba/com_inequalities/fullSize/LLEG_JOINT0_com_constraints.obj",limbOffset=lLegLimbOffset,kinematicConstraintsMin=0.2)
fullBody.runLimbSampleAnalysis(lLegId, "ReferenceConfiguration", True)
#fullBody.saveLimbDatabase(lLegId, "./db/hrp2_lleg_db.db")
rleg = 'RLEG_JOINT0'
rfoot = "RLEG_JOINT5"
lleg = 'LLEG_JOINT0'
lfoot = "LLEG_JOINT5"
limbsCOMConstraints = { rLegId : {'file': "hrp2/RL_com.ineq", 'effector' : 'RLEG_JOINT5'},
lLegId : {'file': "hrp2/LL_com.ineq", 'effector' : 'LLEG_JOINT5'},
......@@ -223,21 +226,35 @@ def _feet_far_enough(fullBody,q):
for i in range(3):
rd += (c[i]-rf[i])*(c[i]-rf[i])
ld += (c[i]-lf[i])*(c[i]-lf[i])
if rd < 0.2 or ld < 0.2:
if rd < 0.35 or ld < 0.35:
return False
else:
return True
def projectMidFeet(fullBody,q,limbs):
fullBody.setCurrentConfig(q)
s = State(fullBody,q=q,limbsIncontact=limbs)
com = np.array(fullBody.getJointPosition(rfoot)[0:3])
com += np.array(fullBody.getJointPosition(lfoot)[0:3])
com /= 2.
com[2] = fullBody.getCenterOfMass()[2]
successProj = s.projectToCOM(com.tolist())
if successProj and fullBody.isConfigValid(s.q())[0] and fullBody.isConfigBalanced(s.q(), limbs, 5) :
return s
else :
return None
def _genbalance(limbs):
def _genbalance(fullBody,limbs):
for i in range(10000):
q = fullBody.client.basic.robot.shootRandomConfig()
q[3:7] = [1,0,0,0]
#q = _boundSO3(q, len(limbs))
if fullBody.isConfigValid(q)[0] and fullBody.isConfigBalanced(q, limbs, 5) and __loosely_z_aligned(limbs[0], q) and __loosely_z_aligned(limbs[1], q) and _feet_far_enough(fullBody,q):
#~ if fullBody.isConfigValid(q)[0] and __loosely_z_aligned(limbs[0], q) and __loosely_z_aligned(limbs[1], q):
return q
fullBody.setCurrentConfig(q)
#r(q)
if __loosely_z_aligned(limbs[0], q) and __loosely_z_aligned(limbs[1], q) and _feet_far_enough(fullBody,q) and fullBody.isConfigValid(q)[0] and fullBody.isConfigBalanced(q, limbs, 5):
return q
print "can't generate equilibrium config"
return []
all_qs = []
def genStates(fullbody,limbs, num_samples = 1000, coplanar = True):
......@@ -249,10 +266,11 @@ def genStates(fullbody,limbs, num_samples = 1000, coplanar = True):
if(coplanar):
q = fullBody.generateGroundContact(limbs)
else:
q = _genbalance(limbs)
qs.append(q)
s = State(fullbody,q=q,limbsIncontact=limbs)
states.append(s)
q = _genbalance(fullBody,limbs)
if len(q)>1 :
qs.append(q)
s = State(fullbody,q=q,limbsIncontact=limbs)
states.append(s)
return states
def genStateWithOneStep(fullbody,limbs, num_samples = 100, coplanar = True):
......@@ -262,35 +280,50 @@ def genStateWithOneStep(fullbody,limbs, num_samples = 100, coplanar = True):
states = []
assert(len(limbs) == 2 and "only implemented for 2 limbs in contact for now")
it = 0
while len(states) < num_samples and it < 5000:
while len(states) < num_samples and it < 10000:
i = len(states)
if(coplanar):
q0 = fullBody.generateGroundContact(limbs)
else:
q0 = _genbalance(limbs)
s0 = State(fullbody,q=q0,limbsIncontact=limbs)
success = False
iter = 0
# try to make a step
while not success and iter < 100 :
if(coplanar):
q2 = fullBody.generateGroundContact(limbs)
else:
q2 = _genbalance(limbs)
s2 = State(fullbody,q=q2,limbsIncontact=limbs)
moving_limb = limbs[i%2]
[p,n]= s2.getCenterOfContactForLimb(moving_limb) # get position of the moving limb in the next state
s1,success = StateHelper.addNewContact(s0,moving_limb,p,n) # try to move the limb for s0 to it's position in s2
if success:
success = fullBody.isConfigValid(s1.q())[0] and fullBody.isConfigBalanced(s1.q(), limbs, 5) and __loosely_z_aligned(limbs[0], s1.q()) and __loosely_z_aligned(limbs[1], s1.q())
iter += 1
if success:
# recreate the states to assure the continuity of the index in fullBody :
state0 = State(fullBody,q=s0.q(),limbsIncontact=limbs)
states += [[s1,state0]]
print "Step "+str(i)+" done."
else :
print "cannot make the step."
q0 = _genbalance(fullBody,limbs)
if len(q0)> 1 :
s0 = projectMidFeet(fullBody,q0,limbs)