From ce31ce49b44d5378659a77d09cc4f1aec76e2795 Mon Sep 17 00:00:00 2001
From: Steve Tonneau <stonneau@axle.laas.fr>
Date: Sat, 6 Aug 2016 12:54:18 +0200
Subject: [PATCH] integrating trajectory optimization method
---
src/CMakeLists.txt | 1 +
src/hpp/corbaserver/rbprm/rbprmfullbody.py | 2 +-
.../corbaserver/rbprm/tools/cwc_trajectory.py | 51 +++++++++++++++++++
3 files changed, 53 insertions(+), 1 deletion(-)
create mode 100644 src/hpp/corbaserver/rbprm/tools/cwc_trajectory.py
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index 23ee4755..a6022a95 100755
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -109,6 +109,7 @@ INSTALL(
${CMAKE_CURRENT_SOURCE_DIR}/hpp/corbaserver/rbprm/tools/__init__.py
${CMAKE_CURRENT_SOURCE_DIR}/hpp/corbaserver/rbprm/tools//generateROMs.py
${CMAKE_CURRENT_SOURCE_DIR}/hpp/corbaserver/rbprm/tools/plot_analytics.py
+ ${CMAKE_CURRENT_SOURCE_DIR}/hpp/corbaserver/rbprm/tools/cwc_trajectory.py
DESTINATION ${PYTHON_SITELIB}/hpp/corbaserver/rbprm/tools
)
# Stand alone corba server
diff --git a/src/hpp/corbaserver/rbprm/rbprmfullbody.py b/src/hpp/corbaserver/rbprm/rbprmfullbody.py
index 90edd25d..7b6f53c6 100755
--- a/src/hpp/corbaserver/rbprm/rbprmfullbody.py
+++ b/src/hpp/corbaserver/rbprm/rbprmfullbody.py
@@ -243,7 +243,7 @@ class FullBody (object):
# \param robustnessTreshold minimum value of the static equilibrium robustness criterion required to accept the configuration (0 by default).
def computeContactPoints(self, stateId):
rawdata = self.client.rbprm.rbprm.computeContactPoints(stateId)
- return [[b[i:i+6] for i in range(0, len(b), 6)] for b in rawdata]
+ return [[b[i:i+3] for i in range(0, len(b), 6)] for b in rawdata], [[b[i+3:i+6] for i in range(0, len(b), 6)] for b in rawdata]
## Given start and goal states
diff --git a/src/hpp/corbaserver/rbprm/tools/cwc_trajectory.py b/src/hpp/corbaserver/rbprm/tools/cwc_trajectory.py
new file mode 100644
index 00000000..8072c9f1
--- /dev/null
+++ b/src/hpp/corbaserver/rbprm/tools/cwc_trajectory.py
@@ -0,0 +1,51 @@
+from cwc import cone_optimization
+import numpy as np
+
+def __get_com(robot, config):
+ save = robot.getCurrentConfig()
+ robot.setCurrentConfig(config)
+ com = robot.getCenterOfMass()
+ robot.setCurrentConfig(save)
+ return com
+
+def gen_trajectory(fullBody, states, state_id, mu = 1, reduce_ineq = True):
+ init_com = __get_com(fullBody, states[state_id])
+ end_com = __get_com(fullBody, states[state_id+1])
+ p, N = fullBody.computeContactPoints(state_id)
+ mass = fullBody.getMass()
+ t_end_phases = [0]
+ [t_end_phases.append(t_end_phases[-1]+0.5) for _ in range(len(p))]
+ print t_end_phases
+ dt = 0.1
+ return cone_optimization(p, N, [init_com + [0,0,0], end_com + [0,0,0]], t_end_phases[1:], dt, mu, mass, 9.81, reduce_ineq)
+
+def draw_trajectory(fullBody, states, state_id, mu = 1, reduce_ineq = True ):
+ var_final, params = gen_trajectory(fullBody, states, state_id, mu , reduce_ineq)
+ p, N = fullBody.computeContactPoints(state_id)
+ print var_final
+ import numpy as np
+ from mpl_toolkits.mplot3d import Axes3D
+ import matplotlib.pyplot as plt
+
+ fig = plt.figure()
+ ax = fig.add_subplot(111, projection='3d')
+ n = 100
+ points = var_final['x']
+ xs = [points[i] for i in range(0,len(points),6)]
+ ys = [points[i] for i in range(1,len(points),6)]
+ zs = [points[i] for i in range(2,len(points),6)]
+ ax.scatter(xs, ys, zs, c='b')
+
+ colors = ["r", "b", "g"]
+ #print contact points of first phase
+ for id_c, points in enumerate(p):
+ xs = [point[0] for point in points]
+ ys = [point[1] for point in points]
+ zs = [point[2] for point in points]
+ ax.scatter(xs, ys, zs, c=colors[id_c])
+
+ ax.set_xlabel('X Label')
+ ax.set_ylabel('Y Label')
+ ax.set_zlabel('Z Label')
+
+ plt.show()
--
GitLab