Skip to content
Snippets Groups Projects
Commit dee0a8ed authored by t steve's avatar t steve
Browse files

working down

parent 96419b2f
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
script/scenarios/demos/siggraph_asia/bezier_traj.py
+ 2
− 2
View file @ dee0a8ed
......@@ -52,11 +52,11 @@ def test(stateid = 1, path = False, use_rand = False, just_one_curve = False, nu
com_2 = __get_com(fullBody, configs[stateid+1])
createPtBox(viewer.client.gui, 0, com_1, 0.01, [0,1,1,1.])
createPtBox(viewer.client.gui, 0, com_2, 0.01, [0,1,1,1.])
data = gen_sequence_data_from_state(fullBody,stateid,configs, mu = 0.3)
data = gen_sequence_data_from_state(fullBody,stateid,configs, mu = 0.5)
c_bounds_1 = get_com_constraint(fullBody, stateid, configs[stateid], limbsCOMConstraints, interm = False)
c_bounds_mid = get_com_constraint(fullBody, stateid, configs[stateid], limbsCOMConstraints, interm = True)
c_bounds_2 = get_com_constraint(fullBody, stateid, configs[stateid+1], limbsCOMConstraints, interm = False)
success, c_mid_1, c_mid_2 = solve_quasi_static(data, c_bounds = [c_bounds_1, c_bounds_2, c_bounds_mid], use_rand = use_rand, mu = 0.3)
success, c_mid_1, c_mid_2 = solve_quasi_static(data, c_bounds = [c_bounds_1, c_bounds_2, c_bounds_mid], use_rand = use_rand, mu = 0.5)
#~ success, c_mid_1, c_mid_2 = solve_dyn(data, c_bounds = [c_bounds_1, c_bounds_2, c_bounds_mid], use_rand = use_rand)
#~ success, c_mid_1, c_mid_2 = solve_dyn(data, c_bounds = [c_bounds_1, c_bounds_2])
......
script/scenarios/demos/siggraph_asia/down_hrp2_interp.py
+ 23
− 338
View file @ dee0a8ed
from hpp.corbaserver.rbprm.rbprmbuilder import Builder
from hpp.corbaserver.rbprm.rbprmfullbody import FullBody
from hpp.gepetto import Viewer
from hpp.gepetto import PathPlayer
import down_hrp2_path as tp
import down_hrp2_path as path_planner
import hrp2_model_grasp as model
from hrp2_model import *
import time
ps = path_planner.ProblemSolver( model.fullBody )
r = path_planner.Viewer (ps, viewerClient=path_planner.r.client)
fullBody = model.fullBody
fullBody.setJointBounds ("base_joint_xyz", [-3,3, -2, 2, 0, 1])
pp = PathPlayer (fullBody.client.basic, r)
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,3, -0.5, 0.5, -0.4, 0.6])
ps = tp.ProblemSolver( fullBody )
r = tp.Viewer (ps, viewerClient=tp.r.client)
#~ 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, 10000, "manipulability", 0.1)
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, "manipulability", 0.1)
rarmId = '3Rarm'
rarm = 'RARM_JOINT0'
rHand = 'RARM_JOINT5'
rArmOffset = [0,0,-0.1]
rArmNormal = [0,0,1]
rArmx = 0.024; rArmy = 0.024
#disabling collision for hook
fullBody.addLimb(rarmId,rarm,rHand,rArmOffset,rArmNormal, rArmx, rArmy, 10000, "manipulability", 0.05, "_6_DOF", True)
#~ 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.runLimbSampleAnalysis(rLegId, "jointLimitsDistance", True)
#~ fullBody.runLimbSampleAnalysis(lLegId, "jointLimitsDistance", True)
#~ fullBody.client.basic.robot.setJointConfig('LARM_JOINT0',[1])
#~ fullBody.client.basic.robot.setJointConfig('RARM_JOINT0',[-1])
from plan_execute import a, b, c, d, e, init_plan_execute
init_plan_execute(model.fullBody, r, path_planner, pp)
q_0 = fullBody.getCurrentConfig();
#~ fullBody.createOctreeBoxes(r.client.gui, 1, rarmId, q_0,)
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]
q_init = fullBody.getCurrentConfig(); q_init[0:7] = path_planner.q_init[0:7]
q_goal = fullBody.getCurrentConfig(); q_goal[0:7] = path_planner.q_goal[0:7]
fullBody.setCurrentConfig (q_init)
q_init = [
0.2,0, 0.48, 1.0, 0.0 , 0.0, 0.0, # Free flyer 0-6
0.2,0, 0.6, 1.0, 0.0 , 0.0, 0.0, # Free flyer 0-6
0.0, 0.0, 0.0, 0.0, # CHEST HEAD 7-10
0.261799388, 0.174532925, 0.0, -0.523598776, 0.0, 0.0, 0.17, # LARM 11-17
0.261799388, -0.174532925, 0.0, -0.523598776, 0.0, 0.0, 0.17, # RARM 18-24
......@@ -98,273 +33,23 @@ q_init = [
0.0, 0.0, -0.453785606, 0.872664626, -0.41887902, 0.0, # RLEG 31-36
]; r (q_init)
fullBody.setCurrentConfig (q_goal)
#~ r(q_goal)
q_goal = fullBody.generateContacts(q_goal, [0,0,1])
q_init = fullBody.generateContacts(q_init, [0,0,1])
#~ r(q_goal)
#~ fullBody.setStartState(q_init,[rLegId,lLegId,rarmId]) #,rarmId,larmId])
#~ fullBody.setStartState(q_init,[rLegId,lLegId,larmId, rarmId]) #,rarmId,larmId])
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.15)
i = 0;
configs = []
limbsCOMConstraints = { rLegId : {'file': "hrp2/RL_com.ineq", 'effector' : 'RLEG_JOINT5'},
lLegId : {'file': "hrp2/LL_com.ineq", 'effector' : 'LLEG_JOINT5'},
rarmId : {'file': "hrp2/RA_com.ineq", 'effector' : rHand} }
#~ larmId : {'file': "hrp2/LA_com.ineq", 'effector' : lHand} }
#~ fullBody.limbRRTFromRootPath(0,len(configs)-1,0,2)
from hpp.corbaserver.rbprm.tools.cwc_trajectory_helper import step, clean,stats, saveAllData, play_traj
from hpp.gepetto import PathPlayer
pp = PathPlayer (fullBody.client.basic, r)
def act(i, numOptim = 0, use_window = 0, friction = 0.5, optim_effectors = True, verbose = False, draw = False):
return step(fullBody, configs, i, numOptim, pp, limbsCOMConstraints, 0.4, optim_effectors = optim_effectors, time_scale = 20., useCOMConstraints = True, use_window = use_window,
verbose = verbose, draw = draw)
configs = d(0.005); e()
def play(frame_rate = 1./24.):
play_traj(fullBody,pp,frame_rate)
def saveAll(name):
saveAllData(fullBody, r, name)
def initConfig():
r.client.gui.setVisibility("hrp2_14", "ON")
tp.cl.problem.selectProblem("default")
tp.r.client.gui.setVisibility("toto", "OFF")
tp.r.client.gui.setVisibility("hrp2_trunk_flexible", "OFF")
r(q_init)
def endConfig():
r.client.gui.setVisibility("hrp2_14", "ON")
tp.cl.problem.selectProblem("default")
tp.r.client.gui.setVisibility("toto", "OFF")
tp.r.client.gui.setVisibility("hrp2_trunk_flexible", "OFF")
r(q_goal)
def rootPath():
tp.cl.problem.selectProblem("rbprm_path")
r.client.gui.setVisibility("hrp2_14", "OFF")
tp.r.client.gui.setVisibility("toto", "OFF")
r.client.gui.setVisibility("hyq", "OFF")
r.client.gui.setVisibility("hrp2_trunk_flexible", "ON")
tp.pp(0)
r.client.gui.setVisibility("hrp2_trunk_flexible", "OFF")
r.client.gui.setVisibility("hyq", "ON")
tp.cl.problem.selectProblem("default")
def genPlan(stepsize=0.1):
r.client.gui.setVisibility("hrp2_14", "ON")
tp.cl.problem.selectProblem("default")
tp.r.client.gui.setVisibility("toto", "OFF")
tp.r.client.gui.setVisibility("hrp2_trunk_flexible", "OFF")
global configs
start = time.clock()
print "BEFORE"
configs = fullBody.interpolate(stepsize, 0, 2, True)
print "AFTER"
end = time.clock()
print "Contact plan generated in " + str(end-start) + "seconds"
def contactPlan(step = 0.5):
r.client.gui.setVisibility("hrp2_14", "ON")
tp.cl.problem.selectProblem("default")
tp.r.client.gui.setVisibility("toto", "OFF")
tp.r.client.gui.setVisibility("hrp2_trunk_flexible", "OFF")
for i in range(0,len(configs)):
r(configs[i]);
time.sleep(step)
def a():
print "initial configuration"
initConfig()
def b():
print "end configuration"
endConfig()
def c():
print "displaying root path"
rootPath()
def d(step=0.1):
print "computing contact plan"
genPlan(step)
def e(step = 0.5):
print "displaying contact plan"
contactPlan(step)
print "Root path WXXSD in " + str(tp.t) + " ms."
d(0.005); e()
print "Root path SDDSD in " + str(tp.t) + " ms."
#~ from gen_data_from_rbprm import *
#~
#~ for config in configs:
#~ r(config)
#~ print(fullBody.client.basic.robot.getComPosition())
#~
#~ gen_and_save(fullBody,configs, "stair_bauzil_contacts_data")
#~ main()
from gen_data_from_rbprm import *
from hpp.corbaserver.rbprm.tools.com_constraints import get_com_constraint
#computing com bounds 0 and 1
def __get_com(robot, config):
save = robot.getCurrentConfig()
robot.setCurrentConfig(config)
com = robot.getCenterOfMass()
robot.setCurrentConfig(save)
return com
from numpy import matrix, asarray
from numpy.linalg import norm
from spline import bezier
def __curveToWps(curve):
return asarray(curve.waypoints().transpose()).tolist()
def __Bezier(wps, init_acc = [0.,0.,0.], end_acc = [0.,0.,0.], init_vel = [0.,0.,0.], end_vel = [0.,0.,0.]):
c = curve_constraints();
c.init_vel = matrix(init_vel);
c.end_vel = matrix(end_vel);
c.init_acc = matrix(init_acc);
c.end_acc = matrix(end_acc);
matrix_bezier = matrix(wps).transpose()
return __curveToWps(bezier(matrix_bezier, c))
#~ return __curveToWps(bezier(matrix_bezier))
allpaths = []
def play_all_paths():
for _, pid in enumerate(allpaths):
pp(pid)
def play_all_paths_smooth():
for i, pid in enumerate(allpaths):
if i % 2 == 1 :
pp(pid)
def play_all_paths_qs():
for i, pid in enumerate(allpaths):
if i % 2 == 0 :
pp(pid)
def test(stateid = 1, path = False, use_rand = False, just_one_curve = False) :
com_1 = __get_com(fullBody, configs[stateid])
com_2 = __get_com(fullBody, configs[stateid+1])
data = gen_sequence_data_from_state(fullBody,stateid,configs)
c_bounds_1 = get_com_constraint(fullBody, stateid, configs[stateid], limbsCOMConstraints, interm = False)
c_bounds_mid = get_com_constraint(fullBody, stateid, configs[stateid], limbsCOMConstraints, interm = True)
c_bounds_2 = get_com_constraint(fullBody, stateid, configs[stateid+1], limbsCOMConstraints, interm = False)
success, c_mid_1, c_mid_2 = solve_quasi_static(data, c_bounds = [c_bounds_1, c_bounds_2, c_bounds_mid], use_rand = use_rand)
#~ success, c_mid_1, c_mid_2 = solve_dyn(data, c_bounds = [c_bounds_1, c_bounds_2, c_bounds_mid], use_rand = use_rand)
#~ success, c_mid_1, c_mid_2 = solve_dyn(data, c_bounds = [c_bounds_1, c_bounds_2])
paths_ids = []
if path and success:
#~ fullBody.straightPath([c_mid_1[0].tolist(),c_mid_2[0].tolist()])
#~ fullBody.straightPath([c_mid_2[0].tolist(),com_2])
if just_one_curve:
bezier_0 = __Bezier([com_1,c_mid_1[0].tolist(),c_mid_2[0].tolist(),com_2])
p0 = fullBody.generateCurveTrajParts(bezier_0,[0.,0.1,0.9,1.])
print "p0", p0
#~ pp.displayPath(p0+1)
#~ pp.displayPath(p0+2)
pp.displayPath(p0)
paths_ids = [int(el) for el in fullBody.comRRTFromPos(stateid,p0+1,p0+2,p0+3)]
else:
bezier_0 = __Bezier([com_1,c_mid_1[0].tolist()] , end_acc = c_mid_1[1].tolist() , end_vel = [0.,0.,0.])
bezier_1 = __Bezier([c_mid_1[0].tolist(),c_mid_2[0].tolist()], end_acc = c_mid_2[1].tolist(), init_acc = c_mid_1[1].tolist(), init_vel = [0.,0.,0.], end_vel = [0.,0.,0.])
bezier_2 = __Bezier([c_mid_2[0].tolist(),com_2] , init_acc = c_mid_2[1].tolist(), init_vel = [0.,0.,0.])
p0 = fullBody.generateCurveTraj(bezier_0)
print "p0", p0
fullBody.generateCurveTraj(bezier_1)
fullBody.generateCurveTraj(bezier_2)
pp.displayPath(p0)
pp.displayPath(p0+1)
pp.displayPath(p0+2)
paths_ids = [int(el) for el in fullBody.comRRTFromPos(stateid,p0,p0+1,p0+2)]
#~ paths_ids = []
global allpaths
allpaths += paths_ids[:-1]
pp(paths_ids[-1])
#~ return success, paths_ids, c_mid_1, c_mid_2
return success, c_mid_1, c_mid_2, paths_ids
#~ data = gen_sequence_data_from_state(fullBody,3,configs)
def prepare_whole_interp(stateid, stateid_end):
all_points = []
allSuc = True
for i in range(stateid, stateid_end):
com_1 = __get_com(fullBody, configs[stateid])
success, c_mid_1, c_mid_2, paths_ids = test(i, False, True, False)
allSuc = success and allSuc
if not success:
break
all_points = all_points + [com_1, c_mid_1[0].tolist(), c_mid_2[0].tolist()]
all_points = all_points + [__get_com(fullBody, configs[stateid_end])]
if allSuc:
bezier_0 = __Bezier(all_points)
p0 = fullBody.generateCurveTraj(bezier_0)
pp.displayPath(p0)
num_paths = stateid_end - stateid
num_sub_paths = num_paths * 3
increment = 1. / float(num_paths)
partitions = [0.]
for i in range(0, num_paths):
dec = increment * float(i)
partitions += [dec + 0.01 * increment, dec + 0.99 * increment,dec + 1. * increment]
print "partitions", partitions, len(partitions)
p0 = fullBody.generateCurveTrajParts(bezier_0,partitions) +1
paths_ids = []
for i in range(0, num_paths):
print "***************************3i", p0+3*i
paths_ids += [int(el) for el in fullBody.comRRTFromPos(stateid + i,p0+3*i,p0+3*i+1,p0+3*i+2)]
#~ paths_ids = []
global allpaths
allpaths += paths_ids[:-1]
#~ pp(paths_ids[-1])
#~ success, paths_ids, c_mid_1, c_mid_2 = test(0, True, True, False)
#~ prepare_whole_interp(1, 2)
#~ pp(29),pp(9),pp(17)
from hpp.corbaserver.rbprm.tools.path_to_trajectory import *
from bezier_traj import *
init_bezier_traj(model.fullBody, r, pp, configs, model.limbsCOMConstraints)
#~ AFTER loading obstacles
def gen(ine_curve =False):
#~ test(0, True, True, ine_curve)
#~ test(0, True, True, True)
#~ test(1, True, True, ine_curve)
#~ test(1, True, True, True)
test(2, True, True, ine_curve)
#~ test(2, True, True, True)
test(3, True, True, ine_curve)
#~ test(3, True, True, True)
test(4, True, True, ine_curve)
test(5, True, True, ine_curve)
a = gen_trajectory_to_play(fullBody, pp, allpaths, flatten([[0.1, 0.9, 0.1] for _ in range(len(allpaths) / 3)]))
#~ pp(29),pp(9),pp(17)
#~ gen(True)
#~ test_ineq(0,{ rLegId : {'file': "hrp2/RL_com.ineq", 'effector' : 'RLEG_JOINT5'}}, 1000, [1,0,0,1])
#~ test_ineq(0,{ lLegId : {'file': "hrp2/LL_com.ineq", 'effector' : 'LLEG_JOINT5'}}, 1000, [0,0,1,1])
#~ gen(0,1)
script/scenarios/demos/siggraph_asia/down_hrp2_path.py
+ 1
− 1
View file @ dee0a8ed
......@@ -50,7 +50,7 @@ r = Viewer (ps)
q_init = rbprmBuilder.getCurrentConfig ();
q_init [0:3] = [0, 0, 0.58]; rbprmBuilder.setCurrentConfig (q_init); r (q_init)
q_init [0:3] = [0.2, 0, 0.48]; rbprmBuilder.setCurrentConfig (q_init); r (q_init)
#~ q_init [0:3] = [0.2, 0, 0.48]; rbprmBuilder.setCurrentConfig (q_init); r (q_init)
#~ q_init [0:3] = [0, -0.63, 0.6]; rbprmBuilder.setCurrentConfig (q_init); r (q_init)
#~ q_init [3:7] = [ 0.98877108, 0. , 0.14943813, 0. ]
......
script/scenarios/demos/siggraph_asia/grasp_hrp2_interp.py
+ 2
− 3
View file @ dee0a8ed
......@@ -19,7 +19,6 @@ srdfSuffix = ""
fullBody = FullBody ()
fullBody.loadFullBodyModel(urdfName, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
fullBody.setJointBounds ("base_joint_xyz", [-3,3, -2, 2, 0, 1])
ps = tp.ProblemSolver( fullBody )
......@@ -50,7 +49,7 @@ lHand = 'LARM_JOINT5'
lArmOffset = [0,0,-0.1075]
lArmNormal = [0,0,1]
lArmx = 0.024; lArmy = 0.024
fullBody.addLimb(larmId,larm,lHand,lArmOffset,lArmNormal, lArmx, lArmy, 10000, "manipulability", 0.1, "_6_DOF", False,grasp = True)
fullBody.addLimb(larmId,larm,lHand,lArmOffset,lArmNormal, lArmx, lArmy, 10000, "manipulability", 0.1, "_6_DOF", True,grasp = True)
#~ fullBody.addLimb(larmId,larm,lHand,lArmOffset,lArmNormal, lArmx, lArmy, 10000, "manipulability", 0.1, "_6_DOF", True)
#~ fullBody.addLimb(larmId,larm,lHand,lArmOffset,lArmNormal, lArmx, lArmy, 10000, "manipulability", 0.1, "_6_DOF")
#~ fullBody.addLimb(larmId,larm,lHand,lArmOffset,lArmNormal, lArmx, lArmy, 10000, 0.05)
......@@ -63,7 +62,7 @@ rArmOffset = [0,0,-0.1075]
rArmNormal = [0,0,1]
rArmx = 0.024; rArmy = 0.024
#disabling collision for hook
fullBody.addLimb(rarmId,rarm,rHand,rArmOffset,rArmNormal, rArmx, rArmy, 10000, "manipulability", 0.1, "_6_DOF", False,grasp = True)
fullBody.addLimb(rarmId,rarm,rHand,rArmOffset,rArmNormal, rArmx, rArmy, 10000, "manipulability", 0.1, "_6_DOF", True,grasp = True)
#~ fullBody.addLimb(rarmId,rarm,rHand,rArmOffset,rArmNormal, rArmx, rArmy, 10000, "manipulability", 0.1, "_6_DOF", True)
#~ fullBody.addLimb(rarmId,rarm,rHand,rArmOffset,rArmNormal, rArmx, rArmy, 10000, "manipulability", 0.1, "_6_DOF")
#~
......
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