Commit 00e31c55 authored by Pierre Fernbach's avatar Pierre Fernbach
Browse files

only take the first step in flat_ground

parent 12a7887e
......@@ -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_dynamic_config import *
from planning.configs.straight_walk_config import *
class Robot (Parent):
......
......@@ -40,13 +40,13 @@ r.client.gui.refresh()
#COM :
name = 'scom'
r.client.gui.addSphere(name,0.01,r.color.red)
r.client.gui.setVisibility(name,"ALWAYS_ON_TOP")
r.client.gui.addToGroup(name,r.sceneName)
r.addLandmark(name,1)
r.client.gui.addSphere("scom",0.01,r.color.red)
r.client.gui.setVisibility("scom","ALWAYS_ON_TOP")
r.client.gui.addToGroup("scom",r.sceneName)
r.addLandmark("scom",2)
r.client.gui.applyConfiguration("scom",q+[0,0,-1,0])
r.client.gui.applyConfiguration(name,fullBody.getCenterOfMass()+[0,0,-1,0])
r.client.gui.refresh()
......
......@@ -31,7 +31,7 @@ def __find_q_t(robot, path_player, path_id, t):
if(a >= b):
print "ERROR, a > b, t does not exist"
if abs(current_t - t) < __EPS:
print "last config q = ",q[-1]
#print "last config q = ",q[-1]
return q[:-1]
elif(current_t - t) < __EPS:
a = u
......@@ -67,12 +67,12 @@ def gen_trajectory_to_play(robot, path_player, path_ids, total_time_per_paths, d
activeid = 0
print "path_ids = ", path_ids
for i, path_id in enumerate(path_ids):
config_size_path = len(path_player.client.problem.configAtParam (path_id, 0))
config_size_path = len(path_player.client.problem.configAtParam (int(path_id), 0))
if(config_size_path > config_size):
#~ if(i == 1 ):
res+= follow_trajectory_path(robot, path_player, path_id, total_time_per_paths[i], dt_framerate)
res+= follow_trajectory_path(robot, path_player, int(path_id), total_time_per_paths[i], dt_framerate)
else:
res+= linear_interpolate_path(robot, path_player, path_id, total_time_per_paths[i], dt_framerate)
res+= linear_interpolate_path(robot, path_player, int(path_id), total_time_per_paths[i], dt_framerate)
activeid +=1
return res
......
Supports Markdown
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