From ea274d54eab7f05bdda694d45e5e651a3f9729c3 Mon Sep 17 00:00:00 2001
From: Guilhem Saurel <guilhem.saurel@laas.fr>
Date: Tue, 22 Oct 2019 12:13:17 +0200
Subject: [PATCH] [wip/hpp-rbprm-corba] fix patch-aa
---
hpp-rbprm-corba/Makefile | 2 +-
hpp-rbprm-corba/distinfo | 2 +-
hpp-rbprm-corba/patches/patch-aa | 81 ++++++++++++++++++++++++++++++++
3 files changed, 83 insertions(+), 2 deletions(-)
diff --git a/hpp-rbprm-corba/Makefile b/hpp-rbprm-corba/Makefile
index a8713b3a..d3edd325 100644
--- a/hpp-rbprm-corba/Makefile
+++ b/hpp-rbprm-corba/Makefile
@@ -4,7 +4,7 @@
HPP_PACKAGE= hpp-rbprm-corba
HPP_COMMENT= Corba server for RB-PRM
-PKGREVISION= 1
+PKGREVISION= 2
CATEGORIES= wip
diff --git a/hpp-rbprm-corba/distinfo b/hpp-rbprm-corba/distinfo
index fa1603c9..3dfeb294 100644
--- a/hpp-rbprm-corba/distinfo
+++ b/hpp-rbprm-corba/distinfo
@@ -1,4 +1,4 @@
SHA1 (hpp-rbprm-corba-4.7.0.tar.gz) = a362a05965cdb76ea2c29bc93895eaa9f489c6a7
RMD160 (hpp-rbprm-corba-4.7.0.tar.gz) = 22572f81b009299a7bf01985d0a2bfb375a87f6f
Size (hpp-rbprm-corba-4.7.0.tar.gz) = 31644468 bytes
-SHA1 (patch-aa) = da39a3ee5e6b4b0d3255bfef95601890afd80709
+SHA1 (patch-aa) = ba8562ee4d32cb890aa7ed3c758bc5fa08e6705c
diff --git a/hpp-rbprm-corba/patches/patch-aa b/hpp-rbprm-corba/patches/patch-aa
index e69de29b..010e3e40 100644
--- a/hpp-rbprm-corba/patches/patch-aa
+++ b/hpp-rbprm-corba/patches/patch-aa
@@ -0,0 +1,81 @@
+--- src/hpp/corbaserver/rbprm/tools/path_to_trajectory.py.orig 2019-09-26 22:54:34.000000000 +0200
++++ src/hpp/corbaserver/rbprm/tools/path_to_trajectory.py 2019-10-22 12:11:55.371089605 +0200
+@@ -1,11 +1,17 @@
+-__24fps = 1. / 24.
+-__EPS = 0.00001
+ from __future__ import print_function
++
++import time
++
+ from numpy.linalg import norm
+
++__24fps = 1. / 24.
++__EPS = 0.00001
++
++
+ def __linear_interpolation(p0,p1,dist_p0_p1, val):
+ return p0 + (p1 - p0) * val / dist_p0_p1
+
++
+ def __gen_frame_positions(com_waypoints, dt, dt_framerate=__24fps):
+ total_time = (len(com_waypoints) - 1) * dt
+ dt_final = total_time * dt_framerate
+@@ -14,7 +20,9 @@
+ ids_val = []
+ for i in range(len(com_waypoints) - 1):
+ ids_val += [(i,val-dt*i) for val in dt_finals if (val < (i+1) *dt) and (val > i*dt)]
+- return [com_waypoints[0]] + [__linear_interpolation(com_waypoints[i], com_waypoints[i+1], dt, val) for (i,val) in ids_val] + [com_waypoints[-1]]
++ return [com_waypoints[0]
++ ] + [__linear_interpolation(com_waypoints[i], com_waypoints[i + 1], dt, val)
++ for (i, val) in ids_val] + [com_waypoints[-1]]
+
+
+ def __find_q_t(robot, path_player, path_id, t):
+@@ -41,6 +49,7 @@
+ u = (b+a)/2.
+ return u
+
++
+ def linear_interpolate_path(robot, path_player, path_id, total_time, dt_framerate=__24fps):
+ pp = path_player
+ length = pp.client.problem.pathLength (path_id)
+@@ -49,6 +58,7 @@
+ dt_finals = [dt*i for i in range(int(round(num_frames_required))-1)] + [length]
+ return[pp.client.problem.configAtParam (path_id, t) for t in dt_finals]
+
++
+ 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)
+@@ -60,6 +70,7 @@
+ dt_finals = [dt*i / length for i in range(int(round(num_frames_required)))]
+ 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)
+ config_size = len(robot.getCurrentConfig())
+@@ -77,7 +88,7 @@
+ activeid +=1
+ return res
+
+-import time
++
+ def play_trajectory(fullBody, path_player, configs, dt_framerate=__24fps):
+ for q in configs:
+ start = time.time()
+@@ -87,12 +98,14 @@
+ if elapsed < dt_framerate :
+ time.sleep(dt_framerate-elapsed)
+
++
+ def displayFeetTrajectory(path_player,end_effector_names,colors,path_ids):
+ assert len(end_effector_names)==len(colors)
+ for path_id in path_ids:
+ for i in range(len(end_effector_names)):
+ path_player.displayPath(int(path_id),colors[i],end_effector_names[i])
+
++
+ #~ import numpy as np
+ #~ com_waypoints = [np.array([i,i,i]) for i in range(6)]
+ #~ dt = 0.2
--
GitLab