Commit 73e4eec2 authored by Steve Tonneau's avatar Steve Tonneau
Browse files

starting trajectory playing at right time values

parent e4be3062
......@@ -110,6 +110,7 @@ INSTALL(
${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
${CMAKE_CURRENT_SOURCE_DIR}/hpp/corbaserver/rbprm/tools/path_to_trajectory.py
${CMAKE_CURRENT_SOURCE_DIR}/hpp/corbaserver/rbprm/tools/obj_to_constraints.py
DESTINATION ${PYTHON_SITELIB}/hpp/corbaserver/rbprm/tools
)
......
__24fps = 1. / 24.
__EPS = 0.1
from numpy.linalg import norm
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
num_frames_required = total_time / dt_framerate + 1
dt_finals = [dt_final*i for i in range(int(num_frames_required))]
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]]
def __find_q_t(robot, path_player, path_id, t):
u = t
current_t = -1
length = pp.client.problem.pathLength (path_id)
q = []
a = 0.
b = length
#~ print [pp.client.problem.configAtParam (path_id, i)[-1] for i in [length / 5. *j for j in range(6)]]
while(abs(current_t -t )> __EPS):
current_t = pp.client.problem.configAtParam (path_id, u)[-1]
print "current_t",current_t
print "t",t
if(current_t - t) > __EPS:
print "upb",b
b = u
print "upb",b
elif (t - current_t) > __EPS:
print "upa",a
a = u
print "upa",a
if (pp.client.problem.configAtParam (path_id, a)[-1]) > t and (pp.client.problem.configAtParam (path_id, b)[-1] > t):
print "error, tout au dessus!!!!!!!!!!!!!!!"
return 0
if (pp.client.problem.configAtParam (path_id, a)[-1]) < t and (pp.client.problem.configAtParam (path_id, b)[-1] < t):
print "error, tout en dessous!!!!!!!!!!!!!!!"
return 0
#~ print "cassos"
print "current_l", u
u = (b-a)/2.
print "current_l", u
#~ return 0
#~ print "current_t", current_t
#~ print "t", t
#~ print "a", a
#~ print "b", b
#~ print "length", length
print "gagne"
return q[:-1]
def linear_interpolate_path(robot, path_player, path_id, total_time, dt_framerate=__24fps):
pp = path_player
length = pp.client.problem.pathLength (path_id)
num_frames_required = total_time / dt_framerate
dt = float(length) / num_frames_required
dt_finals = [dt*i for i in range(int(num_frames_required))] + [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
length = pp.client.problem.pathLength (path_id)
num_frames_required = total_time / dt_framerate
dt = float(length) / num_frames_required
#matches the extradof normalized
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 playCOM(pathId):
#~ length = pp.end*pp.client.problem.pathLength (pathId)
#~ t = pp.start*pp.client.problem.pathLength (pathId)
#~ while t < length :
#~ start = time.time()
#~ q = pp.client.problem.configAtParam (pathId, t)
#~ fullBody.setCurrentConfig(q)
#~ q[0:3] = fullBody.getCenterOfMass()
#~ pp.publisher.robotConfig = q
#~ pp.publisher.publishRobots ()
#~ t += (pp.dt * pp.speed)
#~ elapsed = time.time() - pp.start
#~ if elapsed < pp.dt :
#~ time.sleep(pp.dt-elapsed))
import numpy as np
com_waypoints = [np.array([i,i,i]) for i in range(6)]
dt = 0.2
dt_framerate = __24fps
res = __gen_frame_positions(com_waypoints, dt, dt_framerate)
......@@ -28,6 +28,9 @@
#include "hpp/rbprm/sampling/sample-db.hh"
#include "hpp/model/urdf/util.hh"
#include "hpp/core/straight-path.hh"
#include "hpp/core/config-validations.hh"
#include "hpp/core/collision-validation-report.hh"
#include <hpp/core/subchain-path.hh>
#include <fstream>
......@@ -693,12 +696,6 @@ namespace hpp {
}
bool success;
State intermediaryState = intermediary(lastStatesComputed_[stateId], lastStatesComputed_[stateId+1],stateId,success);
for(std::map<std::string, fcl::Vec3f>::const_iterator cit = intermediaryState.contactPositions_.begin();
cit != intermediaryState.contactPositions_.end(); ++cit)
{
std::cout << cit->first << std::endl;
std::cout << "WTF " << std::endl;
}
if(!success)
{
throw std::runtime_error ("No contact breaks, hence no intermediate state from state " + std::string(""+(stateId)));
......@@ -1077,26 +1074,58 @@ assert(s2 == s1 +1);
resPath->appendPath(interpolation::comRRT(fullBody_,problemSolver_->problem(), paths[i],
*(cit),*(cit+1), numOptimizations));
}*/
ValidationReportPtr_t rport (ValidationReportPtr_t(new CollisionValidationReport));
fullBody_->device_->currentConfiguration(s1Ter.configuration_);
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;
throw std::runtime_error ("could not project without collision at state " + std::string(""+s1));
// fallback to limbRRT instead
//return -1; //limbRRT(s1, s2, numOptimizations);
}
core::WeighedDistancePtr_t distance = core::WeighedDistance::create(fullBody_->device_);
/*resPath->appendPath(core::StraightPath::create(fullBody_->device_,state1.configuration_,s1Bis.configuration_,
(*distance)(state1.configuration_,s1Bis.configuration_)));
resPath->appendPath(core::StraightPath::create(fullBody_->device_,s1Bis.configuration_,s1Ter.configuration_,
(*distance)(s1Bis.configuration_,s1Ter.configuration_)));*/
resPath->appendPath(core::StraightPath::create(fullBody_->device_,state1.configuration_,s1Ter.configuration_, 0.5));
//(*distance)(state1.configuration_,s1Ter.configuration_)));
resPath->appendPath(interpolation::comRRT(fullBody_,problemSolver_->problem(), paths[1],
s1Ter,s2Bis, numOptimizations));
//resPath->appendPath(core::StraightPath::create(fullBody_->device_,state1.configuration_,s1Ter.configuration_, 0.5));
if(state1.configuration_ != s1Ter.configuration_)
{
core::PathPtr_t p1 = interpolation::comRRT(fullBody_,problemSolver_->problem(), paths[0],
state1,s1Ter, numOptimizations,false);
resPath->appendPath(p1);
AddPath(p1,problemSolver_);
}
core::PathPtr_t p2 =interpolation::comRRT(fullBody_,problemSolver_->problem(), paths[1],
s1Ter,s2Bis, numOptimizations,true);
AddPath(p2,problemSolver_);
// reduce path to remove extradof
core::SizeInterval_t interval(0, p2->initial().rows()-1);
core::SizeIntervals_t intervals;
intervals.push_back(interval);
PathPtr_t reducedPath = core::SubchainPath::create(p2,intervals);
resPath->appendPath(reducedPath);
/*resPath->appendPath(core::StraightPath::create(fullBody_->device_,s2Bis.configuration_,s2Ter.configuration_,
(*distance)(s2Bis.configuration_,s2Ter.configuration_)));
resPath->appendPath(core::StraightPath::create(fullBody_->device_,s2Ter.configuration_,state2.configuration_,
(*distance)(s2Ter.configuration_,state2.configuration_)));*/
resPath->appendPath(core::StraightPath::create(fullBody_->device_,s2Bis.configuration_,state2.configuration_, 0.5));
//(*distance)(s2Bis.configuration_,state2.configuration_)));
//resPath->appendPath(core::StraightPath::create(fullBody_->device_,s2Bis.configuration_,state2.configuration_, 0.5));;
if(s2Bis.configuration_ != state2.configuration_)
{
core::PathPtr_t p3= interpolation::comRRT(fullBody_,problemSolver_->problem(), paths[1],
s2Bis,state2, numOptimizations,false);
resPath->appendPath(p3);
AddPath(p3,problemSolver_);
}
/*T_State tg; tg.push_back(s1Bis);
tg.push_back(s2Bis);
resPath->appendPath(interpolation::limbRRT(fullBody_,problemSolver_->problem(),
tg.begin(),tg.begin()+1, numOptimizations));*/
return AddPath(resPath,problemSolver_);
return AddPath(reducedPath,problemSolver_);
}
catch(std::runtime_error& e)
{
......
Markdown is supported
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