Commit bf371717 authored by Steve Tonneau's avatar Steve Tonneau
Browse files

stair

parent 00c17065
<robot name="hrp2_rom">
<robot name="box">
<link name="base_link">
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="package://hpp-rbprm-corba/meshes/hrp2_rom.stl"/>
<mesh filename="package://hpp-rbprm-corba/meshes/box_rom.stl"/>
</geometry>
<material name="white">
<color rgba="1 1 1 1"/>
......@@ -12,7 +12,7 @@
<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="package://hpp-rbprm-corba/meshes/hrp2_rom.stl"/>
<mesh filename="package://hpp-rbprm-corba/meshes/box_rom.stl"/>
</geometry>
</collision>
</link>
......
<robot name="hrp2_rom">
<link name="base_link">
<visual>
<origin xyz="0 0 0" rpy="0 0 0.003" />
<geometry>
<mesh filename="package://hpp-rbprm-corba/meshes/hrp2_rom.stl"/>
</geometry>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0.003" />
<geometry>
<mesh filename="package://hpp-rbprm-corba/meshes/hrp2_rom.stl"/>
</geometry>
</collision>
</link>
</robot>
<robot name="hrp2_trunk">
<link name="base_link">
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<origin xyz="0 0 0" rpy="0 0 0.003" />
<geometry>
<mesh filename="package://hpp-rbprm-corba/meshes/hrp2_trunk.stl"/>
</geometry>
......@@ -10,7 +10,7 @@
</material>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
<origin xyz="0 0 0" rpy="0 0 0.003" />
<geometry>
<mesh filename="package://hpp-rbprm-corba/meshes/hrp2_trunk.stl"/>
</geometry>
......
from hpp.corbaserver.rbprm.rbprmbuilder import Builder
from hpp.corbaserver.rbprm.rbprmfullbody import FullBody
from hpp.gepetto import Viewer
import stair_path_hrp2 as tp
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", [-1,1, -4, -1, 1, 2.2])
ps = tp.ProblemSolver( fullBody )
r = tp.Viewer (ps)
r.loadObstacleModel ('hpp-rbprm-corba', "scene_wall", "contact")
#~ AFTER loading obstacles
rLegId = '7rLeg'
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, 20000, 0.01)
lLegId = '8lLeg'
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, 20000, 0.01)
rarmId = '3Rarm'
rarm = 'RARM_JOINT0'
rHand = 'RARM_JOINT5'
rArmOffset = [-0.05,-0.050,-0.050]
rArmNormal = [1,0,0]
rArmx = 0.024; rArmy = 0.024
fullBody.addLimb(rarmId,rarm,rHand,rArmOffset,rArmNormal, rArmx, rArmy, 20000, 0.01)
#~ 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, 20000, 0.01)
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 = fullBody.generateContacts(q_init, [0,0,1])
r (q_init)
fullBody.setCurrentConfig (q_goal)
#~ q_goal = fullBody.generateContacts(q_goal, [0,0,1])
#~ fullBody.setStartState(q_init,[rLegId,lLegId,rarmId,larmId])
#~ fullBody.setEndState(q_goal,[rLegId,lLegId,rarmId,larmId])
#~
#~ configs = fullBody.interpolate(0.1)
#~ configs2 = fullBody.interpolate(0.05)
i = 0;
r (configs[i]); i=i+1; i-1
......@@ -2,6 +2,9 @@ from hpp.corbaserver.rbprm.rbprmbuilder import Builder
from hpp.corbaserver.rbprm.rbprmfullbody import FullBody
from hpp.gepetto import Viewer
import truck_path_hrp2 as tp
packageName = "hrp2_14_description"
meshPackageName = "hrp2_14_description"
......@@ -17,13 +20,18 @@ fullBody = FullBody ()
fullBody.loadFullBodyModel(urdfName, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
fullBody.setJointBounds ("base_joint_xyz", [-1,1, -4, -1, 1, 2.2])
from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
#~ from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
#~ ps = ProblemSolver( fullBody )
#~ ps.addPathOptimizer("RandomShortcut")
#~ ps.addPathOptimizer("GradientBased")
#~ r = Viewer (ps)
#~ r.loadObstacleModel ('hpp-rbprm-corba', "truck", "car")
ps = ProblemSolver( fullBody )
ps.addPathOptimizer("RandomShortcut")
ps.addPathOptimizer("GradientBased")
r = Viewer (ps)
r.loadObstacleModel ('hpp-rbprm-corba', "truck", "car")
ps = tp.ProblemSolver( fullBody )
r = tp.Viewer (ps)
r.loadObstacleModel ('hpp-rbprm-corba', "truck", "contact")
#~ AFTER loading obstacles
rLegId = '7rLeg'
......@@ -87,23 +95,26 @@ q_0 = fullBody.getCurrentConfig (); r (q_0)
q_init = fullBody.getCurrentConfig (); r (q_init)
fullBody.client.basic.robot.setJointConfig('base_joint_SO3',[0.7316888688738209, 0, -0.6816387600233341, 0]); q_init = fullBody.getCurrentConfig (); r (q_init)
#~ fullBody.client.basic.robot.setJointConfig('base_joint_SO3',[0.7316888688738209, 0, -0.6816387600233341, 0]); q_init = fullBody.getCurrentConfig (); r (q_init)
q_init = fullBody.getCurrentConfig (); r (q_init)
q_init [0:6] = [0.0, -2.1, 2.0, 0.7316888688738209, 0.0, 0.6816387600233341]; fullBody.setCurrentConfig (q_init); r (q_init)
q_goal = q_init [::]
q_goal [0:7] = [0.0, -4.0, 2.0, 1.0, 0.0, 0.0, 0.0]
q_goal = [0.0, -4.0, 2.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 1.5, 0.0, 0.0, 0.0, 0.0, 0.0, 1.5, 0.0, 0.0]
ps.setInitialConfig (q_init)
ps.addGoalConfig (q_goal)
r(q_goal)
ps.solve ()
q_init = fullBody.generateContacts(q_init, [0,0,-1])
r (q_init)
from hpp.gepetto import PathPlayer
pp = PathPlayer (fullBody.client.basic, r)
fullBody.setCurrentConfig (q_goal)
q_goal = fullBody.generateContacts(q_goal, [0,0,-1])
#~ pp (1)
fullBody.setStartState(q_init,[rLegId,lLegId,rarmId,larmId])
fullBody.setEndState(q_goal,[rLegId,lLegId,rarmId,larmId])
#~
configs = fullBody.interpolate(0.5)
#~ configs2 = fullBody.interpolate(0.05)
i = 0;
r (configs[i]); i=i+1; i-1
#~ pp.toFile(0, "/home/stonneau/test.txt")
This diff is collapsed.
from hpp.corbaserver.rbprm.rbprmbuilder import Builder
from hpp.gepetto import Viewer
rootJointType = 'freeflyer'
packageName = 'hpp-rbprm-corba'
meshPackageName = 'hpp-rbprm-corba'
urdfName = 'hrp2_trunk'
urdfNameRom = 'hrp2_rom'
urdfSuffix = ""
srdfSuffix = ""
rbprmBuilder = Builder ()
rbprmBuilder.loadModel(urdfName, urdfNameRom, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
rbprmBuilder.setJointBounds ("base_joint_xyz", [-1,1, -1, 1, 0, 2.2])
#~ from hpp.corbaserver.rbprm. import ProblemSolver
from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
ps = ProblemSolver( rbprmBuilder )
r = Viewer (ps)
q_init = rbprmBuilder.getCurrentConfig (); r (q_init)
#~ fullBody.client.basic.robot.setJointConfig('base_joint_SO3',[0.7316888688738209, 0, -0.6816387600233341, 0]); q_init = rbprmBuilder.getCurrentConfig (); r (q_init)
q_init = rbprmBuilder.getCurrentConfig ();
q_init [0:3] = [-0.3, 0, 0.7]; rbprmBuilder.setCurrentConfig (q_init); r (q_init)
#~ q_0 [0:3] = [-0.2, 0, 0.3]; r (q_0)
q_goal = q_init [::]
q_goal [0:3] = [0.1, 0, 1.7]; r (q_goal)
#~ q_init [0:6] = [0.0, -2.2, 2.0, 0.7316888688738209, 0.0, 0.6816387600233341];
#~ rbprmBuilder.setCurrentConfig (q_init); r (q_init)
ps.addPathOptimizer("RandomShortcut")
ps.addPathOptimizer("GradientBased")
ps.setInitialConfig (q_init)
ps.addGoalConfig (q_goal)
ps.client.problem.selectConFigurationShooter("RbprmShooter")
#~ ps.client.problem.selectPathValidation("RbprmPathValidation",0.05)
r.loadObstacleModel (packageName, "scene_wall", "planning")
ps.solve ()
from hpp.gepetto import PathPlayer
pp = PathPlayer (rbprmBuilder.client.basic, r)
#~
pp (2)
#~ pp (1)
#~ pp.fromFile("/home/stonneau/dev/hpp/src/hpp-rbprm-corba/script/paths/truck.path")
#~ pp (1)
#~ pp.toFile(1, "/home/stonneau/dev/hpp/src/hpp-rbprm-corba/script/paths/truck.path")
from hpp.corbaserver.rbprm.rbprmbuilder import Builder
from hpp.gepetto import Viewer
rootJointType = 'freeflyer'
packageName = 'hpp-rbprm-corba'
meshPackageName = 'hpp-rbprm-corba'
urdfName = 'hrp2_trunk'
urdfNameRom = 'hrp2_rom'
urdfSuffix = ""
srdfSuffix = ""
rbprmBuilder = Builder ()
rbprmBuilder.loadModel(urdfName, urdfNameRom, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
rbprmBuilder.setJointBounds ("base_joint_xyz", [-1,1, -4, -1, 1, 2.2])
#~ from hpp.corbaserver.rbprm. import ProblemSolver
from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
ps = ProblemSolver( rbprmBuilder )
r = Viewer (ps)
q_init = rbprmBuilder.getCurrentConfig (); r (q_init)
#~ fullBody.client.basic.robot.setJointConfig('base_joint_SO3',[0.7316888688738209, 0, -0.6816387600233341, 0]); q_init = rbprmBuilder.getCurrentConfig (); r (q_init)
q_init = rbprmBuilder.getCurrentConfig (); r (q_init)
q_init [0:6] = [0.0, -2.2, 2.0, 0.7316888688738209, 0.0, 0.6816387600233341]; rbprmBuilder.setCurrentConfig (q_init); r (q_init)
q_goal = [0.0, -4.0, 2.0, 1.0, 0.0, 0.0, 0.0]
ps.addPathOptimizer("RandomShortcut")
#~ ps.addPathOptimizer("GradientBased")
ps.setInitialConfig (q_init)
ps.addGoalConfig (q_goal)
#~ ps.client.problem.selectConFigurationShooter("RbprmShooter")
#~ ps.client.problem.selectPathValidation("RbprmPathValidation",0.05)
r.loadObstacleModel (packageName, "truck", "planning")
ps.solve ()
from hpp.gepetto import PathPlayer
pp = PathPlayer (rbprmBuilder.client.basic, r)
#~ pp (0)
#~ pp (1)
#~ pp.fromFile("/home/stonneau/dev/hpp/src/hpp-rbprm-corba/script/paths/truck.path")
pp (1)
#~ pp.toFile(1, "/home/stonneau/dev/hpp/src/hpp-rbprm-corba/script/paths/truck.path")
......@@ -233,10 +233,12 @@ namespace hpp {
dir[i] = direction[i];
}
model::Configuration_t config = dofArrayToConfig (fullBody_->device_, configuration);
std::cout << "configuration loaded " << config << std::endl;
rbprm::State state = rbprm::ComputeContacts(fullBody_,config,
problemSolver_->collisionObstacles(), dir);
hpp::floatSeq* dofArray = new hpp::floatSeq();
dofArray->length(_CORBA_ULong(config.rows()));
std::cout << "configuration out " << state.configuration_ << std::endl;
dofArray->length(_CORBA_ULong(state.configuration_.rows()));
for(std::size_t i=0; i< _CORBA_ULong(config.rows()); i++)
(*dofArray)[(_CORBA_ULong)i] = state.configuration_ [i];
return dofArray;
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment