Commit 538ca47d authored by Pierre Fernbach's avatar Pierre Fernbach
Browse files

[script] update slalom script

parent 86155073
...@@ -7,7 +7,7 @@ import time ...@@ -7,7 +7,7 @@ import time
import omniORB.any import omniORB.any
from constraint_to_dae import * from constraint_to_dae import *
from display_tools import * from display_tools import *
from configs.stairs10_bauzil_stairs import * from configs.slalom_bauzil import *
from disp_bezier import * from disp_bezier import *
from hpp.corbaserver.rbprm.rbprmstate import State,StateHelper from hpp.corbaserver.rbprm.rbprmstate import State,StateHelper
...@@ -24,7 +24,7 @@ fullBody = FullBody () ...@@ -24,7 +24,7 @@ fullBody = FullBody ()
tPlanning = 0. tPlanning = 0.
fullBody.loadFullBodyModel(urdfName, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix) fullBody.loadFullBodyModel(urdfName, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
fullBody.setJointBounds ("base_joint_xyz", [-1,2.5,0.5 ,3, 0.5, 0.7]) fullBody.setJointBounds ("base_joint_xyz", [-1,2.5,0.5 ,3, 0.4, 0.8])
fullBody.setJointBounds ("LLEG_JOINT3", [0.35,2.61799]) fullBody.setJointBounds ("LLEG_JOINT3", [0.35,2.61799])
fullBody.setJointBounds ("RLEG_JOINT3", [0.35,2.61799]) fullBody.setJointBounds ("RLEG_JOINT3", [0.35,2.61799])
...@@ -55,7 +55,7 @@ rLegLimbOffset=[0,0,-0.035]#0.035 ...@@ -55,7 +55,7 @@ rLegLimbOffset=[0,0,-0.035]#0.035
rLegNormal = [0,0,1] rLegNormal = [0,0,1]
rLegx = 0.09; rLegy = 0.05 rLegx = 0.09; rLegy = 0.05
#fullBody.addLimbDatabase("./db/hrp2_rleg_db.db",rLegId,"forward") #fullBody.addLimbDatabase("./db/hrp2_rleg_db.db",rLegId,"forward")
fullBody.addLimb(rLegId,rleg,'',rLegOffset,rLegNormal, rLegx, rLegy, 100000, "fixedStep06", 0.01,"_6_DOF",kinematicConstraintsPath = "package://hpp-rbprm-corba/com_inequalities/fullsize/RLEG_JOINT0_com_constraints.obj",limbOffset=rLegLimbOffset,kinematicConstraintsMin=0.6) fullBody.addLimb(rLegId,rleg,'',rLegOffset,rLegNormal, rLegx, rLegy, 100000, "fixedStep06", 0.01,"_6_DOF",kinematicConstraintsPath = "package://hpp-rbprm-corba/com_inequalities/fullSize/RLEG_JOINT0_com_constraints.obj",limbOffset=rLegLimbOffset,kinematicConstraintsMin=0.4)
fullBody.runLimbSampleAnalysis(rLegId, "ReferenceConfiguration", True) fullBody.runLimbSampleAnalysis(rLegId, "ReferenceConfiguration", True)
#fullBody.saveLimbDatabase(rLegId, "./db/hrp2_rleg_db.db") #fullBody.saveLimbDatabase(rLegId, "./db/hrp2_rleg_db.db")
...@@ -65,7 +65,7 @@ lLegLimbOffset=[0,0,0.035] ...@@ -65,7 +65,7 @@ lLegLimbOffset=[0,0,0.035]
lLegNormal = [0,0,1] lLegNormal = [0,0,1]
lLegx = 0.09; lLegy = 0.05 lLegx = 0.09; lLegy = 0.05
#fullBody.addLimbDatabase("./db/hrp2_lleg_db.db",lLegId,"forward") #fullBody.addLimbDatabase("./db/hrp2_lleg_db.db",lLegId,"forward")
fullBody.addLimb(lLegId,lleg,'',lLegOffset,rLegNormal, lLegx, lLegy, 100000, "fixedStep06", 0.01,"_6_DOF",kinematicConstraintsPath = "package://hpp-rbprm-corba/com_inequalities/fullsize/LLEG_JOINT0_com_constraints.obj",limbOffset=lLegLimbOffset,kinematicConstraintsMin=0.6) fullBody.addLimb(lLegId,lleg,'',lLegOffset,rLegNormal, lLegx, lLegy, 100000, "fixedStep06", 0.01,"_6_DOF",kinematicConstraintsPath = "package://hpp-rbprm-corba/com_inequalities/fullSize/LLEG_JOINT0_com_constraints.obj",limbOffset=lLegLimbOffset,kinematicConstraintsMin=0.4)
fullBody.runLimbSampleAnalysis(lLegId, "ReferenceConfiguration", True) fullBody.runLimbSampleAnalysis(lLegId, "ReferenceConfiguration", True)
#fullBody.saveLimbDatabase(lLegId, "./db/hrp2_lleg_db.db") #fullBody.saveLimbDatabase(lLegId, "./db/hrp2_lleg_db.db")
...@@ -99,7 +99,7 @@ acc_init = [0,0,0] ...@@ -99,7 +99,7 @@ acc_init = [0,0,0]
dir_goal = tp.ps.configAtParam(pId,eps)[tp.indexECS:tp.indexECS+3] dir_goal = tp.ps.configAtParam(pId,eps)[tp.indexECS:tp.indexECS+3]
acc_goal = [0,0,0] acc_goal = [0,0,0]
robTreshold = 3 robTreshold = 1
# copy extraconfig for start and init configurations # copy extraconfig for start and init configurations
q_init[configSize:configSize+3] = dir_init[::] q_init[configSize:configSize+3] = dir_init[::]
q_init[configSize+3:configSize+6] = acc_init[::] q_init[configSize+3:configSize+6] = acc_init[::]
...@@ -169,7 +169,7 @@ import check_qp ...@@ -169,7 +169,7 @@ import check_qp
#cs = generateContactSequence(fullBody,configs,beginState, endState,curves_initGuess =curves_initGuess , timings_initGuess =timings_initGuess) #cs = generateContactSequence(fullBody,configs,beginState, endState,curves_initGuess =curves_initGuess , timings_initGuess =timings_initGuess)
#displayContactSequence(r,configsFull) displayContactSequence(r,configsFull)
filename = OUTPUT_DIR + "/" + OUTPUT_SEQUENCE_FILE filename = OUTPUT_DIR + "/" + OUTPUT_SEQUENCE_FILE
......
...@@ -51,6 +51,7 @@ vMax = 0.2; ...@@ -51,6 +51,7 @@ vMax = 0.2;
aMax = 0.1; aMax = 0.1;
extraDof = 6 extraDof = 6
#rbprmBuilder.setJointBounds ("base_joint_xyz", [-1,2.5,0.5 ,3, 0.6, 0.6])
rbprmBuilder.setJointBounds ("base_joint_xyz", [-1,2.5,0.5 ,3, 0.6, 0.6]) rbprmBuilder.setJointBounds ("base_joint_xyz", [-1,2.5,0.5 ,3, 0.6, 0.6])
rbprmBuilder.setJointBounds('CHEST_JOINT0',[-0.05,0.05]) rbprmBuilder.setJointBounds('CHEST_JOINT0',[-0.05,0.05])
rbprmBuilder.setJointBounds('CHEST_JOINT1',[-0.05,0.05]) rbprmBuilder.setJointBounds('CHEST_JOINT1',[-0.05,0.05])
...@@ -86,10 +87,10 @@ r.addLandmark(r.sceneName,1) ...@@ -86,10 +87,10 @@ r.addLandmark(r.sceneName,1)
# Setting initial and goal configurations # Setting initial and goal configurations
q_init = rbprmBuilder.getCurrentConfig (); q_init = rbprmBuilder.getCurrentConfig ();
q_init[3:7] = [1,0,0,0] q_init[3:7] = [1,0,0,0]
#q_init [0:3] = [-0.9, 1, 0.6]; r (q_init) q_init [0:3] = [-0.9, 1, 0.6]; r (q_init)
q_init [0:3] = [0.5, 2.5, 0.6]; r (q_init) #q_init [0:3] = [0.5, 2.5, 0.6]; r (q_init)
q_init[-6:-3] = [0.05,0,0] q_init[-6:-3] = [0.05,0,0]
#q_init [0:3] = [0.05, 0.86, 0.59]; r (q_init)
rbprmBuilder.setCurrentConfig (q_init) rbprmBuilder.setCurrentConfig (q_init)
q_goal = q_init [::] q_goal = q_init [::]
......
...@@ -6,7 +6,7 @@ from hpp.gepetto import Viewer ...@@ -6,7 +6,7 @@ from hpp.gepetto import Viewer
import time import time
import math import math
import omniORB.any import omniORB.any
from planning.config import * from configs.slalom_bauzil import *
from hpp.corbaserver import Client from hpp.corbaserver import Client
from hpp.corbaserver.robot import Robot as Parent from hpp.corbaserver.robot import Robot as Parent
......
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