Commit 9e79f7ab authored by Steve Tonneau's avatar Steve Tonneau
Browse files

optimization

parent 32d4ac58
......@@ -129,11 +129,13 @@ install(FILES
data/meshes/darpareduced.stl
data/meshes/car.stl
data/meshes/car_simple.stl
data/meshes/ground.stl
data/meshes/ground_table.stl
data/meshes/hrp2_trunk.stl
data/meshes/hrp2_trunk_body.stl
data/meshes/hrp2_trunk_torso.stl
data/meshes/hrp2_trunk_body_view.dae
data/meshes/hrp2_trunk_torso_view.dae
data/meshes/hrp2_trunk_body_view.dae
data/meshes/hrp2_trunk_torso_view.dae
data/meshes/hrp2_rom.stl
data/meshes/hrp2_larm_rom.stl
data/meshes/hrp2_rarm_rom.stl
......@@ -149,7 +151,7 @@ install(FILES
data/meshes/truck.stl
data/meshes/truck_vision.stl
data/meshes/stair_bauzil.stl
data/meshes/stair_bauzil_reduced.stl
data/meshes/stair_bauzil_reduced.stl
data/meshes/climb.stl
data/meshes/stepladder.stl
data/meshes/chair.stl
......
<?xml version="1.0"?>
<robot name="scene_wall2">
<handle name="handle">
<position> 0 0 0 1 0 0 0 </position>
<link name="base_link"/>
</handle>
<handle name="handle2">
<position> 0 0 0
0 0 0.7071067811865476 0.7071067811865476 </position>
<link name="base_link"/>
</handle>
<contact name="box_surface">
<link name="base_link"/>
<point>-0.025 -0.025 -0.025 -0.025 0.025 -0.025 -0.025 -0.025 0.025 -0.025 0.025 0.025
+0.025 -0.025 -0.025 +0.025 0.025 -0.025 +0.025 -0.025 0.025 +0.025 0.025 0.025 </point>
<triangle> 0 2 1 4 5 6</triangle>
</contact>
</robot>
......@@ -3,7 +3,7 @@
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="package:///../hpp-rbprm-corba/meshes/ground.stl"/>
<mesh filename="package://hpp-rbprm-corba/meshes/ground.stl"/>
</geometry>
<material name="white">
<color rgba="1 1 1 1"/>
......
<robot name="scene_wall2">
<link name="base_link">
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="package://hpp-rbprm-corba/meshes/ground_table2.stl"/>
</geometry>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="package://hpp-rbprm-corba/meshes/ground_table2.stl"/>
</geometry>
</collision>
</link>
</robot>
......@@ -291,6 +291,24 @@ module hpp
/// \return id of the root path computed
short comRRT(in double state1, in double state2, in unsigned short comPath, in unsigned short numOptimizations) raises (Error);
/// Provided a path has already been computed and interpolated, generate a continuous path
/// between two indicated states. The states do not need to be consecutive, but increasing in Id.
/// Will fail if the index of the states do not exist
/// The path of the COM of thr robot and limbs not considered by the contact transitions between
/// two states is assumed to be already computed, and registered in the solver under the id specified by the user.
/// It must be valid in the sense of the active PathValidation.
/// \param state1 index of first state.
/// \param rootPositions1 com positions to track for 1st phase
/// \param rootPositions1 com positions to track for 2nd phase
/// \param rootPositions1 com positions to track for 3rd phase
/// \param numOptimizations Number of iterations of the shortcut algorithm to apply between each states
/// \return id of the root path computed
short comRRTFromPos(in double state1,
in floatSeqSeq rootPositions1,
in floatSeqSeq rootPositions2,
in floatSeqSeq rootPositions3,
in unsigned short numOptimizations) raises (Error);
/// Project a given state into a given COM position
/// between two indicated states. The states do not need to be consecutive, but increasing in Id.
/// Will fail if the index of the state does not exist.
......
......@@ -82,7 +82,7 @@ r(q_init)
# computing the contact sequence
configs = fullBody.interpolate(0.1, 1, 0)
r.loadObstacleModel ('hpp-rbprm-corba', "darpa", "contact")
#~ r.loadObstacleModel ('hpp-rbprm-corba', "darpa", "contact")
# calling draw with increasing i will display the sequence
i = 0;
......@@ -90,3 +90,106 @@ fullBody.draw(configs[i],r); i=i+1; i-1
from hpp.gepetto import PathPlayer
pp = PathPlayer (ps.robot.client.basic, r)
from hpp.corbaserver.rbprm.tools.cwc_trajectory import *
pid = 3
def optimize_animate(i):
res = draw_trajectory(fullBody, configs, i, False, 0.5, False)
#~ res = gen_trajectory(fullBody, configs, i, False, 1, False)
pos = [c.tolist() for c in res[0]['c']]
fullBody.generateRootPathStates(pos, configs[i], configs[i+1])
fullBody.interpolateBetweenStatesFromPath(i,i+1,2,0)
fullBody.interpolateBetweenStatesFromPath(i,i+1,2,1)
fullBody.interpolateBetweenStatesFromPath(i,i+1,2,10)
fullBody.interpolateBetweenStatesFromPath(i,i+1,2,50)
global pid
#~ pp(pid);pp(pid+1);pp(pid+2);
pid = pid + 3
print "diff position"
from numpy import array
#~ print (array(pos[-1]) - array(configs[6][0:3]))
#~ print (array(pos[-1]) - array(configs[6][0:3])).norm()
def play(i):
fullBody.setCurrentConfig(configs[i])
r(configs[i])
import time
for j,_ in enumerate (pos):
q=fullBody.getCurrentConfig()
q[0:3] = pos[j]
r(q)
time.sleep(0.25)
q[3:] = configs[i+1][3:]
r(q)
res = []
def displayComPath(pathId,color=[0.,0.75,0.15,0.9]) :
pathPos=[]
length = pp.end*pp.client.problem.pathLength (pathId)
t = pp.start*pp.client.problem.pathLength (pathId)
while t < length :
q = pp.client.problem.configAtParam (pathId, t)
pp.publisher.robot.setCurrentConfig(q)
q = pp.publisher.robot.getCenterOfMass()
pathPos = pathPos + [q[:3]]
t += pp.dt
nameCurve = "path_"+str(pathId)+"_com"
pp.publisher.client.gui.addCurve(nameCurve,pathPos,color)
pp.publisher.client.gui.addToGroup(nameCurve,pp.publisher.sceneName)
pp.publisher.client.gui.refresh()
res = []
from hpp import Error as hpperr
import sys
def act(i, optim):
try:
pid = solve_com_RRT(fullBody, configs, i, True, 0.3, 0.2, False, optim, False, True)
displayComPath(pid)
#~ pp(pid)
global res
res = res + [pid]
except hpperr as e:
print "failed at id " + str(i) , e.strerror
except ValueError as e:
print "failed at id " + str(i) , e
except IndexError as e:
print "failed at id " + str(i) , e
except Exception as e:
print e
except:
return
for i in range(30,47):
act(i, 50)
#~ for i in range(5,35):
#~ act(i, 50)
def displayInSave(pp, pathId, configs):
length = pp.end*pp.client.problem.pathLength (pathId)
t = pp.start*pp.client.problem.pathLength (pathId)
while t < length :
q = pp.client.problem.configAtParam (pathId, t)
configs.append(q)
t += (pp.dt * pp.speed)
respath = []
for p in res:
print p
displayInSave(pp,p, respath)
for p in res:
pp(p)
fullBody.exportAll(r, respath, 'darpa_hyq_full');
#~ from hpp.gepetto.blender.exportmotion import exportPath
#~ for p in res:
#~ exportPath(r,fullBody.client.basic.robot,fullBody.client.basic.problem,p,0.1,'test'+str(p)+'.txt')
print "tg"
......@@ -46,7 +46,7 @@ ps.addGoalConfig (q_goal)
from hpp.corbaserver.affordance.affordance import AffordanceTool
afftool = AffordanceTool ()
afftool.loadObstacleModel (packageName, "darpa", "planning", r)
afftool.loadObstacleModel (packageName, "darpa", "pling", r)
afftool.visualiseAffordances('Support', r, [0.25, 0.5, 0.5])
# Choosing RBPRM shooter and path validation methods.
......@@ -62,3 +62,7 @@ t = ps.solve ()
from hpp.gepetto import PathPlayer
pp = PathPlayer (rbprmBuilder.client.basic, r)
#~ pp (1)
q_far = q_init [::]
q_far [0:3] = [-2, -3, 0.63];
r(q_far)
......@@ -339,6 +339,21 @@ class FullBody (object):
def comRRT(self, state1, state2, path, numOptim = 10):
return self.client.rbprm.rbprm.comRRT(state1, state2, path, numOptim)
## Provided a path has already been computed and interpolated, generate a continuous path
# between two indicated states. The states do not need to be consecutive, but increasing in Id.
# Will fail if the index of the states do not exist
# The path of the COM of thr robot and limbs not considered by the contact transitions between
# two states is assumed to be already computed, and registered in the solver under the id specified by the user.
# It must be valid in the sense of the active PathValidation.
# \param state1 index of first state.
# \param rootPositions1 com positions to track for 1st phase
# \param rootPositions1 com positions to track for 2nd phase
# \param rootPositions1 com positions to track for 3rd phase
# \param numOptimizations Number of iterations of the shortcut algorithm to apply between each states
# \return id of the root path computed
def comRRTFromPos(self, state1, comPos1, comPos2, comPos3, numOptim = 10):
return self.client.rbprm.rbprm.comRRTFromPos(state1, comPos1, comPos2, comPos3, numOptim)
## Project a given state into a given COM position
# between two indicated states. The states do not need to be consecutive, but increasing in Id.
......
......@@ -5,19 +5,16 @@ def __get_com(robot, config):
save = robot.getCurrentConfig()
robot.setCurrentConfig(config)
com = robot.getCenterOfMass()
print 'COM', com
#~ com = config[0:3] #assimilate root and com
robot.setCurrentConfig(save)
return com
def gen_trajectory(fullBody, states, state_id, computeCones = False, mu = 1, reduce_ineq = True):
def gen_trajectory(fullBody, states, state_id, computeCones = False, mu = 1, dt=0.2, reduce_ineq = True, verbose = False):
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]+1) for _ in range(len(p))]
dt = 0.1
cones = None
if(computeCones):
cones = [fullBody.getContactCone(state_id, mu)[0]]
......@@ -25,10 +22,11 @@ def gen_trajectory(fullBody, states, state_id, computeCones = False, mu = 1, red
cones.append(fullBody.getContactIntermediateCone(state_id, mu)[0])
if(len(p) > len(cones)):
cones.append(fullBody.getContactCone(state_id+1, mu)[0])
return cone_optimization(p, N, [init_com + [0,0,0], end_com + [0,0,0]], t_end_phases[1:], dt, cones, mu, mass, 9.81, reduce_ineq)
print "num cones ", len(cones)
return cone_optimization(p, N, [init_com + [0,0,0], end_com + [0,0,0]], t_end_phases[1:], dt, cones, mu, mass, 9.81, reduce_ineq, verbose)
def draw_trajectory(fullBody, states, state_id, computeCones = False, mu = 1, reduce_ineq = True):
var_final, params = gen_trajectory(fullBody, states, state_id, computeCones, mu , reduce_ineq)
def draw_trajectory(fullBody, states, state_id, computeCones = False, mu = 1, dt=0.2, reduce_ineq = True, verbose = False):
var_final, params = gen_trajectory(fullBody, states, state_id, computeCones, mu , dt, reduce_ineq, verbose)
p, N = fullBody.computeContactPoints(state_id)
import numpy as np
from mpl_toolkits.mplot3d import Axes3D
......@@ -58,27 +56,28 @@ def draw_trajectory(fullBody, states, state_id, computeCones = False, mu = 1, re
plt.show()
return var_final, params
def solve_com_RRT(fullBody, states, state_id, computeCones = False, mu = 1, reduce_ineq = True, num_optims = 0, draw = False):
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):
print "callgin gen ",state_id
if(draw):
res = draw_trajectory(fullBody, states, state_id, computeCones, mu, reduce_ineq)
res = draw_trajectory(fullBody, states, state_id, computeCones, mu, dt, reduce_ineq, verbose)
else:
res = gen_trajectory(fullBody, states, state_id, computeCones, mu, reduce_ineq)
res = gen_trajectory(fullBody, states, state_id, computeCones, mu, dt, reduce_ineq, verbose)
t = res[1]["t_init_phases"];
dt = res[1]["dt"];
final_state = res[0]
c0 = res[1]["x_init"][0:3]
print "c0", c0
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[-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])
#~ assert(comPos == [item for sublist in comPosPerPhase for item in sublist])
print "done. generating state path ",state_id
pid = fullBody.generateRootPathStates(comPos, states[state_id], states[state_id+1])
print "done. shortcuting ",state_id
return fullBody.comRRTFromPos(state_id,comPosPerPhase[0],comPosPerPhase[1],comPosPerPhase[2],num_optims)
return fullBody.comRRT(state_id,state_id+1,pid,num_optims)
#~ pid = fullBody.generateRootPathStates(comPos, states[state_id], states[state_id+1])
#~ print "done. shortcuting ",state_id
#~
#~ return fullBody.comRRT(state_id,state_id+1,pid,num_optims)
......@@ -736,11 +736,6 @@ namespace hpp {
++pit;
for(;pit != positions.end()-1; ++pit, u+=size_step)
{
std::cout << "position added \n" << ((*pit).head(3)) << std::endl;
if((*pit)(0) > 0.552368)
{
std::cout << "pos devant \n" << (*pit)(0) << std::endl;
}
current = addRotation(pit, u, q1, q2, ref);
res->appendPath(makePath(device,problem, previous, current));
previous = current;
......@@ -751,15 +746,29 @@ namespace hpp {
return res;
}
core::PathVectorPtr_t generateComPath(RbPrmFullBodyPtr_t& fullBody, core::ProblemSolverPtr_t problemSolver, const hpp::floatSeqSeq& rootPositions,
const model::Configuration_t q1, const model::Configuration_t q2) throw (hpp::Error)
{
try
{
T_Configuration positions = doubleDofArrayToConfig(3, rootPositions);
return addRotations(positions,q1,q2,fullBody->device_->currentConfiguration(),
fullBody->device_,problemSolver->problem());
}
catch(std::runtime_error& e)
{
throw Error(e.what());
}
}
CORBA::Short RbprmBuilder::generateRootPath(const hpp::floatSeqSeq& rootPositions,
const hpp::floatSeq& q1Seq, const hpp::floatSeq& q2Seq) throw (hpp::Error)
{
try
{
model::Configuration_t q1 = dofArrayToConfig(4, q1Seq), q2 = dofArrayToConfig(4, q2Seq);
T_Configuration positions = doubleDofArrayToConfig(3, rootPositions);
return problemSolver_->addPath(addRotations(positions,q1,q2,fullBody_->device_->currentConfiguration(),
fullBody_->device_,problemSolver_->problem()));
return problemSolver_->addPath(generateComPath(fullBody_, problemSolver_, rootPositions, q1, q2));
}
catch(std::runtime_error& e)
{
......@@ -789,10 +798,29 @@ namespace hpp {
lx, -ly, 0,
-lx, -ly, 0,
-lx, ly, 0;
const fcl::Matrix3f& fclRotation = state.contactRotation_.at(name);
for(int i =0; i< 3; ++i)
//create rotation matrix from normal
fcl::Vec3f z_fcl = state.contactNormals_.at(name);
Eigen::Vector3d z,x,y;
for(int i =0; i<3; ++i) z[i] = z_fcl[i];
x = z.cross(Eigen::Vector3d(0,-1,0));
if(x.norm() < 10e-6)
{
y = z.cross(fcl::Vec3f(1,0,0));
y.normalize();
x = y.cross(z);
}
else
{
x.normalize();
y = z.cross(x);
}
R.block<3,1>(0,0) = x;
R.block<3,1>(0,1) = y;
R.block<3,1>(0,2) = z;
//const fcl::Matrix3f& fclRotation = state.contactRotation_.at(name);
/*for(int i =0; i< 3; ++i)
for(int j =0; j<3;++j)
R(i,j) = fclRotation(i,j);
R(i,j) = fclRotation(i,j);*/
for(std::size_t i =0; i<4; ++i)
{
res.push_back(position + (R*p.row(i).transpose()));
......@@ -996,6 +1024,79 @@ assert(s2 == s1 +1);
}
}
CORBA::Short RbprmBuilder::comRRTFromPos(double state1,
const hpp::floatSeqSeq& rootPositions1,
const hpp::floatSeqSeq& rootPositions2,
const hpp::floatSeqSeq& rootPositions3,
unsigned short numOptimizations) throw (hpp::Error)
{
try
{
std::size_t s1((std::size_t)state1), s2((std::size_t)state1+1);
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));
}
const State& state1=lastStatesComputed_[s1], state2=lastStatesComputed_[s2];
// first compute com paths.
std::vector<core::PathVectorPtr_t> paths;
paths.push_back(generateComPath(fullBody_,problemSolver_,rootPositions1,state1.configuration_.segment<4>(3), state2.configuration_.segment<4>(3)));
paths.push_back(generateComPath(fullBody_,problemSolver_,rootPositions2,state1.configuration_.segment<4>(3), state2.configuration_.segment<4>(3)));
paths.push_back(generateComPath(fullBody_,problemSolver_,rootPositions3,state1.configuration_.segment<4>(3), state2.configuration_.segment<4>(3)));
std::vector<State> states;
states.push_back(state1);
State s1Bis(state1);
s1Bis.configuration_ = rbprm::interpolation::projectOnCom(fullBody_, problemSolver_->problem(),s1Bis,paths[0]->end().head<3>());
states.push_back(s1Bis);
State s1Ter(s1Bis);
s1Ter.configuration_ = rbprm::interpolation::projectOnCom(fullBody_, problemSolver_->problem(),s1Ter,paths[1]->initial().head<3>());
states.push_back(s1Ter);
State s2Bis(state2);
s2Bis.configuration_ = rbprm::interpolation::projectOnCom(fullBody_, problemSolver_->problem(),s2Bis,paths[1]->end().head<3>());
states.push_back(s2Bis);
State s2Ter(s2Bis);
s2Ter.configuration_ = rbprm::interpolation::projectOnCom(fullBody_, problemSolver_->problem(),s2Ter,paths[2]->initial().head<3>());
states.push_back(s2Ter);
states.push_back(state2);
std::size_t i =0;
core::PathVectorPtr_t resPath = core::PathVector::create(fullBody_->device_->configSize(), fullBody_->device_->numberDof());
/*for(std::vector<State>::const_iterator cit = states.begin(); cit != states.end();cit=cit+2, ++i)
{
resPath->appendPath(interpolation::comRRT(fullBody_,problemSolver_->problem(), paths[i],
*(cit),*(cit+1), 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_,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_)));
/*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_);
}
catch(std::runtime_error& e)
{
throw Error(e.what());
}
}
hpp::floatSeq* RbprmBuilder::projectToCom(double state, const hpp::floatSeq& targetCom) throw (hpp::Error)
{
try
......
......@@ -149,6 +149,11 @@ namespace hpp {
virtual CORBA::Short limbRRT(double state1, double state2, unsigned short numOptimizations) throw (hpp::Error);
virtual CORBA::Short limbRRTFromRootPath(double state1, double state2, unsigned short path, unsigned short numOptimizations) throw (hpp::Error);
virtual CORBA::Short comRRT(double state1, double state2, unsigned short path, unsigned short numOptimizations) throw (hpp::Error);
virtual CORBA::Short comRRTFromPos(double state1,
const hpp::floatSeqSeq& rootPositions1,
const hpp::floatSeqSeq& rootPositions2,
const hpp::floatSeqSeq& rootPositions3,
unsigned short numOptimizations) throw (hpp::Error);
virtual hpp::floatSeq* projectToCom(double state, const hpp::floatSeq& targetCom) throw (hpp::Error);
virtual void saveComputedStates(const char* filepath) throw (hpp::Error);
virtual void saveLimbDatabase(const char* limbname,const char* filepath) 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