diff --git a/data/meshes/siggraph_asia/scale.stl b/data/meshes/siggraph_asia/scale.stl new file mode 100644 index 0000000000000000000000000000000000000000..d428086279d69bb1db5c2ab57f0cfb4fb7fd2c35 Binary files /dev/null and b/data/meshes/siggraph_asia/scale.stl differ diff --git a/data/srdf/siggraph_asia/scale.srdf b/data/srdf/siggraph_asia/scale.srdf new file mode 100644 index 0000000000000000000000000000000000000000..114223fe8bdb1f152d7d773ccd628eb3f258ef9b --- /dev/null +++ b/data/srdf/siggraph_asia/scale.srdf @@ -0,0 +1,3 @@ +<?xml version="1.0"?> +<robot name="scale"> +</robot> diff --git a/data/urdf/siggraph_asia/scale.urdf b/data/urdf/siggraph_asia/scale.urdf new file mode 100644 index 0000000000000000000000000000000000000000..25c381eb92c3cab2ba4c94416a9de275c4c10164 --- /dev/null +++ b/data/urdf/siggraph_asia/scale.urdf @@ -0,0 +1,19 @@ +<robot name="scale"> + <link name="base_link"> + <visual> + <origin xyz="0 0 0" rpy="0 0 0" /> + <geometry> + <mesh filename="package://hpp-rbprm-corba/meshes/scale.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/scale.stl"/> + </geometry> + </collision> + </link> +</robot> diff --git a/script/scenarios/demos/siggraph_asia/log.txt b/script/scenarios/demos/siggraph_asia/log.txt new file mode 100644 index 0000000000000000000000000000000000000000..7ae6fd80554bfefc3b07a4165b4736732ac563d0 --- /dev/null +++ b/script/scenarios/demos/siggraph_asia/log.txt @@ -0,0 +1,346 @@ +path computation 27 +path computation 8 +path computation 11 +path computation 20 +path computation 4 +path computation 4 +path computation 4 +path computation 9 +path computation 4 +path computation 4 +path computation 7 +path computation 10 +path computation 4 +path computation 8 +path computation 10 +path computation 8 +path computation 9 +path computation 13 +path computation 10 +path computation 8 +path computation 13 +path computation 9 +path computation 10 +path computation 17 +path computation 11 +path computation 8 +path computation 5 +path computation 9 +path computation 9 +path computation 8 +path computation 13 +path computation 9 +path computation 4 +path computation 7 +path computation 14 +path computation 13 +path computation 8 +path computation 4 +path computation 3 +path computation 4 +path computation 6 +path computation 7 +path computation 2 +path computation 8 +path computation 3 +path computation 2 +path computation 10 +path computation 4 +path computation 9 +path computation 8 +path computation 2 +path computation 4 +path computation 3 +path computation 3 +path computation 6 +path computation 20 +path computation 9 +path computation 15 +path computation 17 +path computation 8 +path computation 7 +path computation 10 +path computation 7 +path computation 11 +path computation 8 +path computation 15 +path computation 10 +path computation 12 +path computation 13 +path computation 18 +path computation 17 +path computation 8 +path computation 14 +path computation 17 +path computation 15 +path computation 10 +path computation 15 +path computation 6 +path computation 8 +path computation 14 +path computation 11 +path computation 8 +path computation 12 +path computation 8 +path computation 9 +path computation 12 +path computation 9 +path computation 4 +path computation 12 +path computation 15 +path computation 13 +path computation 7 +path computation 7 +path computation 4 +path computation 10 +path computation 14 +path computation 12 +path computation 12 +path computation 13 +path computation 8 +path computation 11 +path computation 7 +path computation 15 +path computation 8 +path computation 5 +path computation 4 +path computation 7 +path computation 4 +path computation 9 +path computation 3 +path computation 5 +path computation 10 +path computation 9 +path computation 3 +path computation 5 +path computation 1 +path computation 2 +path computation 2 +path computation 5 +path computation 2 +path computation 4 +path computation 4 +path computation 1 +path computation 7 +path computation 1 +path computation 9 +path computation 2 +path computation 5 +path computation 3 +path computation 3 +path computation 2 +path computation 5 +path computation 5 +path computation 6 +path computation 7 +path computation 3 +path computation 4 +path computation 2 +path computation 3 +path computation 11 +path computation 4 +path computation 2 +path computation 6 +path computation 9 +path computation 6 +path computation 9 +path computation 6 +path computation 12 +path computation 16 +path computation 5 +path computation 10 +path computation 7 +path computation 7 +path computation 5 +path computation 3 +path computation 6 +path computation 5 +path computation 6 +path computation 7 +path computation 9 +path computation 8 +path computation 4 +path computation 8 +path computation 5 +path computation 3 +path computation 6 +path computation 11 +path computation 7 +path computation 4 +path computation 5 +path computation 6 +path computation 9 +path computation 10 +path computation 11 +path computation 7 +path computation 9 +path computation 3 +path computation 3 +path computation 4 +path computation 3 +path computation 3 +path computation 3 +path computation 5 +path computation 3 +path computation 4 +path computation 6 +path computation 7 +path computation 9 +path computation 7 +path computation 4 +path computation 7 +path computation 7 +path computation 5 +path computation 4 +path computation 5 +path computation 7 +path computation 7 +path computation 4 +path computation 4 +path computation 6 +path computation 5 +path computation 9 +path computation 9 +path computation 8 +path computation 6 +path computation 3 +path computation 7 +path computation 7 +path computation 9 +path computation 5 +path computation 6 +path computation 5 +path computation 9 +path computation 9 +path computation 9 +path computation 11 +path computation 6 +path computation 11 +path computation 9 +path computation 4 +path computation 10 +path computation 63 +path computation 3 +path computation 7 +path computation 4 +path computation 4 +path computation 11 +path computation 5 +path computation 5 +path computation 6 +path computation 4 +path computation 4 +path computation 4 +path computation 7 +path computation 4 +path computation 3 +path computation 6 +path computation 5 +path computation 8 +path computation 10 +path computation 16 +path computation 16 +path computation 14 +path computation 5 +path computation 8 +path computation 7 +path computation 4 +path computation 5 +path computation 5 +path computation 4 +path computation 3 +path computation 11 +path computation 5 +path computation 3 +path computation 4 +path computation 3 +path computation 11 +path computation 6 +path computation 7 +path computation 4 +path computation 3 +path computation 5 +path computation 9 +path computation 5 +path computation 8 +path computation 8 +path computation 4 +path computation 7 +path computation 7 +path computation 4 +path computation 3 +path computation 3 +path computation 6 +path computation 3 +path computation 4 +path computation 9 +path computation 5 +path computation 3 +path computation 3 +path computation 7 +path computation 8 +path computation 8 +path computation 3 +path computation 9 +path computation 10 +path computation 4 +path computation 5 +path computation 9 +path computation 5 +path computation 6 +path computation 4 +path computation 7 +path computation 11 +path computation 8 +path computation 4 +path computation 10 +path computation 6 +path computation 4 +path computation 8 +path computation 5 +path computation 8 +path computation 5 +path computation 4 +path computation 6 +path computation 3 +path computation 2 +path computation 3 +path computation 11 +path computation 6 +path computation 4 +path computation 12 +path computation 5 +path computation 6 +path computation 4 +path computation 7 +path computation 7 +path computation 5 +path computation 7 +path computation 10 +path computation 10 +path computation 4 +path computation 10 +path computation 7 +path computation 7 +path computation 11 +path computation 4 +path computation 11 +path computation 5 +path computation 5 +path computation 8 +path computation 8 +path computation 5 +path computation 9 +path computation 8 +path computation 6 +path computation 2 +path computation 5 +path computation 2 +path computation 2 +path computation 8 +path computation 13 +path computation 7 +path computation 10 +path computation 9 +path computation 6 +path computation 7 diff --git a/script/scenarios/demos/siggraph_asia/scale_hrp2_interp.py b/script/scenarios/demos/siggraph_asia/scale_hrp2_interp.py index 105790115dfd419bbfc4dd46912c97a6b6b57813..3f6f8d2bdd119b5f7e8368ac795a1162a66f4e78 100644 --- a/script/scenarios/demos/siggraph_asia/scale_hrp2_interp.py +++ b/script/scenarios/demos/siggraph_asia/scale_hrp2_interp.py @@ -111,8 +111,8 @@ 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]) #,rarmId,larmId]) +fullBody.setStartState(q_init,[rLegId,lLegId,rarmId]) #,rarmId,larmId]) +#~ fullBody.setStartState(q_init,[rLegId,lLegId]) #,rarmId,larmId]) fullBody.setEndState(q_goal,[rLegId,lLegId])#,rarmId,larmId]) #~ #~ configs = fullBody.interpolate(0.1) @@ -274,14 +274,14 @@ def play_all_paths_qs(): if i % 2 == 0 : pp(pid) -def test(stateid = 1, path = False, use_rand = False, just_one_curve = False) : +def test(stateid = 1, path = False, use_rand = False, just_one_curve = False, num_optim = 0) : com_1 = __get_com(fullBody, configs[stateid]) com_2 = __get_com(fullBody, configs[stateid+1]) data = gen_sequence_data_from_state(fullBody,stateid,configs, mu = 1.) 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_quasi_static(data, c_bounds = [c_bounds_1, c_bounds_2, c_bounds_mid], use_rand = use_rand, mu = 0.8, fullBody = fullBody) #~ 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]) @@ -292,12 +292,12 @@ def test(stateid = 1, path = False, use_rand = False, just_one_curve = False) : 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.]) + p0 = fullBody.generateCurveTrajParts(bezier_0,[0.,0.3,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)] + paths_ids = [int(el) for el in fullBody.comRRTFromPos(stateid,p0+1,p0+2,p0+3,num_optim)] 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.]) @@ -310,12 +310,12 @@ def test(stateid = 1, path = False, use_rand = False, just_one_curve = False) : 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 = [int(el) for el in fullBody.comRRTFromPos(stateid,p0,p0+1,p0+2,num_optim)] #~ paths_ids = [] global allpaths allpaths += paths_ids[:-1] #~ allpaths += [paths_ids[-1]] - pp(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 @@ -372,7 +372,7 @@ def gen(ine_curve =False): #~ 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)])) + a = gen_trajectory_to_play(fullBody, pp, allpaths, flatten([[0.3, 0.6, 0.1] for _ in range(len(allpaths) / 3)])) #~ pp(29),pp(9),pp(17) #~ gen(True)