Commit 89104692 authored by t steve's avatar t steve
Browse files

ongoing walk

parent a1882e78
......@@ -327,6 +327,15 @@ module hpp
/// \return id of the root path computed
short generateComTraj(in floatSeqSeq positions, in floatSeqSeq velocities, in floatSeqSeq accelerations, in double dt) raises (Error);
/// Create a linear path given list of positions
/// The path is composed of n+1 integrations
/// between the n positions.
/// The resulting path
/// is added to the problem solver
/// \param positions array of positions
/// \return id of the root path computed
short straightPath(in floatSeqSeq positions) 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
......
from hpp.corbaserver.rbprm.rbprmbuilder import Builder
from hpp.corbaserver.rbprm.rbprmfullbody import FullBody
from hpp.gepetto import Viewer
import plane_hrp2_path as tp
import time
packageName = "hrp2_14_description"
meshPackageName = "hrp2_14_description"
rootJointType = "freeflyer"
##
# Information to retrieve urdf and srdf files.
urdfName = "hrp2_14"
urdfSuffix = "_reduced"
srdfSuffix = ""
fullBody = FullBody ()
fullBody.loadFullBodyModel(urdfName, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
fullBody.setJointBounds ("base_joint_xyz", [-0.135,2, -1, 1, 0, 2.2])
ps = tp.ProblemSolver( fullBody )
r = tp.Viewer (ps, viewerClient=tp.r.client)
#~ AFTER loading obstacles
rLegId = '0rLeg'
rLeg = 'RLEG_JOINT0'
rLegOffset = [0,-0.105,0,]
rLegNormal = [0,1,0]
rLegx = 0.09; rLegy = 0.05
fullBody.addLimb(rLegId,rLeg,'',rLegOffset,rLegNormal, rLegx, rLegy, 10000, "manipulability", 0.4)
lLegId = '1lLeg'
lLeg = 'LLEG_JOINT0'
lLegOffset = [0,-0.105,0]
lLegNormal = [0,1,0]
lLegx = 0.09; lLegy = 0.05
fullBody.addLimb(lLegId,lLeg,'',lLegOffset,rLegNormal, lLegx, lLegy, 10000, "manipulability", 0.4)
rarmId = '3Rarm'
rarm = 'RARM_JOINT0'
rHand = 'RARM_JOINT5'
rArmOffset = [0,0,-0.1]
rArmNormal = [0,0,1]
rArmx = 0.024; rArmy = 0.024
#disabling collision for hook
#~ fullBody.addLimb(rarmId,rarm,rHand,rArmOffset,rArmNormal, rArmx, rArmy, 10000, "manipulability", 0.05, "_6_DOF", True)
#~ 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)
#~
#~ fullBody.runLimbSampleAnalysis(rLegId, "jointLimitsDistance", True)
#~ fullBody.runLimbSampleAnalysis(lLegId, "jointLimitsDistance", True)
#~ fullBody.client.basic.robot.setJointConfig('LARM_JOINT0',[1])
#~ fullBody.client.basic.robot.setJointConfig('RARM_JOINT0',[-1])
q_0 = fullBody.getCurrentConfig();
#~ fullBody.createOctreeBoxes(r.client.gui, 1, rarmId, q_0,)
q_init = fullBody.getCurrentConfig(); q_init[0:7] = tp.q_init[0:7]
q_goal = fullBody.getCurrentConfig(); q_goal[0:7] = tp.q_goal[0:7]
fullBody.setCurrentConfig (q_init)
q_init = [
0, -0.82, 0.58, 1.0, 0.0 , 0.0, 0.0, # Free flyer 0-6
0.0, 0.0, 0.0, 0.0, # CHEST HEAD 7-10
0.261799388, 0.174532925, 0.0, -0.523598776, 0.0, 0.0, 0.17, # LARM 11-17
0.261799388, -0.174532925, 0.0, -0.523598776, 0.0, 0.0, 0.17, # RARM 18-24
0.0, 0.0, -0.453785606, 0.872664626, -0.41887902, 0.0, # LLEG 25-30
0.0, 0.0, -0.453785606, 0.872664626, -0.41887902, 0.0, # RLEG 31-36
]; r (q_init)
fullBody.setCurrentConfig (q_goal)
#~ r(q_goal)
q_goal = fullBody.generateContacts(q_goal, [0,0,1])
q_init = fullBody.generateContacts(q_init, [0,0,1])
#~ r(q_goal)
#~ fullBody.setStartState(q_init,[rLegId,lLegId,rarmId]) #,rarmId,larmId])
fullBody.setStartState(q_init,[lLegId,rLegId]) #,rarmId,larmId])
fullBody.setEndState(q_goal,[rLegId,lLegId])#,rarmId,larmId])
#~
#~ configs = fullBody.interpolate(0.1)
#~ configs = fullBody.interpolate(0.15)
i = 0;
configs = []
#~ fullBody.draw(configs[i],r); i=i+1; i-1
#~ r.loadObstacleModel ('hpp-rbprm-corba', "stair_bauzil", "contact")
#~ fullBody.exportAll(r, configs, 'stair_bauzil_hrp2_robust_2');
#~ fullBody.client.basic.robot.setJointConfig('LLEG_JOINT0',[-1])
#~ q_0 = fullBody.getCurrentConfig();
#~ fullBody.draw(q_0,r);
#~ print(fullBody.client.rbprm.rbprm.getOctreeTransform(rarmId, q_0))
#~
#~
#~ fullBody.client.basic.robot.setJointConfig('LLEG_JOINT0',[1])
#~ q_0 = fullBody.getCurrentConfig();
#~ fullBody.draw(q_0,r);
#~ print(fullBody.client.rbprm.rbprm.getOctreeTransform(rarmId, q_0))
#~ q_init = fullBody.generateContacts(q_init, [0,0,-1]); r (q_init)
#~ f1 = open("secondchoice","w+")
#~ f1 = open("hrp2_stair_not_robust_configs","w+")
#~ f1.write(str(configs))
#~ f1.close()
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):
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)
def play(frame_rate = 1./24.):
play_traj(fullBody,pp,frame_rate)
def saveAll(name):
saveAllData(fullBody, r, name)
def initConfig():
r.client.gui.setVisibility("hrp2_14", "ON")
tp.cl.problem.selectProblem("default")
tp.r.client.gui.setVisibility("toto", "OFF")
tp.r.client.gui.setVisibility("hrp2_trunk_flexible", "OFF")
r(q_init)
def endConfig():
r.client.gui.setVisibility("hrp2_14", "ON")
tp.cl.problem.selectProblem("default")
tp.r.client.gui.setVisibility("toto", "OFF")
tp.r.client.gui.setVisibility("hrp2_trunk_flexible", "OFF")
r(q_goal)
def rootPath():
tp.cl.problem.selectProblem("rbprm_path")
r.client.gui.setVisibility("hrp2_14", "OFF")
tp.r.client.gui.setVisibility("toto", "OFF")
r.client.gui.setVisibility("hyq", "OFF")
r.client.gui.setVisibility("hrp2_trunk_flexible", "ON")
tp.pp(0)
r.client.gui.setVisibility("hrp2_trunk_flexible", "OFF")
r.client.gui.setVisibility("hyq", "ON")
tp.cl.problem.selectProblem("default")
def genPlan(stepsize=0.1):
r.client.gui.setVisibility("hrp2_14", "ON")
tp.cl.problem.selectProblem("default")
tp.r.client.gui.setVisibility("toto", "OFF")
tp.r.client.gui.setVisibility("hrp2_trunk_flexible", "OFF")
global configs
start = time.clock()
configs = fullBody.interpolate(stepsize, 1, 5, True)
end = time.clock()
print "Contact plan generated in " + str(end-start) + "seconds"
def contactPlan(step = 0.5):
r.client.gui.setVisibility("hyq", "ON")
tp.cl.problem.selectProblem("default")
tp.r.client.gui.setVisibility("toto", "OFF")
tp.r.client.gui.setVisibility("hyq_trunk_large", "OFF")
for i in range(0,len(configs)):
r(configs[i]);
time.sleep(step)
def a():
print "initial configuration"
initConfig()
def b():
print "end configuration"
endConfig()
def c():
print "displaying root path"
rootPath()
def d(step=0.1):
print "computing contact plan"
genPlan(step)
def e(step = 0.5):
print "displaying contact plan"
contactPlan(step)
print "Root path generated in " + str(tp.t) + " ms."
d(0.05); e(0.01)
print "Root path generated in " + str(tp.t) + " ms."
#~ from gen_data_from_rbprm import *
#~
#~ for config in configs:
#~ r(config)
#~ print(fullBody.client.basic.robot.getComPosition())
#~
#~ gen_and_save(fullBody,configs, "stair_bauzil_contacts_data")
#~ main()
from gen_data_from_rbprm import *
from hpp.corbaserver.rbprm.tools.com_constraints import get_com_constraint
#computing com bounds 0 and 1
def __get_com(robot, config):
save = robot.getCurrentConfig()
robot.setCurrentConfig(config)
com = robot.getCenterOfMass()
robot.setCurrentConfig(save)
return com
def test(stateid = 1, path = False) :
com_1 = __get_com(fullBody, configs[stateid])
com_2 = __get_com(fullBody, configs[stateid+1])
data = gen_sequence_data_from_state(fullBody,stateid,configs)
c_bounds_1 = get_com_constraint(fullBody, stateid, configs[stateid], limbsCOMConstraints, interm = False)
c_bounds_2 = get_com_constraint(fullBody, stateid, configs[stateid+1], limbsCOMConstraints, interm = False)
success, c_mid_1, c_mid_2 = solve_quasi_static(data, c_bounds = [c_bounds_1, c_bounds_2])
if path:
p0 = fullBody.straightPath([com_1,c_mid_1[0].tolist()])
fullBody.straightPath([c_mid_1[0].tolist(),c_mid_2[0].tolist()])
fullBody.straightPath([c_mid_2[0].tolist(),com_2])
pp.displayPath(p0)
pp.displayPath(p0+1)
pp.displayPath(p0+2)
paths_ids = [int(el) for el in fullBody.comRRTFromPos(stateid,p0,p0+1,p0+2)]
pp(paths_ids[-1])
return paths_ids
from hpp.corbaserver.rbprm.rbprmbuilder import Builder
from hpp.gepetto import Viewer
from hpp.corbaserver import Client
from hpp.corbaserver.robot import Robot as Parent
class Robot (Parent):
rootJointType = 'freeflyer'
packageName = 'hpp-rbprm-corba'
meshPackageName = 'hpp-rbprm-corba'
# URDF file describing the trunk of the robot HyQ
urdfName = 'hrp2_trunk_flexible'
urdfSuffix = ""
srdfSuffix = ""
def __init__ (self, robotName, load = True):
Parent.__init__ (self, robotName, self.rootJointType, load)
self.tf_root = "base_footprint"
self.client.basic = Client ()
self.load = load
rootJointType = 'freeflyer'
packageName = 'hpp-rbprm-corba'
meshPackageName = 'hpp-rbprm-corba'
urdfName = 'hrp2_trunk_flexible'
urdfNameRoms = ['hrp2_larm_rom','hrp2_rarm_rom','hrp2_lleg_rom','hrp2_rleg_rom']
urdfSuffix = ""
srdfSuffix = ""
rbprmBuilder = Builder ()
rbprmBuilder.loadModel(urdfName, urdfNameRoms, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
rbprmBuilder.setJointBounds ("base_joint_xyz", [0,2, -1, 1, 0, 2.2])
#~ rbprmBuilder.setFilter(['hrp2_rarm_rom','hrp2_lleg_rom','hrp2_rleg_rom'])
rbprmBuilder.setAffordanceFilter('3Rarm', ['Support'])
rbprmBuilder.setAffordanceFilter('0rLeg', ['Support',])
rbprmBuilder.setAffordanceFilter('1lLeg', ['Support'])
#~ rbprmBuilder.setNormalFilter('hrp2_rarm_rom', [0,0,1], 0.5)
#~ rbprmBuilder.setNormalFilter('hrp2_lleg_rom', [0,0,1], 0.9)
#~ rbprmBuilder.setNormalFilter('hrp2_rleg_rom', [0,0,1], 0.9)
#~ rbprmBuilder.setNormalFilter('hyq_rhleg_rom', [0,0,1], 0.9)
rbprmBuilder.boundSO3([-0.,0,-1,1,-1,1])
#~ from hpp.corbaserver.rbprm. import ProblemSolver
from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
ps = ProblemSolver( rbprmBuilder )
r = Viewer (ps)
q_init = rbprmBuilder.getCurrentConfig ();
q_init [0:3] = [0, -0.82, 0.58]; rbprmBuilder.setCurrentConfig (q_init); r (q_init)
q_init [0:3] = [0.1, -0.82, 0.58]; rbprmBuilder.setCurrentConfig (q_init); r (q_init)
#~ q_init [0:3] = [0, -0.63, 0.6]; rbprmBuilder.setCurrentConfig (q_init); r (q_init)
#~ q_init [3:7] = [ 0.98877108, 0. , 0.14943813, 0. ]
q_goal = q_init [::]
#~ q_goal [3:7] = [ 0.98877108, 0. , 0.14943813, 0. ]
q_goal [0:3] = [2, -0.82, 0.58]; r (q_goal)
#~ q_goal [0:3] = [1.2, -0.65, 1.1]; r (q_goal)
#~ ps.addPathOptimizer("GradientBased")
ps.addPathOptimizer("RandomShortcut")
ps.setInitialConfig (q_init)
ps.addGoalConfig (q_goal)
from hpp.corbaserver.affordance.affordance import AffordanceTool
afftool = AffordanceTool ()
afftool.setAffordanceConfig('Support', [0.5, 0.03, 0.00005])
afftool.loadObstacleModel (packageName, "ground", "planning", r)
#~ afftool.analyseAll()
#~ afftool.visualiseAffordances('Support', r, [0.25, 0.5, 0.5])
#~ afftool.visualiseAffordances('Lean', r, [0, 0, 0.9])
ps.client.problem.selectConFigurationShooter("RbprmShooter")
ps.client.problem.selectPathValidation("RbprmPathValidation",0.05)
#~ ps.solve ()
t = ps.solve ()
print t;
if isinstance(t, list):
t = t[0]* 3600000 + t[1] * 60000 + t[2] * 1000 + t[3]
f = open('log.txt', 'a')
f.write("path computation " + str(t) + "\n")
f.close()
from hpp.gepetto import PathPlayer
pp = PathPlayer (rbprmBuilder.client.basic, r)
#~ pp.fromFile("/home/stonneau/dev/hpp/src/hpp-rbprm-corba/script/paths/stair.path")
#~
#~ pp (2)
#~ pp (0)
#~ pp (1)
#~ pp.toFile(1, "/home/stonneau/dev/hpp/src/hpp-rbprm-corba/script/paths/stair.path")
#~ rbprmBuilder.exportPath (r, ps.client.problem, 1, 0.01, "stair_bauzil_hrp2_path.txt")
cl = Client()
cl.problem.selectProblem("rbprm_path")
rbprmBuilder2 = Robot ("toto")
ps2 = ProblemSolver( rbprmBuilder2 )
cl.problem.selectProblem("default")
cl.problem.movePathToProblem(1,"rbprm_path",rbprmBuilder.getAllJointNames())
r2 = Viewer (ps2, viewerClient=r.client)
r.client.gui.setVisibility("toto", "OFF")
r.client.gui.setVisibility("hrp2_trunk_flexible", "OFF")
#~ r2(q_far)
......@@ -107,7 +107,8 @@ INSTALL(
INSTALL(
FILES
${CMAKE_CURRENT_SOURCE_DIR}/hpp/corbaserver/rbprm/tools/__init__.py
${CMAKE_CURRENT_SOURCE_DIR}/hpp/corbaserver/rbprm/tools//generateROMs.py
${CMAKE_CURRENT_SOURCE_DIR}/hpp/corbaserver/rbprm/tools/generateROMs.py
${CMAKE_CURRENT_SOURCE_DIR}/hpp/corbaserver/rbprm/tools/com_constraints.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/cwc_trajectory_helper.py
......
......@@ -451,13 +451,25 @@ class FullBody (object):
# The resulting path
# is added to the problem solver
# \param positions array of positions
# \return id of the root path computed
def straightPath(self, positions):
return self.client.rbprm.rbprm.straightPath(positions)
## Create a com trajectory given list of positions, velocities and accelerations
# accelerations list contains one less element because it does not have an initial state.
# a set of 3d key points
# The path is composed of n+1 integrations
# between the n positions.
# The resulting path
# is added to the problem solver
# \param positions array of positions
# \param velocities array of velocities
# \param accelerations array of accelerations
# \param dt time between two points
# \return id of the root path computed
def generateComTraj(self, positions, velocities, accelerations, dt):
return self.client.rbprm.rbprm.generateComTraj(positions, velocities, accelerations, dt)
## Create a path for the root given
# a set of 3d key points
# The path is composed of n+1 linear interpolations
......
import matplotlib
#~ matplotlib.use('Agg')
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
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 time
import hpp.corbaserver.rbprm.data.com_inequalities as ine
ineqPath = ine.__path__[0] +"/"
# epsilon for testing whether a number is close to zero
_EPS = np.finfo(float).eps * 4.0
def quaternion_matrix(quaternion):
q = np.array(quaternion, dtype=np.float64, copy=True)
n = np.dot(q, q)
if n < _EPS:
return np.identity(4)
q *= math.sqrt(2.0 / n)
q = np.outer(q, q)
return np.array([
[1.0-q[2, 2]-q[3, 3], q[1, 2]-q[3, 0], q[1, 3]+q[2, 0], 0.0],
[ q[1, 2]+q[3, 0], 1.0-q[1, 1]-q[3, 3], q[2, 3]-q[1, 0], 0.0],
[ q[1, 3]-q[2, 0], q[2, 3]+q[1, 0], 1.0-q[1, 1]-q[2, 2], 0.0],
[ 0.0, 0.0, 0.0, 1.0]])
def __get_com(robot, config):
save = robot.getCurrentConfig()
robot.setCurrentConfig(config)
com = robot.getCenterOfMass()
robot.setCurrentConfig(save)
return com
constraintsComLoaded = {}
lastspeed = np.array([0,0,0])
def get_com_constraint(fullBody, state, config, limbsCOMConstraints, interm = False):
global constraintsLoaded
As = []; bs = []
fullBody.setCurrentConfig(config)
contacts = []
for i, v in limbsCOMConstraints.iteritems():
if not constraintsComLoaded.has_key(i):
constraintsComLoaded[i] = ineq_from_file(ineqPath+v['file'])
contact = (interm and fullBody.isLimbInContactIntermediary(i, state)) or (not interm and fullBody.isLimbInContact(i, state))
if contact:
ineq = constraintsComLoaded[i]
qEffector = fullBody.getJointPosition(v['effector'])
tr = quaternion_matrix(qEffector[3:7])
tr[:3,3] = np.array(qEffector[0:3])
ineq_r = rotate_inequalities(ineq, tr)
As.append(ineq_r.A); bs.append(ineq_r.b);
contacts.append(v['effector'])
return [np.vstack(As), np.hstack(bs)]
......@@ -7,6 +7,7 @@ from obj_to_constraints import ineq_from_file, rotate_inequalities
import numpy as np
import math
from numpy.linalg import norm
from com_constraints import get_com_constraint
import time
import hpp.corbaserver.rbprm.data.com_inequalities as ine
......@@ -40,24 +41,6 @@ constraintsComLoaded = {}
lastspeed = np.array([0,0,0])
def __get_com_constraint(fullBody, state, config, limbsCOMConstraints, interm = False):
global constraintsLoaded
As = []; bs = []
fullBody.setCurrentConfig(config)
contacts = []
for i, v in limbsCOMConstraints.iteritems():
if not constraintsComLoaded.has_key(i):
constraintsComLoaded[i] = ineq_from_file(ineqPath+v['file'])
contact = (interm and fullBody.isLimbInContactIntermediary(i, state)) or (not interm and fullBody.isLimbInContact(i, state))
if contact:
ineq = constraintsComLoaded[i]
qEffector = fullBody.getJointPosition(v['effector'])
tr = quaternion_matrix(qEffector[3:7])
tr[:3,3] = np.array(qEffector[0:3])
ineq_r = rotate_inequalities(ineq, tr)
As.append(ineq_r.A); bs.append(ineq_r.b);
contacts.append(v['effector'])
return [np.vstack(As), np.hstack(bs)]
def compute_state_info(fullBody,states, state_id, phase_dt, mu, computeCones, limbsCOMConstraints):
init_com = __get_com(fullBody, states[state_id])
......@@ -81,11 +64,11 @@ def compute_state_info(fullBody,states, state_id, phase_dt, mu, computeCones, li
cones.append(fullBody.getContactCone(state_id+1, mu)[0])
COMConstraints = None
if(not (limbsCOMConstraints == None)):
COMConstraints = [__get_com_constraint(fullBody, state_id, states[state_id], limbsCOMConstraints)]
COMConstraints = [get_com_constraint(fullBody, state_id, states[state_id], limbsCOMConstraints)]
if(len(p) > 2):
COMConstraints.append(__get_com_constraint(fullBody, state_id, states[state_id], limbsCOMConstraints, True))
COMConstraints.append(get_com_constraint(fullBody, state_id, states[state_id], limbsCOMConstraints, True))
if(len(p) > len(COMConstraints)):
COMConstraints.append(__get_com_constraint(fullBody, state_id + 1, states[state_id + 1], limbsCOMConstraints))
COMConstraints.append(get_com_constraint(fullBody, state_id + 1, states[state_id + 1], limbsCOMConstraints))
print 'num cones ', len(cones)
return p, N, init_com, end_com, t_end_phases, cones, COMConstraints
......
......@@ -1306,6 +1306,32 @@ namespace hpp {
}
}
CORBA::Short RbprmBuilder::straightPath(const hpp::floatSeqSeq& positions) throw (hpp::Error)
{
try
{
T_Configuration c = doubleDofArrayToConfig(3, positions);
if(c.size() <2)
{
throw std::runtime_error("straightPath requires at least 2 configurations to generate path");
}
core::PathVectorPtr_t res = core::PathVector::create(3, 3);
CIT_Configuration cit = c.begin(); ++cit;
int i = 0;
model::vector3_t zero (0.,0.,0.);
for(;cit != c.end(); ++cit, ++i)
{
model::vector3_t speed = (*cit) - *(cit-1);
res->appendPath(interpolation::ComTrajectory::create(*(cit-1),*cit,speed,zero,1.));
}
return problemSolver_->addPath(res);
}
catch(std::runtime_error& e)
{
throw Error(e.what());
}
}
CORBA::Short RbprmBuilder::generateComTraj(const hpp::floatSeqSeq& positions, const hpp::floatSeqSeq& velocities,
const hpp::floatSeqSeq& accelerations,
const double dt) throw (hpp::Error)
......
......@@ -161,6 +161,8 @@ namespace hpp {
virtual hpp::floatSeqSeq* getContactIntermediateCone(unsigned short stateId, double friction) throw (hpp::Error);
virtual CORBA::Short generateComTraj(const hpp::floatSeqSeq& positions, const hpp::floatSeqSeq& velocities,