Skip to content

GitLab

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

grasping

parent 197024a7
Hide whitespace changes
Inline Side-by-side
CMakeLists.txt
View file @ d72e7618
......@@ -94,6 +94,7 @@ install(FILES
data/urdf/polaris.urdf
data/urdf/siggraph_asia/down.urdf
data/urdf/siggraph_asia/scale.urdf
data/urdf/siggraph_asia/grasp.urdf
#~ data/urdf/scene2.urdf
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/urdf
)
......@@ -127,6 +128,7 @@ install(FILES
data/srdf/polaris.srdf
data/srdf/siggraph_asia/down.srdf
data/srdf/siggraph_asia/scale.srdf
data/srdf/siggraph_asia/grasp.srdf
#~ data/srdf/scene2.srdf
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/srdf
)
......@@ -166,6 +168,7 @@ install(FILES
data/meshes/polaris_reduced.stl
data/meshes/siggraph_asia/down.stl
data/meshes/siggraph_asia/scale.stl
data/meshes/siggraph_asia/grasp.stl
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/meshes
)
install(FILES
......
data/srdf/siggraph_asia/grasp.srdf 0 → 100644
View file @ d72e7618
<?xml version="1.0"?>
<robot name="grasp">
</robot>
data/urdf/siggraph_asia/grasp.urdf 0 → 100644
View file @ d72e7618
<robot name="grasp">
<link name="base_link">
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="package://hpp-rbprm-corba/meshes/grasp.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/grasp.stl"/>
</geometry>
</collision>
</link>
</robot>
idl/hpp/corbaserver/rbprm/rbprmbuilder.idl
View file @ d72e7618
......@@ -226,7 +226,8 @@ module hpp
/// \param disableEffectorCollision whether collision detection should be disabled for the end effector bones
void addLimb(in string id, in string limb, in string effector, in floatSeq offset, in floatSeq normal,
in double x, in double y, in unsigned short samples, in string heuristicName,
in double resolution, in string contactType, in double disableEffectorCollision) raises (Error);
in double resolution, in string contactType, in double disableEffectorCollision,
in double grasp) raises (Error);
/// Specifies a subchain of the robot as a limb, which can be used to create contacts.
/// A limb must consist in a simple kinematic chain, where every node has only one child
......@@ -236,7 +237,7 @@ module hpp
/// \param loadValues whether other values computed for the limb database should be loaded
/// \param disableEffectorCollision whether collision detection should be disabled for the end effector bones
void addLimbDatabase(in string databasepath, in string id, in string heuristicName, in double loadValues,
in double disableEffectorCollision) raises (Error);
in double disableEffectorCollision,in double grasp) raises (Error);
/// Set the start state of a contact generation problem
/// environment, ordered by their efficiency
......
script/scenarios/demos/siggraph_asia/grasp_hrp2_interp.py 0 → 100644
View file @ d72e7618
from hpp.corbaserver.rbprm.rbprmbuilder import Builder
from hpp.corbaserver.rbprm.rbprmfullbody import FullBody
from hpp.gepetto import Viewer
import grasp_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", [-3,3, -2, 2, 0, 1])
ps = tp.ProblemSolver( fullBody )
r = tp.Viewer (ps, viewerClient=tp.r.client)
#~ AFTER loading obstacles
rLegId = 'hrp2_rleg_rom'
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.1)
lLegId = 'hrp2_lleg_rom'
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.1)
#~ AFTER loading obstacles
larmId = 'hrp2_larm_rom'
larm = 'LARM_JOINT0'
lHand = 'LARM_JOINT5'
lArmOffset = [-0.05,-0.050,-0.050]
lArmNormal = [1,0,0]
lArmOffset = [0,0,-0.105]
lArmNormal = [0,0,1]
lArmx = 0.024; lArmy = 0.024
fullBody.addLimb(larmId,larm,lHand,lArmOffset,lArmNormal, lArmx, lArmy, 10000, "manipulability", 0.1, "_6_DOF", True,grasp = True)
#~ fullBody.addLimb(larmId,larm,lHand,lArmOffset,lArmNormal, lArmx, lArmy, 10000, "manipulability", 0.1, "_6_DOF", True)
#~ fullBody.addLimb(larmId,larm,lHand,lArmOffset,lArmNormal, lArmx, lArmy, 10000, 0.05)
rarmId = 'hrp2_rarm_rom'
rarm = 'RARM_JOINT0'
rHand = 'RARM_JOINT5'
rArmOffset = [0,0,-0.105]
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.1, "_6_DOF", True,grasp = True)
#~ fullBody.addLimb(rarmId,rarm,rHand,rArmOffset,rArmNormal, rArmx, rArmy, 10000, "manipulability", 0.1, "_6_DOF", True)
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.runLimbSampleAnalysis(larmId, "jointLimitsDistance", True)
#~ fullBody.runLimbSampleAnalysis(rarmId, "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.05, -1.12, 0.5, 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,larmId]) #,rarmId,larmId])
#~ fullBody.setStartState(q_init,[rLegId,lLegId]) #,rarmId,larmId])
fullBody.setEndState(q_goal,[rLegId,lLegId])#,rarmId,larmId])
#~
#~ configs = fullBody.interpolate(0.1)
#~ configs = fullBody.interpolate(0.15)
i = 0;
configs = []
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()
print "BEFORE"
configs = fullBody.interpolate(stepsize, 0, 0, True)
print "AFTER"
end = time.clock()
print "Contact plan generated in " + str(end-start) + "seconds"
def contactPlan(step = 0.5):
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")
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 WXXSD in " + str(tp.t) + " ms."
d(0.005); e()
print "Root path SDDSD 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
from numpy import matrix, asarray
from numpy.linalg import norm
from spline import bezier
def __curveToWps(curve):
return asarray(curve.waypoints().transpose()).tolist()
def __Bezier(wps, init_acc = [0.,0.,0.], end_acc = [0.,0.,0.], init_vel = [0.,0.,0.], end_vel = [0.,0.,0.]):
c = curve_constraints();
c.init_vel = matrix(init_vel);
c.end_vel = matrix(end_vel);
c.init_acc = matrix(init_acc);
c.end_acc = matrix(end_acc);
matrix_bezier = matrix(wps).transpose()
curve = bezier(matrix_bezier, c)
return __curveToWps(curve), curve
#~ return __curveToWps(bezier(matrix_bezier))
allpaths = []
def play_all_paths():
for _, pid in enumerate(allpaths):
pp(pid)
def play_all_paths_smooth():
for i, pid in enumerate(allpaths):
if i % 2 == 1 :
pp(pid)
def play_all_paths_qs():
for i, pid in enumerate(allpaths):
if i % 2 == 0 :
pp(pid)
def test(stateid = 1, path = False, use_rand = False, just_one_curve = False, num_optim = 0) :
com_1 = __get_com(fullBody, configs[stateid])
com_2 = __get_com(fullBody, configs[stateid+1])
data = gen_sequence_data_from_state(fullBody,stateid,configs, mu = 0.8)
c_bounds_1 = get_com_constraint(fullBody, stateid, configs[stateid], limbsCOMConstraints, interm = False)
c_bounds_mid = get_com_constraint(fullBody, stateid, configs[stateid], limbsCOMConstraints, interm = True)
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, c_bounds_mid], use_rand = use_rand, mu = 0.8)
#~ success, c_mid_1, c_mid_2 = solve_dyn(data, c_bounds = [c_bounds_1, c_bounds_2, c_bounds_mid], use_rand = use_rand)
#~ success, c_mid_1, c_mid_2 = solve_dyn(data, c_bounds = [c_bounds_1, c_bounds_2])
paths_ids = []
if path and success:
#~ fullBody.straightPath([c_mid_1[0].tolist(),c_mid_2[0].tolist()])
#~ fullBody.straightPath([c_mid_2[0].tolist(),com_2])
if just_one_curve:
print "just one curve"
bezier_0, curve = __Bezier([com_1,c_mid_1[0].tolist(),c_mid_2[0].tolist(),com_2])
partions = [0.,0.1,0.9,1.]
p0 = fullBody.generateCurveTrajParts(bezier_0,partions)
#testing intermediary configurations
print 'wtf', partions[1], " "
com_interm1 = curve(partions[1])
com_interm2 = curve(partions[2])
print "com_1", com_1
success_proj1 = project_com_colfree(fullBody, stateid , asarray((com_interm1).transpose()).tolist()[0])
success_proj2 = project_com_colfree(fullBody, stateid+1, asarray((com_interm2).transpose()).tolist()[0])
if not success_proj1:
print "proj 1 failed"
return False, c_mid_1, c_mid_2, paths_ids
if not success_proj2:
print "proj 2 failed"
return False, c_mid_1, c_mid_2, paths_ids
print "p0", p0
#~ pp.displayPath(p0+1)
#~ pp.displayPath(p0+2)
pp.displayPath(p0)
paths_ids = [int(el) for el in fullBody.comRRTFromPos(stateid,p0+1,p0+2,p0+3,num_optim)]
else:
print "just all curve"
bezier_0, curve = __Bezier([com_1,c_mid_1[0].tolist()] , end_acc = c_mid_1[1].tolist() , end_vel = [0.,0.,0.])
bezier_1, curve = __Bezier([c_mid_1[0].tolist(),c_mid_2[0].tolist()], end_acc = c_mid_2[1].tolist(), init_acc = c_mid_1[1].tolist(), init_vel = [0.,0.,0.], end_vel = [0.,0.,0.])
bezier_2, curve = __Bezier([c_mid_2[0].tolist(),com_2] , init_acc = c_mid_2[1].tolist(), init_vel = [0.,0.,0.])
p0 = fullBody.generateCurveTraj(bezier_0)
print "p0", p0
fullBody.generateCurveTraj(bezier_1)
fullBody.generateCurveTraj(bezier_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,num_optim)]
#~ paths_ids = []
global allpaths
allpaths += paths_ids[:-1]
#~ allpaths += [paths_ids[-1]]
#~ pp(paths_ids[-1])
#~ return success, paths_ids, c_mid_1, c_mid_2
return success, c_mid_1, c_mid_2, paths_ids
#~ data = gen_sequence_data_from_state(fullBody,3,configs)
def prepare_whole_interp(stateid, stateid_end):
all_points = []
allSuc = True
for i in range(stateid, stateid_end):
com_1 = __get_com(fullBody, configs[stateid])
success, c_mid_1, c_mid_2, paths_ids = test(i, False, True, False)
allSuc = success and allSuc
if not success:
break
all_points = all_points + [com_1, c_mid_1[0].tolist(), c_mid_2[0].tolist()]
all_points = all_points + [__get_com(fullBody, configs[stateid_end])]
if allSuc:
bezier_0 = __Bezier(all_points)
p0 = fullBody.generateCurveTraj(bezier_0)
pp.displayPath(p0)
num_paths = stateid_end - stateid
num_sub_paths = num_paths * 3
increment = 1. / float(num_paths)
partitions = [0.]
for i in range(0, num_paths):
dec = increment * float(i)
partitions += [dec + 0.01 * increment, dec + 0.99 * increment,dec + 1. * increment]
print "partitions", partitions, len(partitions)
p0 = fullBody.generateCurveTrajParts(bezier_0,partitions) +1
paths_ids = []
for i in range(0, num_paths):
print "***************************3i", p0+3*i
paths_ids += [int(el) for el in fullBody.comRRTFromPos(stateid + i,p0+3*i,p0+3*i+1,p0+3*i+2)]
#~ paths_ids = []
global allpaths
allpaths += paths_ids[:-1]
#~ pp(paths_ids[-1])
#~ success, paths_ids, c_mid_1, c_mid_2 = test(0, True, True, False)
#~ prepare_whole_interp(1, 2)
#~ pp(29),pp(9),pp(17)
from hpp.corbaserver.rbprm.tools.path_to_trajectory import *
def gen(len_con = 10, num_optim = 0, ine_curve =True):
for i in range (len_con):
if not test(i, True, False, ine_curve,num_optim):
for j in range(10):
found = test(i, True, True, ine_curve, num_optim)
if found:
break
a = gen_trajectory_to_play(fullBody, pp, allpaths, flatten([[0.3, 0.6, 0.1] for _ in range(len(allpaths) / 3)]))
return a
#~ pp(29),pp(9),pp(17)
#~ gen(True)
script/scenarios/demos/siggraph_asia/grasp_hrp2_path.py 0 → 100644
View file @ d72e7618
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", [-3,3, -2, 2, 0, 1])
#~ rbprmBuilder.setFilter(['hrp2_larm_rom','hrp2_rarm_rom','hrp2_lleg_rom','hrp2_rleg_rom'])
#~ rbprmBuilder.setFilter(['hrp2_lleg_rom','hrp2_rleg_rom'])
#~ rbprmBuilder.setAffordanceFilter('hrp2_rarm_rom', ['Support','Lean'])
#~ rbprmBuilder.setAffordanceFilter('hrp2_larm_rom', ['Support','Lean'])
#~ rbprmBuilder.setAffordanceFilter('hrp2_rarm_rom', ['Support'])
#~ rbprmBuilder.setAffordanceFilter('hrp2_larm_rom', ['Support'])
rbprmBuilder.setAffordanceFilter('hrp2_rarm_rom', ['Lean'])
rbprmBuilder.setAffordanceFilter('hrp2_larm_rom', ['Lean'])
rbprmBuilder.setAffordanceFilter('hrp2_lleg_rom', ['Support',])
rbprmBuilder.setAffordanceFilter('hrp2_rleg_rom', ['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 [3:7] = [ 0.98877108, 0. , 0.14943813, 0. ]
q_init [0:3] = [-0.05, -1.12, 0.5]; rbprmBuilder.setCurrentConfig (q_init); r (q_init)
#~ q_init [0:3] = [0.1, -0.82, 0.648702]; 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] = [-0.05, -0.12, 0.5]; 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.000005, 0.000005, 0.00005])
afftool.setAffordanceConfig('Lean', [0.00005, 0.000005, 0.00005])
afftool.loadObstacleModel (packageName, "grasp", "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.