diff --git a/CMakeLists.txt b/CMakeLists.txt
index 22991513c66cc410187dc37d04317adeef2c16b7..e778048ed72f6985121ace66b9b9261588745273 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -45,6 +45,12 @@ IF (HPP_DEBUG)
SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DHPP_DEBUG")
ENDIF()
+# search for python
+IF(NOT DEFINED PYTHON_DESIRED_VERSION)
+ SET(PYTHON_DESIRED_VERSION 2.7)
+ENDIF()
+FINDPYTHON(${PYTHON_DESIRED_VERSION} EXACT)
+
ADD_DOC_DEPENDENCY("hpp-core >= 4.3")
ADD_REQUIRED_DEPENDENCY("hpp-corbaserver >= 4.3")
ADD_REQUIRED_DEPENDENCY("hpp-rbprm >= 4.3")
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index e92f9596c1697ef24ed5f30d9b53f8eb55d9b342..05caec46481f3d1dbbb0a0b0cf8144ffe7b79503 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -24,9 +24,6 @@ OMNIIDL_INCLUDE_DIRECTORIES(
${HPP_CORBASERVER_DATAROOTDIR}/idl ${CMAKE_SOURCE_DIR}/idl
)
-# search for python
-FINDPYTHON(2.7 EXACT REQUIRED)
-
INCLUDE_DIRECTORIES(${CMAKE_BINARY_DIR}/src)
FILE(MAKE_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}/hpp/corbaserver/rbprm)
diff --git a/src/hpp/corbaserver/rbprm/__init__.py b/src/hpp/corbaserver/rbprm/__init__.py
index 32c1be5fec19deb386b588745a7a265145f5e529..3ff722bf11996f80e40cc1a236b5b39d00fd3306 100644
--- a/src/hpp/corbaserver/rbprm/__init__.py
+++ b/src/hpp/corbaserver/rbprm/__init__.py
@@ -1 +1 @@
-from client import Client
+from .client import Client
diff --git a/src/hpp/corbaserver/rbprm/rbprmfullbody.py b/src/hpp/corbaserver/rbprm/rbprmfullbody.py
index 95f43ffad138968dd9cb9da5f0acb482329bb1f2..07e43a6a35271cd7a1f4a7bfca32cfa6f186c8ba 100755
--- a/src/hpp/corbaserver/rbprm/rbprmfullbody.py
+++ b/src/hpp/corbaserver/rbprm/rbprmfullbody.py
@@ -407,7 +407,7 @@ class FullBody (Robot):
Ns.append({})
if(len(P[i]) > 0):
targetName = limb
- if(dicEffector.has_key(limb)):
+ if(limb in dicEffector):
targetName = dicEffector[limb]['effector']
Ps[i][targetName] = P[i]
Ns[i][targetName] = N[i]
@@ -829,7 +829,7 @@ class FullBody (Robot):
# \param viewer gepetto viewer instance
def draw(self, configuration, viewer):
viewer(configuration)
- for limb, groupid in self.octrees.iteritems():
+ for limb, groupid in self.octrees.items():
transform = self.clientRbprm.rbprm.getOctreeTransform(limb, configuration)
viewer.client.gui.applyConfiguration(groupid,transform)
viewer.client.gui.refresh()
diff --git a/src/hpp/corbaserver/rbprm/state_alg.py b/src/hpp/corbaserver/rbprm/state_alg.py
index 2490f07d46f5a66afa3e9e34ec8ab32f1ea964c8..62ef1cfb76127b01afe214e11fddc36e935ea941 100644
--- a/src/hpp/corbaserver/rbprm/state_alg.py
+++ b/src/hpp/corbaserver/rbprm/state_alg.py
@@ -16,6 +16,7 @@
# hpp-manipulation-corba. If not, see
# <http://www.gnu.org/licenses/>.
+from __future__ import print_function
from hpp.corbaserver.rbprm.rbprmstate import State
from lp_find_point import find_valid_c_cwc, find_valid_c_cwc_qp, lp_ineq_4D
from hpp.corbaserver.rbprm.tools.com_constraints import *
@@ -53,7 +54,7 @@ def isContactReachable(state, limbName, p, n, limbsCOMConstraints):
res_ineq = [np.vstack([new_ineq[0],active_ineq[0]]), np.hstack([new_ineq[1],active_ineq[1]])]
success, status_ok , res = lp_ineq_4D(res_ineq[0],-res_ineq[1])
if not success:
- print "In isContactReachable no stability, Lp failed (should not happen) ", status_ok
+ print("In isContactReachable no stability, Lp failed (should not happen) ", status_ok)
return False, [-1,-1,-1]
return (res[3] >= 0), res[0:3]
@@ -134,8 +135,8 @@ def projectToFeasibleCom(state, ddc =[0.,0.,0.], max_num_samples = 10, friction
ps = state.getContactPosAndNormals()
p = ps[0][0]
N = ps[1][0]
- print "p", p
- print "N", N
+ print("p", p)
+ print("N", N)
#~ try:
H = compute_CWC(p, N, state.fullBody.client.basic.robot.getMass(), mu = friction, simplify_cones = False)
c_ref = state.getCenterOfMass()
@@ -150,12 +151,12 @@ def projectToFeasibleCom(state, ddc =[0.,0.,0.], max_num_samples = 10, friction
x[2] += 0.35
for i in range(10):
if state.fullBody.projectStateToCOM(state.sId ,x, max_num_samples):
- print "success after " + str(i) + " trials"
+ print("success after " + str(i) + " trials")
return True
else:
x[2]-=0.05
else:
- print "qp failed"
+ print("qp failed")
return False;
def isContactCreated(s1, s2):
diff --git a/src/hpp/corbaserver/rbprm/tools/com_constraints.py b/src/hpp/corbaserver/rbprm/tools/com_constraints.py
index d9b6b00d6e936bc1d5c51e83db3f09adb2848a86..c3927b1faa52d81ff6027db8c43e64381ff4dc54 100644
--- a/src/hpp/corbaserver/rbprm/tools/com_constraints.py
+++ b/src/hpp/corbaserver/rbprm/tools/com_constraints.py
@@ -1,9 +1,10 @@
+from __future__ import print_function
import matplotlib
#~ matplotlib.use('Agg')
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
from cwc import cone_optimization
-from obj_to_constraints import ineq_from_file, rotate_inequalities
+from .obj_to_constraints import ineq_from_file, rotate_inequalities
import numpy as np
import math
from numpy.linalg import norm
@@ -45,8 +46,8 @@ def get_com_constraint(fullBody, state, config, limbsCOMConstraints, interm = Fa
As = []; bs = []
fullBody.setCurrentConfig(config)
contacts = []
- for i, v in limbsCOMConstraints.iteritems():
- if not constraintsComLoaded.has_key(i):
+ for i, v in limbsCOMConstraints.items():
+ if i not in constraintsComLoaded:
constraintsComLoaded[i] = ineq_from_file(ineqPath+v['file'])
contact = (interm and fullBody.isLimbInContactIntermediary(i, state)) or (not interm and fullBody.isLimbInContact(i, state))
if(contact):
@@ -65,12 +66,12 @@ def get_com_constraint(fullBody, state, config, limbsCOMConstraints, interm = Fa
if(len(As) > 0):
return [np.vstack(As), np.hstack(bs)]
else:
- print "Warning: no active contact in get_com_constraint"
+ print("Warning: no active contact in get_com_constraint")
return [np.zeros([3,3]), np.zeros(3)]
def get_com_constraint_at_transform(pos_quat, limb, limbsCOMConstraints):
global constraintsLoaded
- if not constraintsComLoaded.has_key(limb):
+ if limb not in constraintsComLoaded:
constraintsComLoaded[limb] = ineq_from_file(ineqPath+limbsCOMConstraints[limb]['file'])
tr = quaternion_matrix(pos_quat[3:7])
tr[:3,3] = np.array(pos_quat[0:3])
diff --git a/src/hpp/corbaserver/rbprm/tools/cwc_trajectory.py b/src/hpp/corbaserver/rbprm/tools/cwc_trajectory.py
index 58a52957589c9a2105483b52a34ff9b834236c95..1897cbff9fc8a94370a2e8052294df3deb42effb 100644
--- a/src/hpp/corbaserver/rbprm/tools/cwc_trajectory.py
+++ b/src/hpp/corbaserver/rbprm/tools/cwc_trajectory.py
@@ -1,13 +1,14 @@
+from __future__ import print_function
import matplotlib
#~ matplotlib.use('Agg')
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
from cwc import cone_optimization
-from obj_to_constraints import ineq_from_file, rotate_inequalities
+from .obj_to_constraints import ineq_from_file, rotate_inequalities
import numpy as np
import math
from numpy.linalg import norm
-from com_constraints import get_com_constraint
+from .com_constraints import get_com_constraint
import time
import hpp.corbaserver.rbprm.data.com_inequalities as ine
@@ -43,7 +44,7 @@ lastspeed = np.array([0,0,0])
def compute_state_info(fullBody,states, state_id, phase_dt, mu, computeCones, limbsCOMConstraints, pathId = 0):
- print "phase dt in compute_state_info:",phase_dt
+ print("phase dt in compute_state_info:",phase_dt)
init_com = __get_com(fullBody, states[state_id])
end_com = __get_com(fullBody, states[state_id+1])
p, N = fullBody.computeContactPoints(state_id)
@@ -70,7 +71,7 @@ def compute_state_info(fullBody,states, state_id, phase_dt, mu, computeCones, li
COMConstraints.append(get_com_constraint(fullBody, state_id, states[state_id], limbsCOMConstraints, True))
if(len(p) > len(COMConstraints)):
COMConstraints.append(get_com_constraint(fullBody, state_id + 1, states[state_id + 1], limbsCOMConstraints))
- print 'num cones ', len(cones)
+ print('num cones ', len(cones))
return p, N, init_com, end_com, t_end_phases, cones, COMConstraints
def compute_initial_guess(fullBody,t_end_phases,pathId,state_id,dt=0.01):
@@ -117,8 +118,8 @@ reduce_ineq = True, verbose = False, limbsCOMConstraints = None, profile = False
t_end_phases += [t_end_phases[-1] + t for t in t_end_phases1[1:]]
initial_guess = compute_initial_guess(fullBody,t_end_phases,pathId,state_id,dt)
if (not profile):
- print "num cones ", len(cones)
- print "end_phases", t_end_phases
+ print("num cones ", len(cones))
+ print("end_phases", t_end_phases)
timeelapsed = 0
if (profile):
@@ -129,10 +130,10 @@ reduce_ineq = True, verbose = False, limbsCOMConstraints = None, profile = False
else:
init_vel=[0,0,0]
end_vel=[0,0,0]
- print "init_vel ="
- print init_vel
- print "end_vel = "
- print end_vel
+ print("init_vel =")
+ print(init_vel)
+ print("end_vel = ")
+ print(end_vel)
var_final, params = cone_optimization(p, N, [init_com + init_vel, end_com + end_vel], t_end_phases[1:], dt, cones, COMConstraints, mu, mass, 9.81, reduce_ineq, verbose,
constraints, param_constraints,initial_guess = initial_guess)
#~ print "end_com ", end_com , "computed end come", var_final['c'][-1], var_final['c_end']
@@ -148,20 +149,20 @@ reduce_ineq = True, verbose = False, limbsCOMConstraints = None, profile = False
var_final['dc'] = var_final['dc'][:init_waypoint_time+1]
var_final['ddc'] = var_final['ddc'][:init_waypoint_time+1]
params["t_init_phases"] = params["t_init_phases"][:4]
- print "trying to project on com (from, to) ", init_end_com, var_final['c'][-1]
+ print("trying to project on com (from, to) ", init_end_com, var_final['c'][-1])
if (fullBody.projectStateToCOM(state_id+1, (var_final['c'][-1]).tolist())):
#~ print "PROJECTED", init_end_com, var_final['c'][-1]
states[state_id+1] = fullBody.getConfigAtState(state_id+1) #updating config from python side)
lastspeed = var_final['dc'][init_waypoint_time]
- print "init speed", lastspeed
+ print("init speed", lastspeed)
else:
use_window = 0
- print "reached com is not good, restarting problem with window = ",use_window
+ print("reached com is not good, restarting problem with window = ",use_window)
return gen_trajectory(fullBody, states, state_id, computeCones, mu, dt, phase_dt, reduce_ineq, verbose, limbsCOMConstraints, profile, use_window = use_window, pathId = pathId)
else:
if norm(np.array(var_final['c'][-1]) - np.array(__get_com(fullBody, states[state_id+1]))) > 0.00001:
- print "error in com desired: obtained ", __get_com(fullBody, states[state_id+1]), var_final['c'][-1]
- print "restarting problem with windows = ",use_window+1
+ print("error in com desired: obtained ", __get_com(fullBody, states[state_id+1]), var_final['c'][-1])
+ print("restarting problem with windows = ",use_window+1)
return gen_trajectory(fullBody, states, state_id, computeCones, mu, dt, phase_dt ,reduce_ineq, verbose, limbsCOMConstraints, profile , use_window + 1,use_velocity, pathId)
lastspeed = np.array([0,0,0])
@@ -194,8 +195,8 @@ def draw_trajectory(fullBody, states, state_id, computeCones = False, mu = 1, d
#plt.show()
plt.savefig('/tmp/figCWC/c'+ str(state_id)+ '.png')
- print "plotting speed "
- print "end target ", params['x_end']
+ print("plotting speed ")
+ print("end target ", params['x_end'])
fig = plt.figure()
ax = fig.add_subplot(111)
if(use_window > 0):
@@ -213,7 +214,7 @@ def draw_trajectory(fullBody, states, state_id, computeCones = False, mu = 1, d
#~ plt.show()
plt.savefig('/tmp/figCWC/dc'+ str(state_id)+ '.png')
- print "plotting acceleration "
+ print("plotting acceleration ")
fig = plt.figure()
ax = fig.add_subplot(111)
if(use_window > 0):
@@ -229,7 +230,7 @@ def draw_trajectory(fullBody, states, state_id, computeCones = False, mu = 1, d
#~ plt.show()
plt.savefig('/tmp/figCWC/ddc'+ str(state_id)+ '.png')
- print "plotting Dl "
+ print("plotting Dl ")
fig = plt.figure()
ax = fig.add_subplot(111)
points = var_final['dL']
@@ -244,7 +245,7 @@ def draw_trajectory(fullBody, states, state_id, computeCones = False, mu = 1, d
def __cVarPerPhase(var, dt, t, final_state, addValue):
varVals = addValue + [v.tolist() for v in final_state[var]]
- print "cVarPerPhase : t = ", t
+ print("cVarPerPhase : t = ", t)
varPerPhase = [[varVals[(int)(round(t_id/dt)) ] for t_id in np.arange(t[index],t[index+1]-_EPS,dt)] for index, _ in enumerate(t[:-1]) ]
#print "varperPhase ="
#print varPerPhase
@@ -264,15 +265,15 @@ def __cVarPerPhase(var, dt, t, final_state, addValue):
def __optim__threading_ok(fullBody, states, state_id, computeCones = False, mu = 1, dt =0.1, phase_dt = [0.4, 1], reduce_ineq = True,
num_optims = 0, draw = False, verbose = False, limbsCOMConstraints = None, profile = False, use_window = 0,use_velocity=False, pathId = 0):
- print "callgin gen ",state_id
+ print("callgin gen ",state_id)
if(draw):
res = draw_trajectory(fullBody, states, state_id, computeCones, mu, dt, phase_dt, reduce_ineq, verbose, limbsCOMConstraints, use_window,use_velocity, pathId)
else:
res = gen_trajectory(fullBody, states, state_id, computeCones, mu, dt, phase_dt, reduce_ineq, verbose, limbsCOMConstraints, profile, use_window,use_velocity, pathId)
alpha = res[1]['alpha']
- print "t in optim_threading :",res[1]["t_init_phases"]
+ print("t in optim_threading :",res[1]["t_init_phases"])
t = [ti * alpha for ti in res[1]["t_init_phases"]]
- print "t after alpha in optim_threading :",t
+ print("t after alpha in optim_threading :",t)
dt = res[1]["dt"] * alpha
final_state = res[0]
c0 = res[1]["x_init"][0:3]
@@ -289,22 +290,22 @@ def solve_com_RRT(fullBody, states, state_id, computeCones = False, mu = 1, dt =
reduce_ineq = True, num_optims = 0, draw = False, verbose = False, limbsCOMConstraints = None, profile = False, use_window = 0, trackedEffectors = [],use_velocity=False,pathId = 0):
comPosPerPhase, timeElapsed, final_state = __optim__threading_ok(fullBody, states, state_id, computeCones, mu, dt, phase_dt,
reduce_ineq, num_optims, draw, verbose, limbsCOMConstraints, profile, use_window,use_velocity, pathId)
- print "done. generating state trajectory ",state_id
+ print("done. generating state trajectory ",state_id)
paths_ids = [int(el) for el in fullBody.comRRTFromPos(state_id,comPosPerPhase[0],comPosPerPhase[1],comPosPerPhase[2],num_optims)]
- print "done. computing final trajectory to display ",state_id, "path ids ", paths_ids[-1], " ," , paths_ids[:-1]
+ print("done. computing final trajectory to display ",state_id, "path ids ", paths_ids[-1], " ," , paths_ids[:-1])
return paths_ids[-1], paths_ids[:-1], timeElapsed, final_state
def solve_effector_RRT(fullBody, states, state_id, computeCones = False, mu = 1, dt =0.1, phase_dt = [0.4, 1],
reduce_ineq = True, num_optims = 0, draw = False, verbose = False, limbsCOMConstraints = None, profile = False, use_window = 0, trackedEffectors = [],use_velocity=False,pathId = 0):
comPosPerPhase, timeElapsed, final_state = __optim__threading_ok(fullBody, states, state_id, computeCones, mu, dt, phase_dt,
reduce_ineq, num_optims, draw, verbose, limbsCOMConstraints, profile, use_window,use_velocity, pathId)
- print "done. generating state trajectory ",state_id
+ print("done. generating state trajectory ",state_id)
if(len(trackedEffectors) == 0):
paths_ids = [int(el) for el in fullBody.effectorRRT(state_id,comPosPerPhase[0],comPosPerPhase[1],comPosPerPhase[2],num_optims)]
else:
- print "handling extra effector constraints"
+ print("handling extra effector constraints")
refPathId = trackedEffectors[0]; path_start = trackedEffectors[1]; path_to = trackedEffectors[2]; effectorstracked = trackedEffectors[3]
paths_ids = [int(el) for el in fullBody.effectorRRTFromPath(state_id, refPathId, path_start, path_to, comPosPerPhase[0],comPosPerPhase[1],comPosPerPhase[2],num_optims, effectorstracked)]
- print "done. computing final trajectory to display ",state_id, "path ids ", paths_ids[-1], " ," , paths_ids[:-1]
+ print("done. computing final trajectory to display ",state_id, "path ids ", paths_ids[-1], " ," , paths_ids[:-1])
return paths_ids[-1], paths_ids[:-1], timeElapsed, final_state
diff --git a/src/hpp/corbaserver/rbprm/tools/cwc_trajectory_helper.py b/src/hpp/corbaserver/rbprm/tools/cwc_trajectory_helper.py
index 9a965e2071da5551d090a156d5e0020b5f1568f9..50e4612c20fdcacf38d1fc01bb54f7005d970270 100644
--- a/src/hpp/corbaserver/rbprm/tools/cwc_trajectory_helper.py
+++ b/src/hpp/corbaserver/rbprm/tools/cwc_trajectory_helper.py
@@ -1,4 +1,4 @@
-
+from __future__ import print_function
from hpp.corbaserver.rbprm.tools.cwc_trajectory import *
from hpp.corbaserver.rbprm.tools.path_to_trajectory import *
from cwc import OptimError, cone_optimization
@@ -35,7 +35,7 @@ def displayComPath(pp, pathId,color=[0.,0.75,0.15,0.9]) :
def genPandNperFrame(fullBody, stateid, limbsCOMConstraints, pp, path_ids, times, dt_framerate=1./24.):
p, N= fullBody.computeContactPointsPerLimb(stateid, limbsCOMConstraints.keys(), limbsCOMConstraints)
- freeEffectors = [ [limbsCOMConstraints[limb]['effector'] for limb in limbsCOMConstraints.keys() if not p[i].has_key(limbsCOMConstraints[limb]['effector'])] for i in range(len(p))]
+ freeEffectors = [ [limbsCOMConstraints[limb]['effector'] for limb in limbsCOMConstraints.keys() if limbsCOMConstraints[limb]['effector'] not in p[i]] for i in range(len(p))]
config_size = len(fullBody.getCurrentConfig())
interpassed = False
pRes = []
@@ -127,9 +127,9 @@ def __getTimes(fullBody, configs, i, time_scale):
def __getTimes(fullBody, configs, i, time_scale,use_window=0):
t = fullBody.getTimeAtState(i+1) - fullBody.getTimeAtState(i)
dt = 0.02
- print "t = ",t
+ print("t = ",t)
t = time_scale*t
- print "after scale, t = ",t
+ print("after scale, t = ",t)
trunk_distance = np.linalg.norm(np.array(configs[i+1][0:3]) - np.array(configs[i][0:3]))
distance = max(fullBody.getEffectorDistance(i,i+1), trunk_distance)
# TODO : si t = 0, hardcoded ...
@@ -146,7 +146,7 @@ def __getTimes(fullBody, configs, i, time_scale,use_window=0):
"""
times[1] = t - 2*times[0]
times[1] = float((int)(math.floor(times[1]*100.))) / 100.
- print "times : ",times
+ print("times : ",times)
return times, dt, distance,use_window
@@ -154,30 +154,30 @@ from hpp import Error as hpperr
import sys, time
def step(fullBody, configs, i, optim, pp, limbsCOMConstraints, friction = 0.5, optim_effectors = True, time_scale = 20., useCOMConstraints = False, use_window = 0, verbose = False, draw = False,
trackedEffectors = [],use_velocity=False,pathId = 0):
- print "##########################################"
+ print("##########################################")
global errorid
global stat_data
fail = 0
#~ try:
- print "Use window = ",use_window
+ print("Use window = ",use_window)
if(True):
times = [];
dt = 1000;
times, dt, distance,use_window = __getTimes(fullBody, configs, i, time_scale,use_window)
- print "Use window = ",use_window
+ print("Use window = ",use_window)
if distance == 0:
use_window = use_window+1
use_window = max(0, min(use_window, (len(configs) - 1) - (i + 2))) # can't use preview if last state is reached
w = 0
while w < (use_window+1):
times2, dt2, dist2,use_window = __getTimes(fullBody, configs, i+w, time_scale,use_window)
- print "Use window = ",use_window
+ print("Use window = ",use_window)
times += times2
dt = min(dt, dt2)
w = w+1
time_per_path = [times[0]] + [times[1]] + [times [0]]
- print 'time per path', times, time_per_path
- print 'dt', dt
+ print('time per path', times, time_per_path)
+ print('dt', dt)
if(distance > 0.0001):
stat_data["num_trials"] += 1
if(useCOMConstraints):
@@ -200,9 +200,9 @@ trackedEffectors = [],use_velocity=False,pathId = 0):
#~ if(len(trajec) > 0):
#~ frame_rate = 1./25.
#~ frame_rate_andrea = 1./1001.
- print "first traj :"
+ print("first traj :")
new_traj = gen_trajectory_to_play(fullBody, pp, trajectory, time_per_path, frame_rate)
- print "traj Andrea : "
+ print("traj Andrea : ")
new_traj_andrea = gen_trajectory_to_play(fullBody, pp, trajectory, time_per_path,frame_rate_andrea)
#~ new_contacts = gencontactsPerFrame(fullBody, i, limbsCOMConstraints, pp, trajectory, times, frame_rate_andrea)
Ps, Ns, freeEffectorsPerPhase = genPandNperFrame(fullBody, i, limbsCOMConstraints, pp, trajectory, time_per_path, frame_rate_andrea)
@@ -231,7 +231,7 @@ trackedEffectors = [],use_velocity=False,pathId = 0):
#assert(len(trajec_mil) == len(pos) and len(normals) == len(pos) and len(normals) == len(coms) and len(coms) == len(pEffs))
stat_data["num_success"] += 1
else:
- print "TODO, NO CONTACT VARIATION, LINEAR INTERPOLATION REQUIRED"
+ print("TODO, NO CONTACT VARIATION, LINEAR INTERPOLATION REQUIRED")
#~ except hpperr as e:
#~ print "hpperr failed at id " + str(i) , e.strerror
#~ if (use_window == 0 and (len(configs) - 1) - (i + 2) > 0):
@@ -335,27 +335,27 @@ def step_profile(fullBody, configs, i, optim, limbsCOMConstraints, friction = 0
__update_cwc_time(timeelapsed)
stat_data["num_success"] += 1
else:
- print "TODO, NO CONTACT VARIATION, LINEAR INTERPOLATION REQUIRED"
+ print("TODO, NO CONTACT VARIATION, LINEAR INTERPOLATION REQUIRED")
except hpperr as e:
- print "hpperr failed at id " + str(i) , e.strerror
+ print("hpperr failed at id " + str(i) , e.strerror)
stat_data["error_com_proj"] += 1
stat_data["num_errors"] += 1
errorid += [i]
fail+=1
except OptimError as e:
- print "OptimError failed at id " + str(i) , e.strerror
+ print("OptimError failed at id " + str(i) , e.strerror)
stat_data["error_optim_fail"] += 1
stat_data["num_errors"] += 1
errorid += [i]
fail+=1
except ValueError as e:
- print "ValueError failed at id " + str(i) , e
+ print("ValueError failed at id " + str(i) , e)
stat_data["error_unknown"] += 1
stat_data["num_errors"] += 1
errorid += [i]
fail+=1
except IndexError as e:
- print "IndexError failed at id " + str(i) , e
+ print("IndexError failed at id " + str(i) , e)
stat_data["error_unknown"] += 1
stat_data["num_errors"] += 1
errorid += [i]
@@ -363,7 +363,7 @@ def step_profile(fullBody, configs, i, optim, limbsCOMConstraints, friction = 0
except Exception as e:
stat_data["error_unknown"] += 1
stat_data["num_errors"] += 1
- print e
+ print(e)
errorid += [i]
fail+=1
except:
@@ -472,7 +472,7 @@ def write_stats(filename):
def profile(fullBody, configs, i_start, i_end, limbsCOMConstraints, friction = 0.5, optim_effectors = False, time_scale = 20., useCOMConstraints = False, filename ="log.txt"):
global stat_data
if(i_end > len(configs)-1):
- print "ERROR: i_end < len_configs ", i_end, len(configs)-1
+ print("ERROR: i_end < len_configs ", i_end, len(configs)-1)
return # no point in trying optim, path was not fully computed
for i in range(i_start, i_end):
step_profile(fullBody, configs, i, 0, limbsCOMConstraints, friction, optim_effectors, time_scale, useCOMConstraints)
diff --git a/src/hpp/corbaserver/rbprm/tools/path_to_trajectory.py b/src/hpp/corbaserver/rbprm/tools/path_to_trajectory.py
index 848313e1ecee4342ab2d80f78c1b5615d2fc0d58..25ee323342485384348aa819e57940e783ba911e 100644
--- a/src/hpp/corbaserver/rbprm/tools/path_to_trajectory.py
+++ b/src/hpp/corbaserver/rbprm/tools/path_to_trajectory.py
@@ -1,5 +1,6 @@
__24fps = 1. / 24.
__EPS = 0.00001
+from __future__ import print_function
from numpy.linalg import norm
def __linear_interpolation(p0,p1,dist_p0_p1, val):
@@ -29,7 +30,7 @@ def __find_q_t(robot, path_player, path_id, t):
q = pp.client.problem.configAtParam (path_id, u)
current_t = q[-1]
if(a >= b):
- print "ERROR, a > b, t does not exist"
+ print("ERROR, a > b, t does not exist")
if abs(current_t - t) < __EPS:
#print "last config q = ",q[-1]
return q[:-1]
@@ -50,7 +51,7 @@ def linear_interpolate_path(robot, path_player, path_id, total_time, dt_framerat
def follow_trajectory_path(robot, path_player, path_id, total_time, dt_framerate=__24fps):
pp = path_player
- print "total_times in follow path : ",total_time
+ print("total_times in follow path : ",total_time)
length = pp.client.problem.pathLength (path_id)
num_frames_required = total_time / dt_framerate
dt = float(length) / num_frames_required
@@ -60,12 +61,12 @@ def follow_trajectory_path(robot, path_player, path_id, total_time, dt_framerate
return[__find_q_t(robot, path_player, path_id, t) for t in dt_finals]
def gen_trajectory_to_play(robot, path_player, path_ids, total_time_per_paths, dt_framerate=__24fps):
- print "gen_trajectory : dt = ",dt_framerate
+ print("gen_trajectory : dt = ",dt_framerate)
config_size = len(robot.getCurrentConfig())
res = []
pp = path_player
activeid = 0
- print "path_ids = ", path_ids
+ print("path_ids = ", path_ids)
for i, path_id in enumerate(path_ids):
config_size_path = len(path_player.client.problem.configAtParam (int(path_id), 0))
if(config_size_path > config_size):
diff --git a/src/hpp/corbaserver/rbprm/tools/time_out.py b/src/hpp/corbaserver/rbprm/tools/time_out.py
index 08a6dcdb8035f76677ba36ba7921ed3cf053cbdc..58ec3c5a92d47ece96a54d3c0450d037e37b87b5 100644
--- a/src/hpp/corbaserver/rbprm/tools/time_out.py
+++ b/src/hpp/corbaserver/rbprm/tools/time_out.py
@@ -1,3 +1,4 @@
+from __future__ import print_function
import signal
# Register an handler for the timeout
@@ -19,17 +20,17 @@ if __name__ == '__main__':
def loop(dt1, dt2):
import time
for i in range(dt1 + dt2):
- print "sec"
+ print("sec")
time.sleep(1)
try:
run_time_out(loop, 3, 3, 2)
- except Exception, exc:
- print exc
- print "exception caught, ok"
+ except Exception as exc:
+ print(exc)
+ print("exception caught, ok")
try:
run_time_out(loop, 6, 3, 2)
- except Exception, exc:
- print exc
- print "exception should NOT becaught"
+ except Exception as exc:
+ print(exc)
+ print("exception should NOT becaught")