Skip to content
Snippets Groups Projects
Commit bf371717 authored by Steve Tonneau's avatar Steve Tonneau
Browse files

stair

parent 00c17065
No related branches found
No related tags found
No related merge requests found
No preview for this file type
<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")
File added
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")
File added
......@@ -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;
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment