Commit ce31ce49 authored by Steve Tonneau's avatar Steve Tonneau
Browse files

integrating trajectory optimization method

parent 39a0cae9
......@@ -109,6 +109,7 @@ INSTALL(
DESTINATION ${PYTHON_SITELIB}/hpp/corbaserver/rbprm/tools
# Stand alone corba server
......@@ -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
from cwc import cone_optimization
import numpy as np
def __get_com(robot, config):
save = robot.getCurrentConfig()
com = robot.getCenterOfMass()
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')
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