Commit 1384a29e authored by Steve Tonneau's avatar Steve Tonneau
Browse files

interpolation method for manipulation

parent 39789ced
......@@ -349,11 +349,32 @@ module hpp
/// \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
floatSeq effectorRRT(in double state1,
floatSeq effectorRRT(in double state1,
in floatSeqSeq rootPositions1,
in floatSeqSeq rootPositions2,
in floatSeqSeq rootPositions3,
in unsigned short numOptimizations) raises (Error);
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
floatSeq effectorRRTFromPath(in double state1, in unsigned short refPath,
in double path_from, in double path_to,
in floatSeqSeq rootPositions1,
in floatSeqSeq rootPositions2,
in floatSeqSeq rootPositions3,
in unsigned short numOptimizations,
in Names_t trackedEffectors) 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.
......
from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
from hpp.corbaserver.rbprm.rbprmbuilder import Builder
from hpp.corbaserver.rbprm.rbprmfullbody import FullBody
from hpp.corbaserver import Client
from hpp.gepetto import ViewerFactory
from hpp.gepetto import Viewer
import sys
import manipulation_hrp2_path as manipulation
from os import environ
ins_dir = environ['DEVEL_DIR']
db_dir = ins_dir+"/install/share/hyq-rbprm/database/hyq_"
# Create a new manipulation problem
cl = Client()
cl.problem.selectProblem("rbprm")
packageName = "hrp2_14_description"
meshPackageName = "hrp2_14_description"
rootJointType = "freeflyer"
##
# Information to retrieve urdf and srdf files.
urdfName = "hrp2_14"
urdfSuffix = ""
srdfSuffix = ""
fullBody = FullBody ()
fullBody.loadFullBodyModel(urdfName, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
fullBody.setJointBounds ("base_joint_xyz", [-4,4,-4,4,-4,4])
def manipToRbPrm(qManip):
q = fullBody.getCurrentConfig()
for jn, rk in fullBody.rankInConfiguration.iteritems():
l = fullBody.getJointConfigSize(jn)
if manipulation.robot.rankInConfiguration.has_key("hrp2/" + jn):
rk2 = manipulation.robot.rankInConfiguration["hrp2/" + jn]
q[rk:rk + l] = qManip[rk2:rk2 + l]
else:
print "Joint not set", jn
return q
ps = ProblemSolver( fullBody )
#~ vf = ViewerFactory (ps)
r = Viewer (ps)
from hpp.corbaserver.affordance.affordance import AffordanceTool
afftool = AffordanceTool ()
afftool.loadObstacleModel ("hpp_environments", "hrp2/floor_as_mesh", "floor", r)
afftool.visualiseAffordances('Support', r, [0.25, 0.5, 0.5])
fullBody.client.rbprm.rbprm.setAffordanceFilter('0rLeg', ['Support',])
fullBody.client.rbprm.rbprm.setAffordanceFilter('1lLeg', ['Support'])
#~ vf.loadObstacleModel("hpp_environments", "hrp2/floor_as_mesh", "floor")
cl.problem.selectProblem("manipulationProblem")
ps.client.problem.movePathToProblem(manipulation.ps.numberPaths()-1, "rbprm", ["hrp2/" + jn for jn in fullBody.jointNames])
cl.problem.selectProblem("rbprm")
#~ AFTER loading obstacles
rLegId = '0rLeg'
rLeg = 'RLEG_JOINT0'
rLegOffset = [0,0,0,]
rLegNormal = [0,1,0]
rLegx = 0.09; rLegy = 0.05
# fullBody.addLimb(rLegId,rLeg,'',rLegOffset,rLegNormal, rLegx, rLegy, 10000, "manipulability", 0.1)
fullBody.addLimb(rLegId,rLeg,'',rLegOffset,rLegNormal, rLegx, rLegy, 10000, "static", 0.1)
lLegId = '1lLeg'
lLeg = 'LLEG_JOINT0'
lLegOffset = [0,0,0]
lLegNormal = [0,1,0]
lLegx = 0.09; lLegy = 0.05
# fullBody.addLimb(lLegId,lLeg,'',lLegOffset,rLegNormal, lLegx, lLegy, 10000, "manipulability", 0.1)
fullBody.addLimb(lLegId,lLeg,'',lLegOffset,rLegNormal, lLegx, lLegy, 10000, "static", 0.1)
#~ AFTER loading obstacles
larmId = '4Larm'
larm = 'LARM_JOINT0'
lHand = 'LARM_JOINT5'
lArmOffset = [-0.05,-0.050,-0.050]
lArmNormal = [1,0,0]
lArmx = 0.024; lArmy = 0.024
#~ fullBody.addLimb(larmId,larm,lHand,lArmOffset,lArmNormal, lArmx, lArmy, 10000, 0.05)
rKneeId = '0RKnee'
rLeg = 'RLEG_JOINT0'
rKnee = 'RLEG_JOINT3'
rLegOffset = [0.105,0.055,0.017]
rLegNormal = [-1,0,0]
rLegx = 0.05; rLegy = 0.05
#~ fullBody.addLimb(rKneeId, rLeg,rKnee,rLegOffset,rLegNormal, rLegx, rLegy, 10000, 0.01)
#~
lKneeId = '1LKnee'
lLeg = 'LLEG_JOINT0'
lKnee = 'LLEG_JOINT3'
lLegOffset = [0.105,0.055,0.017]
lLegNormal = [-1,0,0]
lLegx = 0.05; lLegy = 0.05
#~ fullBody.addLimb(lKneeId,lLeg,lKnee,lLegOffset,lLegNormal, lLegx, lLegy, 10000, 0.01)
#~
q_init = manipToRbPrm(manipulation.q_init)
q_goal = manipToRbPrm(manipulation.q_goal)
fullBody.setStartState(q_init,[rLegId,lLegId])
fullBody.setEndState(q_goal,[rLegId,lLegId])
fullBody.runLimbSampleAnalysis(rLegId, "jointLimitsDistance", True)
fullBody.runLimbSampleAnalysis(lLegId, "jointLimitsDistance", True)
configs = fullBody.interpolate(0.15, 0, 10, True)
limbsCOMConstraints = { rLegId : {'file': "hrp2/RL_com.ineq", 'effector' : 'RLEG_JOINT5'},
lLegId : {'file': "hrp2/LL_com.ineq", 'effector' : 'LLEG_JOINT5'}, }
#~ rarmId : {'file': "hrp2/RA_com.ineq", 'effector' : rHand},
#~ larmId : {'file': "hrp2/LA_com.ineq", 'effector' : lHand} }
#~ fullBody.limbRRTFromRootPath(0,len(configs)-1,0,2)
from hpp.corbaserver.rbprm.tools.cwc_trajectory_helper import step, clean,stats, saveAllData, play_traj
from hpp.gepetto import PathPlayer
pp = PathPlayer (fullBody.client.basic, r)
def act(i, numOptim = 0, use_window = 0, friction = 0.5, optim_effectors = True, verbose = False, draw = False, trackedEffectors = []):
return step(fullBody, configs, i, numOptim, pp, limbsCOMConstraints, 0.4, optim_effectors = optim_effectors, time_scale = 20., useCOMConstraints = True, use_window = use_window,
verbose = verbose, draw = draw, trackedEffectors = trackedEffectors)
def play(frame_rate = 1./24.):
play_traj(fullBody,pp,frame_rate)
def saveAll(name):
saveAllData(fullBody, r, name)
def playPaths(rs = None):
import time
ps.client.problem.selectProblem("rbprm")
ls = [ ps.pathLength(i) for i in range(ps.numberPaths()) ]
if rs is None:
rs = [ vf.createViewer() ]
ps.client.problem.selectProblem("manipulationProblem")
rs.append( manipulation.vf.createViewer() )
for i in range(1000):
ps.client.problem.selectProblem("rbprm")
rs[0] (ps.configAtParam(1,i * ls[1] / 1000.))
ps.client.problem.selectProblem("manipulationProblem")
rs[1] (manipulation.ps.configAtParam(0, i * ls[0] / 1000.))
time.sleep(0.5)
return rs
#~ for i in range(1,5):
#~ act(i,60, use_window = 0, optim_effectors = True, draw = False, verbose = True)
trackedEffectors = [0, 0, 0.15, ['LARM_JOINT5']]
# vim: foldmethod=marker foldlevel=2
from hpp.corbaserver.manipulation.hrp2 import Robot
from hpp.corbaserver.manipulation import ProblemSolver as ManipulationProblemSolver, ConstraintGraph, Client as ManipClient
from hpp.gepetto.manipulation import ViewerFactory as ManipulationViewerFactory
from hpp.gepetto import PathPlayer, PathPlayerGui
# Create a new manipulation problem
cl = ManipClient()
cl.problem.selectProblem("manipulationProblem")
class Box (object):
rootJointType = 'freeflyer'
packageName = 'hpp_tutorial'
urdfName = 'box'
urdfSuffix = ""
srdfSuffix = ""
# Robot.urdfSuffix = '_capsule_mesh'
Robot.urdfSuffix = ''
Robot.srdfSuffix = '_manipulation'
# Load HRP2 and a box {{{3
robot = Robot ('hrp2-box', 'hrp2')
ps = ManipulationProblemSolver (robot)
ps.selectPathPlanner ("M-RRT")
vf = ManipulationViewerFactory (ps)
vf.loadObjectModel (Box, 'box')
for d in ["hrp2", "box"]:
robot.setJointBounds (d+"/base_joint_xyz", [-4,4,-4,4,-4,4])
ps.selectPathProjector ("Progressive", 0.2)
ps.setErrorThreshold (1e-3)
ps.setMaxIterations (40)
# 3}}}
# Define configurations {{{3
half_sitting = q = robot.getInitialConfig ()
q_init = half_sitting [::]
# Set initial position of screw-driver
q_init [-7:] = [2, 1, 0.65, 1, 0, 0, 0]
# q_init [-7:] = [2, 1, 0.65, 0.7071067811865476, 0.5, -0.5, 0]
# Open left hand
ilh = robot.rankInConfiguration ['hrp2/LARM_JOINT6']
q_init [ilh:ilh+6] = [0.75, -0.75, 0.75, -0.75, 0.75, -0.75]
q_goal = q_init [::]
q_goal [-7:] = [2, -1, 0.65, 1, 0, 0, 0]
# 3}}}
# Generate constraints {{{3
jointNames = dict ()
jointNames['all'] = robot.getJointNames ()
jointNames['hrp2'] = list ()
for n in jointNames['all']:
if n.startswith ("hrp2"):
jointNames['hrp2'].append (n)
ps.addPassiveDofs ('hrp2', jointNames ['hrp2'])
lockscrewgun = ps.lockFreeFlyerJoint ('box/base_joint', 'screwgun_lock')
locklhand = ['larm_6','lhand_0','lhand_1','lhand_2','lhand_3','lhand_4']
ps.createLockedJoint ('larm_6' , 'hrp2/LARM_JOINT6' , [q_init[ilh],])
ps.createLockedJoint \
('lhand_0', 'hrp2/LHAND_JOINT0', [q_init[ilh + 1],])
ps.createLockedJoint \
('lhand_1', 'hrp2/LHAND_JOINT1', [q_init[ilh + 2],])
ps.createLockedJoint \
('lhand_2', 'hrp2/LHAND_JOINT2', [q_init[ilh + 3],])
ps.createLockedJoint \
('lhand_3', 'hrp2/LHAND_JOINT3', [q_init[ilh + 4],])
ps.createLockedJoint \
('lhand_4', 'hrp2/LHAND_JOINT4', [q_init[ilh + 5],])
ps.createOrientationConstraint ("hrp2/waist_ori", robot.waist, "", (1,0,0,0), (True, True, False))
ps.addPartialCom ('hrp2', ['hrp2/base_joint_xyz'])
ps.createStaticStabilityConstraints ("balance-hrp2", q_init, 'hrp2')
# 3}}}
# Make a graph
graph = ConstraintGraph.buildGenericGraph (robot, "graph",
['hrp2/leftHand'], ['box'], [['box/handle']], [[]],
[], [])
graph.setConstraints (graph=True, lockDof = locklhand, numConstraints=ps.balanceConstraints () + ["hrp2/waist_ori"])
ps.setInitialConfig (q_init)
ps.addGoalConfig (q_goal)
t = ps.solve()
print t
# r = vf.createRealClient()
# pp = PathPlayer (robot.client.basic, r)
......@@ -440,6 +440,22 @@ class FullBody (object):
def effectorRRT(self, state1, comPos1, comPos2, comPos3, numOptim = 10):
return self.client.rbprm.rbprm.effectorRRT(state1, comPos1, comPos2, comPos3, 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 effectorRRTFromPath(self, state1, refPathId, path_start, path_to, comPos1, comPos2, comPos3, numOptim = 10, trackedEffectors = []):
return self.client.rbprm.rbprm.effectorRRTFromPath(state1, refPathId, path_start, path_to, comPos1, comPos2, comPos3, numOptim, trackedEffectors)
## 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.
......
......@@ -99,7 +99,8 @@ reduce_ineq = True, verbose = False, limbsCOMConstraints = None, profile = False
use_window = max(0, min(use_window, (len(states) - 1) - (state_id + 2))) # can't use preview if last state is reached
assert( len(phase_dt) >= 2 + use_window * 2 ), "phase_dt does not describe all phases"
constraints = ['cones_constraint', 'end_reached_constraint','end_speed_constraint']
#~ constraints = ['cones_constraint', 'end_reached_constraint','end_speed_constraint']
constraints = ['cones_constraint', 'end_reached_constraint','end_speed_constraint', 'com_kinematic_constraint']
param_constraints = []
mass = fullBody.getMass()
......@@ -140,10 +141,10 @@ reduce_ineq = True, verbose = False, limbsCOMConstraints = None, profile = False
if(use_window > 0):
var_final['c'] = var_final['c'][:init_waypoint_time+1]
params["t_init_phases"] = params["t_init_phases"][:-3*use_window]
lastspeed = var_final['dc'][init_waypoint_time]
print "trying to project on com (from, to) ", init_end_com, var_final['c'][-1]
if (fullBody.projectStateToCOM(state_id+1, (var_final['c'][-1]).tolist())):
states[state_id+1] = fullBody.getConfigAtState(state_id+1) #updating config from python side)
lastspeed = var_final['dc'][init_waypoint_time]
else:
print "reached com is not good, restarting problem with 0 window"
return gen_trajectory(fullBody, states, state_id, computeCones, mu, dt, phase_dt[:2], reduce_ineq, verbose, limbsCOMConstraints, profile, use_window = 0)
......@@ -227,7 +228,7 @@ def __optim__threading_ok(fullBody, states, state_id, computeCones = False, mu =
return comPosPerPhase, res[2] #res[2] is timeelapsed
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, profile = False, use_window = 0):
reduce_ineq = True, num_optims = 0, draw = False, verbose = False, limbsCOMConstraints = None, profile = False, use_window = 0, trackedEffectors = []):
comPosPerPhase, timeElapsed = __optim__threading_ok(fullBody, states, state_id, computeCones, mu, dt, phase_dt,
reduce_ineq, num_optims, draw, verbose, limbsCOMConstraints, profile, use_window)
print "done. generating state trajectory ",state_id
......@@ -236,11 +237,16 @@ reduce_ineq = True, num_optims = 0, draw = False, verbose = False, limbsCOMConst
return paths_ids[-1], paths_ids[:-1], timeElapsed
def solve_effector_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, profile = False, use_window = 0):
reduce_ineq = True, num_optims = 0, draw = False, verbose = False, limbsCOMConstraints = None, profile = False, use_window = 0, trackedEffectors = []):
comPosPerPhase, timeElapsed = __optim__threading_ok(fullBody, states, state_id, computeCones, mu, dt, phase_dt,
reduce_ineq, num_optims, draw, verbose, limbsCOMConstraints, profile, use_window)
print "done. generating state trajectory ",state_id
paths_ids = [int(el) for el in fullBody.effectorRRT(state_id,comPosPerPhase[0],comPosPerPhase[1],comPosPerPhase[2],num_optims)]
if(len(trackedEffectors) == 0):
paths_ids = [int(el) for el in fullBody.effectorRRT(state_id,comPosPerPhase[0],comPosPerPhase[1],comPosPerPhase[2],num_optims)]
else:
print "handling extra effector constraints"
refPathId = trackedEffectors[0]; path_start = trackedEffectors[1]; path_to = trackedEffectors[2]; effectorstracked = trackedEffectors[3]
paths_ids = [int(el) for el in fullBody.effectorRRTFromPath(state_id, refPathId, path_start, path_to, comPosPerPhase[0],comPosPerPhase[1],comPosPerPhase[2],num_optims, effectorstracked)]
print "done. computing final trajectory to display ",state_id
return paths_ids[-1], paths_ids[:-1], timeElapsed
......
......@@ -129,10 +129,12 @@ def __getTimes(fullBody, configs, i, time_scale):
from hpp import Error as hpperr
import sys, time
def step(fullBody, configs, i, optim, pp, limbsCOMConstraints, friction = 0.5, optim_effectors = True, time_scale = 20., useCOMConstraints = False, use_window = 0, verbose = False, draw = False):
def step(fullBody, configs, i, optim, pp, limbsCOMConstraints, friction = 0.5, optim_effectors = True, time_scale = 20., useCOMConstraints = False, use_window = 0, verbose = False, draw = False,
trackedEffectors = []):
global errorid
global stat_data
fail = 0
print "ste"
try:
#~ if(True):
times = [];
......@@ -152,9 +154,9 @@ def step(fullBody, configs, i, optim, pp, limbsCOMConstraints, friction = 0.5,
else:
comC = None
if(optim_effectors):
pid, trajectory, timeelapsed = solve_effector_RRT(fullBody, configs, i, True, friction, dt, times, False, optim, draw, verbose, comC, False, use_window=use_window)
pid, trajectory, timeelapsed = solve_effector_RRT(fullBody, configs, i, True, friction, dt, times, False, optim, draw, verbose, comC, False, use_window=use_window, trackedEffectors = trackedEffectors)
else :
pid, trajectory, timeelapsed = solve_com_RRT(fullBody, configs, i, True, friction, dt, times, False, optim, draw, verbose, comC, False, use_window=use_window)
pid, trajectory, timeelapsed = solve_com_RRT(fullBody, configs, i, True, friction, dt, times, False, optim, draw, verbose, comC, False, use_window=use_window, trackedEffectors = trackedEffectors)
displayComPath(pp, pid)
#~ pp(pid)
global res
......@@ -188,12 +190,23 @@ def step(fullBody, configs, i, optim, pp, limbsCOMConstraints, friction = 0.5,
stat_data["num_success"] += 1
else:
print "TODO, NO CONTACT VARIATION, LINEAR INTERPOLATION REQUIRED"
except hpperr as e:
except hpperr as e:
print "hpperr failed at id " + str(i) , e.strerror
stat_data["error_com_proj"] += 1
stat_data["num_errors"] += 1
errorid += [i]
fail+=1
if (use_window == 0 and (len(configs) - 1) - (i + 2) > 0):
print "could not project com, trying to increase velocity "
try:
return step(fullBody, configs, i, optim, pp, limbsCOMConstraints, friction, optim_effectors, time_scale, useCOMConstraints, 1, verbose, draw, trackedEffectors = trackedEffectors)
except:
if ((len(configs) - 1) - (i + 3) > 0):
print "could not project com, trying to increase velocity more "
step(fullBody, configs, i, optim, pp, limbsCOMConstraints, friction, optim_effectors, time_scale, useCOMConstraints, 2, verbose, draw, trackedEffectors = trackedEffectors)
else:
print "In hpperr and window != 0"
print "hpperr failed at id " + str(i) , e.strerror
stat_data["error_com_proj"] += 1
stat_data["num_errors"] += 1
errorid += [i]
fail+=1
except OptimError as e:
print "OptimError failed at id " + str(i) , e.strerror
stat_data["error_optim_fail"] += 1
......@@ -213,22 +226,39 @@ def step(fullBody, configs, i, optim, pp, limbsCOMConstraints, friction = 0.5,
#~ errorid += [i]
#~ fail+=1
except Exception as e:
stat_data["error_unknown"] += 1
stat_data["num_errors"] += 1
print e
errorid += [i]
fail+=1
if (use_window == 0 and (len(configs) - 1) - (i + 2) > 0):
print "could not project com, trying ti increase velocity "
return step(fullBody, configs, i, optim, pp, limbsCOMConstraints, friction, optim_effectors, time_scale, useCOMConstraints, 1, verbose, draw)
print "could not project com, trying to increase velocity "
try:
return step(fullBody, configs, i, optim, pp, limbsCOMConstraints, friction, optim_effectors, time_scale, useCOMConstraints, 1, verbose, draw, trackedEffectors = trackedEffectors)
except:
print "faile twice"
if ((len(configs) - 1) - (i + 3) > 0):
print "could not project com, trying to increase velocity more "
step(fullBody, configs, i, optim, pp, limbsCOMConstraints, friction, optim_effectors, time_scale, useCOMConstraints, 2, verbose, draw, trackedEffectors = trackedEffectors)
else:
print "In Exception and window != 0"
stat_data["error_unknown"] += 1
stat_data["num_errors"] += 1
print e
errorid += [i]
fail+=1
except:
stat_data["error_unknown"] += 1
stat_data["num_errors"] += 1
errorid += [i]
fail+=1
print "unknown"
if (use_window == 0 and (len(configs) - 1) - (i + 2) > 0):
print "could not project com, trying ti increase velocity "
return step(fullBody, configs, i, optim, pp, limbsCOMConstraints, friction, optim_effectors, time_scale, useCOMConstraints, 1, verbose, draw)
print "could not project com, trying to increase velocity "
try:
return step(fullBody, configs, i, optim, pp, limbsCOMConstraints, friction, optim_effectors, time_scale, useCOMConstraints, 1, verbose, draw, trackedEffectors = trackedEffectors)
except:
if ((len(configs) - 1) - (i + 3) > 0):
print "could not project com, trying to increase velocity more "
step(fullBody, configs, i, optim, pp, limbsCOMConstraints, friction, optim_effectors, time_scale, useCOMConstraints, 2, verbose, draw, trackedEffectors = trackedEffectors)
else:
print "In unknown and window != 0"
stat_data["error_unknown"] += 1
stat_data["num_errors"] += 1
errorid += [i]
fail+=1
return fail
def step_profile(fullBody, configs, i, optim, limbsCOMConstraints, friction = 0.5, optim_effectors = True, time_scale = 20., useCOMConstraints = False):
......
......@@ -874,6 +874,8 @@ namespace hpp {
std::pair<stability::MatrixXX, stability::VectorX> cone = stability::ComputeCentroidalCone(fullBody, state, friction);
res->length ((_CORBA_ULong)cone.first.rows());
_CORBA_ULong size = (_CORBA_ULong) cone.first.cols()+1;
std::cout << "H cols" << cone.first.cols() << std::endl;
std::cout << "H rows" << cone.first.rows() << std::endl;
for(int i=0; i < cone.first.rows(); ++i)
{
double* dofArray = hpp::floatSeq::allocbuf(size);
......@@ -901,6 +903,7 @@ namespace hpp {
}
catch(std::runtime_error& e)
{
std::cout << "ERROR " << e.what() << std::endl;
throw Error(e.what());
}
}
......@@ -943,6 +946,7 @@ namespace hpp {
}
catch(std::runtime_error& e)
{
std::cout << "ERROR " << e.what() << std::endl;
throw Error(e.what());
}
}
......@@ -1280,8 +1284,8 @@ 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;
throw std::runtime_error ("could not project without collision at state " + std::string(""+s1));
std::cout << "could not project without collision at state " << s1 << std::endl;
throw std::runtime_error ("could not project without collision at state " + s1 );
// fallback to limbRRT instead
//return -1; //limbRRT(s1, s2, numOptimizations);
}
......@@ -1387,8 +1391,8 @@ 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;
throw std::runtime_error ("could not project without collision at state " + std::string(""+s1));
std::cout << "could not project without collision at state " << s1 << std::endl;
throw std::runtime_error (std::string("could not project without collision at state " + s1));
// fallback to limbRRT instead
//return -1; //limbRRT(s1, s2, numOptimizations);
}
......@@ -1433,6 +1437,102 @@ assert(s2 == s1 +1);
}
}
hpp::floatSeq* RbprmBuilder::effectorRRTFromPath(double state1,
unsigned short refpath, double path_from, double path_to,
const hpp::floatSeqSeq& rootPositions1,
const hpp::floatSeqSeq& rootPositions2,
const hpp::floatSeqSeq& rootPositions3,
unsigned short numOptimizations, const Names_t &trackedEffector) throw (hpp::Error)
{
try
{
std::vector<std::string> trackedEffectorNames = stringConversion(trackedEffector);
std::vector<CORBA::Short> pathsIds;
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_ = project_or_throw(fullBody_, problemSolver_->problem(),s1Bis,paths[0]->end().head<3>());
states.push_back(s1Bis);
State s1Ter(s1Bis);
s1Ter.configuration_ = project_or_throw(fullBody_, problemSolver_->problem(),s1Ter,paths[1]->initial().head<3>());
states.push_back(s1Ter);
State s2Bis(state2);
s2Bis.configuration_ = project_or_throw(fullBody_, problemSolver_->problem(),s2Bis,paths[1]->end().head<3>());
states.push_back(s2Bis);
State s2Ter(s2Bis);
s2Ter.configuration_ = project_or_throw(fullBody_, problemSolver_->problem(),s2Ter,paths[2]->initial().head<3>());
states.push_back(s2Ter);
states.push_back(state2);
core::PathVectorPtr_t resPath = core::PathVector::create(fullBody_->device_->configSize(), fullBody_->device_->numberDof());
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 " << s1 << std::endl;
throw std::runtime_error (std::string("could not project without collision at state " + s1));
// fallback to limbRRT instead
//return -1; //limbRRT(s1, s2, numOptimizations);
}