From d72e761859211404647a8ab7ea86d079eacdfa40 Mon Sep 17 00:00:00 2001
From: t steve <pro@stevetonneau.fr>
Date: Fri, 12 May 2017 16:24:44 +0200
Subject: [PATCH] grasping
---
CMakeLists.txt | 3 +
data/meshes/siggraph_asia/grasp.stl | Bin 0 -> 1284 bytes
data/srdf/siggraph_asia/grasp.srdf | 3 +
data/urdf/siggraph_asia/grasp.urdf | 19 +
idl/hpp/corbaserver/rbprm/rbprmbuilder.idl | 5 +-
.../demos/siggraph_asia/grasp_hrp2_interp.py | 397 ++++++++++++++++++
.../demos/siggraph_asia/grasp_hrp2_path.py | 116 +++++
script/scenarios/demos/siggraph_asia/log.txt | 99 +++++
.../demos/siggraph_asia/plane_hrp2_interp.py | 4 +-
.../demos/siggraph_asia/scale_hrp2_interp.py | 8 +-
.../demos/siggraph_asia/scale_hrp2_path.py | 8 +-
src/hpp/corbaserver/rbprm/rbprmfullbody.py | 14 +-
src/rbprmbuilder.impl.cc | 9 +-
src/rbprmbuilder.impl.hh | 4 +-
14 files changed, 670 insertions(+), 19 deletions(-)
create mode 100644 data/meshes/siggraph_asia/grasp.stl
create mode 100644 data/srdf/siggraph_asia/grasp.srdf
create mode 100644 data/urdf/siggraph_asia/grasp.urdf
create mode 100644 script/scenarios/demos/siggraph_asia/grasp_hrp2_interp.py
create mode 100644 script/scenarios/demos/siggraph_asia/grasp_hrp2_path.py
diff --git a/CMakeLists.txt b/CMakeLists.txt
index e32b7d7..dede5ec 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
GIT binary patch
literal 1284
zcmb7?u}TC%5Jbn^572Bw4HUf#OwL;r`~g7_QH106Ku}Ls1rwqD0Rz84b0aShL|70*
zBmEz7YUXOJbs{&pg09!qJ=a@TXXE;!8Wy8^d{S&4SEs|OUg|8bb&G}b%R#ZS=>GN1
zMYlO;CFI7zZU1}kvHx-O-W$!#XZmgSdOadj!stSUv}$w$Vdeev!vsoQJ)gI|`q{jA
zn))>(v_cwvsasPu$RL5f`+g5*P&31spA3X)g*05$=!DMJ_&vH%+#HiAO`Q{3A&uFo
z`Bu(N4sSZh(h9eQi&LdN5}_65!KIxreaLw786{o%A3!tj_jUbLfzS&3!lj);grPqa
ziJiOMl7#B7au9s*1|r=RDx_6|j_$vA1s7jIbxzQQ3NuuLMh3ocCF!nE;kK%ok>3?6
znn%ry{H{PSLv>Ejg$iF+b($)!B6o#~ZqBJ%gkgKFFb^)~wFtadnjMpnhGri0bfEBK
MtqS|X#k@%T0NJ{yhX4Qo
literal 0
HcmV?d00001
diff --git a/data/srdf/siggraph_asia/grasp.srdf b/data/srdf/siggraph_asia/grasp.srdf
new file mode 100644
index 0000000..94553c6
--- /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 0000000..5e59557
--- /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 3da7d88..de1e702 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 0000000..7c0d59d
--- /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 0000000..cce1ab6
--- /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 7ae6fd8..ab23625 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 80404b0..9f09189 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 41fc17b..6885954 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 1ff7e83..2230b75 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 a25e474..4481d93 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 77d1d3c..9fb446f 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 6d7687a..4e5ad15 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);
--
GitLab