Commit 803738f1 authored by Steve Tonneau's avatar Steve Tonneau
Browse files

adapt trajectory temporality depending on effector distance traveled

parent a03d84be
......@@ -150,6 +150,13 @@ module hpp
/// \return the value computed for the given sample and analytics
double getSampleValue(in string limbName, in string valueName, in unsigned short sampleId) raises (Error);
/// compute the distance between all effectors replaced between two states
/// does not account for contact not present in both states
/// \param state1 from state
/// \param state2 destination state
/// \return the value computed for the given sample and analytics
double getEffectorDistance(in unsigned short state1, in unsigned short state2) raises (Error);
/// Generate all possible contact in a given configuration
/// \param dofArray initial configuration of the robot
/// \param direction desired direction of motion for the robot
......
......@@ -158,6 +158,14 @@ class FullBody (object):
# array of size 4, where each entry is the position of a corner of the contact surface
def getEffectorPosition(self, limbName, configuration):
return self.client.rbprm.rbprm.getEffectorPosition(limbName,configuration)
##compute the distance between all effectors replaced between two states
# does not account for contact not present in both states
# \param state1 from state
# \param state2 destination state
# \return the value computed for the given sample and analytics
def getEffectorDistance(self, state1, state2):
return self.client.rbprm.rbprm.getEffectorDistance(state1,state2)
## Generates a balanced contact configuration, considering the
# given current configuration of the robot, and a direction of motion.
......@@ -305,6 +313,7 @@ class FullBody (object):
# \return H matrix and h column, such that H w <= h
def getContactCone(self, stateId, friction = 0.5):
H_h = array(self.client.rbprm.rbprm.getContactCone(stateId, friction))
#~ print "H_h", len(H_h)
# now decompose cone
return H_h[:,:-1], H_h[:, -1]
......
......@@ -2,6 +2,7 @@ from cwc import cone_optimization
from obj_to_constraints import ineq_from_file, rotate_inequalities
import numpy as np
import math
from numpy.linalg import norm
import hpp.corbaserver.rbprm.data.com_inequalities as ine
from hpp.corbaserver.rbprm.tools.path_to_trajectory import gen_trajectory_to_play
......@@ -58,13 +59,21 @@ def __get_com_constraint(fullBody, state, config, limbsCOMConstraints, interm =
return [np.vstack(As), np.hstack(bs)]
def gen_trajectory(fullBody, states, state_id, computeCones = False, mu = 1, dt=0.2, reduce_ineq = True, verbose = False, limbsCOMConstraints = None):
def gen_trajectory(fullBody, states, state_id, computeCones = False, mu = 1, dt=0.2, phase_dt = [0.4, 1], reduce_ineq = True, verbose = False, limbsCOMConstraints = None):
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()
fly_time = phase_dt [1]
support_time = phase_dt [0]
t_end_phases = [0]
[t_end_phases.append(t_end_phases[-1]+1) for _ in range(len(p))]
[t_end_phases.append(t_end_phases[-1]+fly_time) for _ in range(len(p))]
if(len(t_end_phases) == 4):
t_end_phases[1] = support_time
t_end_phases[2] = t_end_phases[1] + fly_time
t_end_phases[3] = t_end_phases[2] + support_time
t_end_phases = [float((int)(math.ceil(el*10.))) / 10. for el in t_end_phases]
print "end_phases", t_end_phases
cones = None
if(computeCones):
cones = [fullBody.getContactCone(state_id, mu)[0]]
......@@ -86,8 +95,8 @@ def gen_trajectory(fullBody, states, state_id, computeCones = False, mu = 1, dt=
return cone_optimization(p, N, [init_com + [0,0,0], end_com + [0,0,0]], t_end_phases[1:], dt, cones, COMConstraints, mu, mass, 9.81, reduce_ineq, verbose)
def draw_trajectory(fullBody, states, state_id, computeCones = False, mu = 1, dt=0.2, reduce_ineq = True, verbose = False, limbsCOMConstraints = None):
var_final, params = gen_trajectory(fullBody, states, state_id, computeCones, mu , dt, reduce_ineq, verbose, limbsCOMConstraints)
def draw_trajectory(fullBody, states, state_id, computeCones = False, mu = 1, dt=0.2, phase_dt = [0.4, 1], reduce_ineq = True, verbose = False, limbsCOMConstraints = None):
var_final, params = gen_trajectory(fullBody, states, state_id, computeCones, mu , dt, phase_dt, reduce_ineq, verbose, limbsCOMConstraints)
p, N = fullBody.computeContactPoints(state_id)
from mpl_toolkits.mplot3d import Axes3D
import matplotlib.pyplot as plt
......@@ -113,26 +122,86 @@ def draw_trajectory(fullBody, states, state_id, computeCones = False, mu = 1, d
ax.set_ylabel('Y Label')
ax.set_zlabel('Z Label')
plt.show()
print "plotting speed "
print "end target ", params['x_end']
fig = plt.figure()
ax = fig.add_subplot(111)
points = var_final['dc']
print "points", points
ys = [norm(el) for el in points]
xs = [i * params['dt'] for i in range(0,len(points))]
ax.scatter(xs, ys, c='b')
plt.show()
print "plotting acceleration "
fig = plt.figure()
ax = fig.add_subplot(111)
points = var_final['ddc']
ys = [norm(el) for el in points]
xs = [i * params['dt'] for i in range(0,len(points))]
ax.scatter(xs, ys, c='b')
plt.show()
plt.show()
return var_final, params
def solve_com_RRT(fullBody, states, state_id, computeCones = False, mu = 1, dt =0.1, reduce_ineq = True, num_optims = 0, draw = False, verbose = False, limbsCOMConstraints = None):
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, resultDic = {}):
print "callgin gen ",state_id
if(draw):
res = draw_trajectory(fullBody, states, state_id, computeCones, mu, dt, reduce_ineq, verbose, limbsCOMConstraints)
res = draw_trajectory(fullBody, states, state_id, computeCones, mu, dt, phase_dt, reduce_ineq, verbose, limbsCOMConstraints)
else:
res = gen_trajectory(fullBody, states, state_id, computeCones, mu, dt, reduce_ineq, verbose, limbsCOMConstraints)
res = gen_trajectory(fullBody, states, state_id, computeCones, mu, dt, phase_dt, reduce_ineq, verbose, limbsCOMConstraints)
t = res[1]["t_init_phases"];
dt = res[1]["dt"];
final_state = res[0]
c0 = res[1]["x_init"][0:3]
comPos = [c0] + [c.tolist() for c in final_state['c']]
comPosPerPhase = [[comPos[(int)(t_id/dt)] for t_id in np.arange(t[index],t[index+1],dt)] for index, _ in enumerate(t[:-1]) ]
comPosPerPhase = [[comPos[(int)(t_id/dt)] for t_id in np.arange(t[index],t[index+1]-_EPS,dt)] for index, _ in enumerate(t[:-1]) ]
comPosPerPhase[-1].append(comPos[-1])
assert(len(comPos) == len(comPosPerPhase[0]) + len(comPosPerPhase[1]) + len(comPosPerPhase[2]))
#~ assert(comPos == [item for sublist in comPosPerPhase for item in sublist])
resultDic[str(state_id)] = comPosPerPhase
return comPosPerPhase
def solve_com_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):
comPosPerPhase = __optim__threading_ok(fullBody, states, state_id, computeCones, mu, dt, phase_dt, reduce_ineq, num_optims, draw, verbose, limbsCOMConstraints)
print "done. generating state trajectory ",state_id
print "comePhaseLength", len(comPosPerPhase[0]), len(comPosPerPhase[1]), len(comPosPerPhase[2])
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
return paths_ids[-1], paths_ids[:-1]
#~ from multiprocessing import Process
#~ def solve_com_RRTs(fullBody, states, state_ids, computeCones = False, mu = 1, dt =0.1, phase_dt = 1, reduce_ineq = True, num_optims = 0, draw = False, verbose = False, limbsCOMConstraints = None):
#~ results = {}
#~ processes = {}
#~ allpathsids =[[],[]]
#~ errorid = []
#~ for sid in state_ids:
#~ pid = str(sid)
#~ p = Process(target=__optim__threading_ok, args=(fullBody, states, sid, computeCones, mu, dt, phase_dt, reduce_ineq, num_optims, draw, verbose, limbsCOMConstraints, results))
#~ processes[str(sid)] = p
#~ p.start()
#~ for i,p in processes.iteritems():
#~ p.join()
#~ print results
#~ print "done. generating state trajectory "
#~ for sid in state_ids:
#~ comPosPerPhase = results[str(sid)]
#~ try:
#~ 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
#~ allpathsids[0].append(paths_ids[-1])
#~ allpathsids[1].append(paths_ids[:-1])
#~ except:
#~ errorid += [i]
#~ print "errors at states: "; errorid
#~ return allpathsids[0], allpathsids[1]
......@@ -56,16 +56,18 @@ def follow_trajectory_path(robot, path_player, path_id, total_time, dt_framerate
dt_finals = [dt*i / length for i in range(int(num_frames_required))] + [1]
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_path, dt_framerate=__24fps):
def gen_trajectory_to_play(robot, path_player, path_ids, total_time_per_paths, dt_framerate=__24fps):
config_size = len(robot.getCurrentConfig())
res = []
pp = path_player
activeid = 0
for path_id in path_ids:
config_size_path = len(path_player.client.problem.configAtParam (path_id, 0))
if(config_size_path > config_size):
res+= follow_trajectory_path(robot, path_player, path_id, total_time_per_path, dt_framerate)
res+= follow_trajectory_path(robot, path_player, path_id, total_time_per_paths[1], dt_framerate)
else:
res+= linear_interpolate_path(robot, path_player, path_id, total_time_per_path, dt_framerate)
res+= linear_interpolate_path(robot, path_player, path_id, total_time_per_paths[0], dt_framerate)
activeid +=1
return res
import time
......
......@@ -405,6 +405,24 @@ namespace hpp {
return cit->second[sampleId];
}
double RbprmBuilder::getEffectorDistance(unsigned short state1, unsigned short state2) throw (hpp::Error)
{
try
{
std::size_t s1((std::size_t)state1), s2((std::size_t)state2);
if(lastStatesComputed_.size () < s1 || lastStatesComputed_.size () < s2 )
{
throw std::runtime_error ("did not find a states at indicated indices: " + std::string(""+s1) + ", " + std::string(""+s2));
}
return rbprm::effectorDistance(lastStatesComputed_[s1], lastStatesComputed_[s2]);
}
catch(std::runtime_error& e)
{
throw Error(e.what());
}
}
std::vector<std::string> stringConversion(const hpp::Names_t& dofArray)
{
std::vector<std::string> res;
......@@ -944,6 +962,10 @@ namespace hpp {
try
{
T_Configuration positions = doubleDofArrayToConfig(3, rootPositions);
if(positions.size() <2)
{
throw std::runtime_error("generateComPath requires at least 2 configurations to generate path");
}
return addRotations(positions,q1,q2,fullBody->device_->currentConfiguration(),
fullBody->device_,problemSolver->problem());
}
......@@ -1214,7 +1236,7 @@ assert(s2 == s1 +1);
if(!(problemSolver_->problem()->configValidations()->validate(s1Ter.configuration_, rport)
&& problemSolver_->problem()->configValidations()->validate(s2Bis.configuration_, rport)))
{
//std::cout << "could not project without collision at state " << std::string(""+s1) << std::endl;
std::cout << "could not project without collision at state " << std::string(""+s1) << std::endl;
throw std::runtime_error ("could not project without collision at state " + std::string(""+s1));
// fallback to limbRRT instead
//return -1; //limbRRT(s1, s2, numOptimizations);
......
......@@ -121,6 +121,7 @@ namespace hpp {
virtual CORBA::UShort getNumSamples(const char* limb) throw (hpp::Error);
virtual hpp::floatSeq* getOctreeNodeIds(const char* limb) throw (hpp::Error);
virtual double getSampleValue(const char* limb, const char* valueName, unsigned short sampleId) throw (hpp::Error);
virtual double getEffectorDistance(unsigned short state1, unsigned short state2) throw (hpp::Error);
virtual hpp::floatSeq* generateContacts(const hpp::floatSeq& configuration,
const hpp::floatSeq& direction) throw (hpp::Error);
......
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