diff --git a/CMakeLists.txt b/CMakeLists.txt
index e32b7d72cce496f32a3228c9575cdb8a984f0e3a..dede5ece6c11da21500059eb5ba7b89843db7a70 100755
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -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
diff --git a/data/meshes/siggraph_asia/grasp.stl b/data/meshes/siggraph_asia/grasp.stl
new file mode 100644
index 0000000000000000000000000000000000000000..ba16412c6a2678de742dfcbac2c9c8a4bb43e47a
Binary files /dev/null and b/data/meshes/siggraph_asia/grasp.stl differ
diff --git a/data/srdf/siggraph_asia/grasp.srdf b/data/srdf/siggraph_asia/grasp.srdf
new file mode 100644
index 0000000000000000000000000000000000000000..94553c608f67cde363aec22454fe3739c3b1b8af
--- /dev/null
+++ b/data/srdf/siggraph_asia/grasp.srdf
@@ -0,0 +1,3 @@
+<?xml version="1.0"?>
+<robot name="grasp">
+</robot>
diff --git a/data/urdf/siggraph_asia/grasp.urdf b/data/urdf/siggraph_asia/grasp.urdf
new file mode 100644
index 0000000000000000000000000000000000000000..5e5955716ea671e6ba9bb98a6fdb718445bb558f
--- /dev/null
+++ b/data/urdf/siggraph_asia/grasp.urdf
@@ -0,0 +1,19 @@
+<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>
diff --git a/idl/hpp/corbaserver/rbprm/rbprmbuilder.idl b/idl/hpp/corbaserver/rbprm/rbprmbuilder.idl
index 3da7d8848dd55d7d0c6962b88e86ce370c82e12e..de1e702aba116a41ceb85fc67dd9e133f1f08b2f 100755
--- a/idl/hpp/corbaserver/rbprm/rbprmbuilder.idl
+++ b/idl/hpp/corbaserver/rbprm/rbprmbuilder.idl
@@ -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
diff --git a/script/scenarios/demos/siggraph_asia/grasp_hrp2_interp.py b/script/scenarios/demos/siggraph_asia/grasp_hrp2_interp.py
new file mode 100644
index 0000000000000000000000000000000000000000..7c0d59d2723afb5d3a458074967cc83d84cffa73
--- /dev/null
+++ b/script/scenarios/demos/siggraph_asia/grasp_hrp2_interp.py
@@ -0,0 +1,397 @@
+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)
diff --git a/script/scenarios/demos/siggraph_asia/grasp_hrp2_path.py b/script/scenarios/demos/siggraph_asia/grasp_hrp2_path.py
new file mode 100644
index 0000000000000000000000000000000000000000..cce1ab636babf6038404560098e266621127edfc
--- /dev/null
+++ b/script/scenarios/demos/siggraph_asia/grasp_hrp2_path.py
@@ -0,0 +1,116 @@
+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.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)
diff --git a/script/scenarios/demos/siggraph_asia/log.txt b/script/scenarios/demos/siggraph_asia/log.txt
index 7ae6fd80554bfefc3b07a4165b4736732ac563d0..ab23625cc3007a76274ea37c7bdbfdad937b25e1 100644
--- a/script/scenarios/demos/siggraph_asia/log.txt
+++ b/script/scenarios/demos/siggraph_asia/log.txt
@@ -344,3 +344,102 @@ path computation 10
path computation 9
path computation 6
path computation 7
+path computation 21
+path computation 28
+path computation 3
+path computation 4
+path computation 22
+path computation 3
+path computation 3
+path computation 3
+path computation 3
+path computation 3
+path computation 1
+path computation 3
+path computation 1
+path computation 1
+path computation 2
+path computation 1
+path computation 1
+path computation 2
+path computation 2
+path computation 1
+path computation 3
+path computation 1
+path computation 3
+path computation 3
+path computation 3
+path computation 2
+path computation 3
+path computation 2
+path computation 2
+path computation 2
+path computation 4
+path computation 4
+path computation 3
+path computation 4
+path computation 2
+path computation 2
+path computation 2
+path computation 3
+path computation 3
+path computation 3
+path computation 2
+path computation 3
+path computation 3
+path computation 19
+path computation 2
+path computation 3
+path computation 3
+path computation 2
+path computation 5
+path computation 2
+path computation 2
+path computation 3
+path computation 3
+path computation 3
+path computation 3
+path computation 22
+path computation 2
+path computation 3
+path computation 10
+path computation 15
+path computation 15
+path computation 2
+path computation 3
+path computation 2
+path computation 2
+path computation 7
+path computation 3
+path computation 4
+path computation 3
+path computation 4
+path computation 3
+path computation 3
+path computation 3
+path computation 3
+path computation 4
+path computation 2
+path computation 17
+path computation 17
+path computation 3
+path computation 4
+path computation 2
+path computation 3
+path computation 3
+path computation 6
+path computation 3
+path computation 3
+path computation 3
+path computation 3
+path computation 4
+path computation 3
+path computation 2
+path computation 2
+path computation 1
+path computation 1
+path computation 1
+path computation 2
+path computation 1
+path computation 1
+path computation 1
diff --git a/script/scenarios/demos/siggraph_asia/plane_hrp2_interp.py b/script/scenarios/demos/siggraph_asia/plane_hrp2_interp.py
index 80404b0fc3fb79c453118b8ba42112febc0062af..9f09189f414956178a4713f229ae4e9edcebd4de 100644
--- a/script/scenarios/demos/siggraph_asia/plane_hrp2_interp.py
+++ b/script/scenarios/demos/siggraph_asia/plane_hrp2_interp.py
@@ -30,13 +30,15 @@ rLegId = '0rLeg'
rLeg = 'RLEG_JOINT0'
rLegOffset = [0,-0.105,0,]
rLegNormal = [0,1,0]
+#~ rLegNormal = [0,0,1]
rLegx = 0.09; rLegy = 0.05
fullBody.addLimb(rLegId,rLeg,'',rLegOffset,rLegNormal, rLegx, rLegy, 10000, "manipulability", 0.1)
lLegId = '1lLeg'
lLeg = 'LLEG_JOINT0'
lLegOffset = [0,-0.105,0]
-lLegNormal = [0,1,0]
+lLegNormal = [0,1,0]
+#~ lLegNormal = [0,0,1]
lLegx = 0.09; lLegy = 0.05
fullBody.addLimb(lLegId,lLeg,'',lLegOffset,rLegNormal, lLegx, lLegy, 10000, "manipulability", 0.1)
diff --git a/script/scenarios/demos/siggraph_asia/scale_hrp2_interp.py b/script/scenarios/demos/siggraph_asia/scale_hrp2_interp.py
index 41fc17b823287693a518e56939577bddaf699395..68859543c6cf3c080a416e581b833595515a4497 100644
--- a/script/scenarios/demos/siggraph_asia/scale_hrp2_interp.py
+++ b/script/scenarios/demos/siggraph_asia/scale_hrp2_interp.py
@@ -51,7 +51,8 @@ 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)
+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)
@@ -62,7 +63,8 @@ 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)
+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'
@@ -176,7 +178,7 @@ def genPlan(stepsize=0.1):
global configs
start = time.clock()
print "BEFORE"
- configs = fullBody.interpolate(stepsize, 0, 2, True)
+ configs = fullBody.interpolate(stepsize, 0, 0, True)
print "AFTER"
end = time.clock()
print "Contact plan generated in " + str(end-start) + "seconds"
diff --git a/script/scenarios/demos/siggraph_asia/scale_hrp2_path.py b/script/scenarios/demos/siggraph_asia/scale_hrp2_path.py
index 1ff7e839754c9eda864e72ddcc443167a81408c8..2230b753f5c9cbb2b06bbadce8153d928ce8f4f3 100644
--- a/script/scenarios/demos/siggraph_asia/scale_hrp2_path.py
+++ b/script/scenarios/demos/siggraph_asia/scale_hrp2_path.py
@@ -32,8 +32,10 @@ rbprmBuilder.loadModel(urdfName, urdfNameRoms, rootJointType, meshPackageName, p
rbprmBuilder.setJointBounds ("base_joint_xyz", [-1,3, -1, 1, 0, 2.2])
#~ 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','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',])
@@ -54,7 +56,7 @@ r = Viewer (ps)
q_init = rbprmBuilder.getCurrentConfig ();
q_init [3:7] = [ 0.98877108, 0. , 0.14943813, 0. ]
-q_init [0:3] = [-0., -0.82, 0.50]; rbprmBuilder.setCurrentConfig (q_init); r (q_init)
+q_init [0:3] = [-0.05, -0.82, 0.50]; 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. ]
diff --git a/src/hpp/corbaserver/rbprm/rbprmfullbody.py b/src/hpp/corbaserver/rbprm/rbprmfullbody.py
index a25e474b3e0dd8fe433a676cfa59490271b89284..4481d93ed5bfae08cb38660891874b88fb770f46 100755
--- a/src/hpp/corbaserver/rbprm/rbprmfullbody.py
+++ b/src/hpp/corbaserver/rbprm/rbprmfullbody.py
@@ -130,14 +130,17 @@ class FullBody (object):
# \param heuristicName: name of the selected heuristic for configuration evaluation
# \param loadValues: whether values computed, other than the static ones, should be loaded in memory
# \param disableEffectorCollision: whether collision detection should be disabled for end effector bones
- def addLimbDatabase(self, databasepath, limbId, heuristicName, loadValues = True, disableEffectorCollision = False):
+ def addLimbDatabase(self, databasepath, limbId, heuristicName, loadValues = True, disableEffectorCollision = False, grasp =False):
boolVal = 0.
boolValEff = 0.
+ graspv = 0.
if(loadValues):
boolVal = 1.
if(disableEffectorCollision):
boolValEff = 1.
- self.client.rbprm.rbprm.addLimbDatabase(databasepath, limbId, heuristicName, boolVal,boolValEff)
+ if(grasp):
+ graspv = 1.
+ self.client.rbprm.rbprm.addLimbDatabase(databasepath, limbId, heuristicName, boolVal,boolValEff,graspv)
## Add a limb to the model
#
@@ -158,11 +161,14 @@ class FullBody (object):
# This can be problematic in terms of performance. The default value is 3 cm.
# \param contactType whether the contact is punctual ("_3_DOF") or surfacic ("_6_DOF")
# \param disableEffectorCollision: whether collision detection should be disabled for end effector bones
- def addLimb(self, limbId, name, effectorname, offset, normal, x, y, samples, heuristicName, resolution, contactType="_6_DOF",disableEffectorCollision = False):
+ def addLimb(self, limbId, name, effectorname, offset, normal, x, y, samples, heuristicName, resolution, contactType="_6_DOF",disableEffectorCollision = False, grasp =False):
boolValEff = 0.
if(disableEffectorCollision):
boolValEff = 1.
- self.client.rbprm.rbprm.addLimb(limbId, name, effectorname, offset, normal, x, y, samples, heuristicName, resolution,contactType, boolValEff)
+ graspv = 0.
+ if(grasp):
+ graspv = 1.
+ self.client.rbprm.rbprm.addLimb(limbId, name, effectorname, offset, normal, x, y, samples, heuristicName, resolution,contactType, boolValEff,graspv)
## Returns the configuration of a limb described by a sample
#
diff --git a/src/rbprmbuilder.impl.cc b/src/rbprmbuilder.impl.cc
index 77d1d3c83d1b815b6b89cd05722e252de38d14a1..9fb446f2f47961c5ed08cb0b8fe22a31b1cba817 100755
--- a/src/rbprmbuilder.impl.cc
+++ b/src/rbprmbuilder.impl.cc
@@ -955,7 +955,7 @@ namespace hpp {
}
void RbprmBuilder::addLimb(const char* id, const char* limb, const char* effector, const hpp::floatSeq& offset, const hpp::floatSeq& normal, double x, double y,
- unsigned short samples, const char* heuristicName, double resolution, const char *contactType, double disableEffectorCollision) throw (hpp::Error)
+ unsigned short samples, const char* heuristicName, double resolution, const char *contactType, double disableEffectorCollision, double grasp) throw (hpp::Error)
{
if(!fullBodyLoaded_)
throw Error ("No full body robot was loaded");
@@ -973,7 +973,7 @@ namespace hpp {
cType = hpp::rbprm::_3_DOF;
}
fullBody_->AddLimb(std::string(id), std::string(limb), std::string(effector), off, norm, x, y,
- problemSolver_->collisionObstacles(), samples,heuristicName,resolution,cType,disableEffectorCollision > 0.5);
+ problemSolver_->collisionObstacles(), samples,heuristicName,resolution,cType,disableEffectorCollision > 0.5, grasp > 0.5);
}
catch(std::runtime_error& e)
{
@@ -982,7 +982,7 @@ namespace hpp {
}
- void RbprmBuilder::addLimbDatabase(const char* databasePath, const char* id, const char* heuristicName, double loadValues, double disableEffectorCollision) throw (hpp::Error)
+ void RbprmBuilder::addLimbDatabase(const char* databasePath, const char* id, const char* heuristicName, double loadValues, double disableEffectorCollision, double grasp) throw (hpp::Error)
{
if(!fullBodyLoaded_)
throw Error ("No full body robot was loaded");
@@ -990,7 +990,7 @@ namespace hpp {
{
std::string fileName(databasePath);
fullBody_->AddLimb(fileName, std::string(id), problemSolver_->collisionObstacles(), heuristicName, loadValues > 0.5,
- disableEffectorCollision > 0.5);
+ disableEffectorCollision > 0.5, grasp > 0.5);
}
catch(std::runtime_error& e)
{
@@ -1684,6 +1684,7 @@ assert(s2 == s1 +1);
{
bool success(false);
core::Configuration_t res = rbprm::interpolation::projectOnCom(fulllBody, problem,state,targetCom, success);
+ std::cout << "no com " << targetCom << std::endl;
if(!success)
{
throw std::runtime_error("could not project state on COM constraint");
diff --git a/src/rbprmbuilder.impl.hh b/src/rbprmbuilder.impl.hh
index 6d7687a2c2b7a28dc41fdee6dd26d725fd3ba700..4e5ad154bf981f2083ca0f89004a4683944d253f 100755
--- a/src/rbprmbuilder.impl.hh
+++ b/src/rbprmbuilder.impl.hh
@@ -146,9 +146,9 @@ namespace hpp {
virtual void addLimb(const char* id, const char* limb, const char* effector, const hpp::floatSeq& offset, const hpp::floatSeq& normal, double x, double y,
unsigned short samples, const char *heuristicName, double resolution, const char *contactType,
- double disableEffectorCollision) throw (hpp::Error);
+ double disableEffectorCollision, double grasp) throw (hpp::Error);
virtual void addLimbDatabase(const char* databasePath, const char* id, const char* heuristicName, double loadValues,
- double disableEffectorCollision) throw (hpp::Error);
+ double disableEffectorCollision, double grasp) throw (hpp::Error);
virtual void setStartState(const hpp::floatSeq& configuration, const hpp::Names_t& contactLimbs) throw (hpp::Error);
virtual void setEndState(const hpp::floatSeq& configuration, const hpp::Names_t& contactLimbs) throw (hpp::Error);