Skip to content
GitLab
Projects
Groups
Snippets
/
Help
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Contribute to GitLab
Sign in / Register
Toggle navigation
Menu
Open sidebar
Jason Chemin
hpp-rbprm-corba
Commits
b28053d8
Commit
b28053d8
authored
Jan 14, 2019
by
stevet
Browse files
cleaning darpa
parent
4437e7c0
Changes
1
Hide whitespace changes
Inline
Side-by-side
script/scenarios/demos/darpa_hyq.py
View file @
b28053d8
...
...
@@ -9,7 +9,7 @@ from hyq_ref_pose import hyq_ref
import
darpa_hyq_path
as
tp
from
os
import
environ
ins_dir
=
environ
[
'DEVEL_DIR'
]
ins_dir
=
environ
[
'DEVEL_
HPP_
DIR'
]
db_dir
=
ins_dir
+
"/install/share/hyq-rbprm/database/hyq_"
...
...
@@ -42,7 +42,6 @@ cType = "_3_DOF"
rLegId
=
'rfleg'
rLeg
=
'rf_haa_joint'
rfoot
=
'rf_foot_joint'
#~ offset = [0.,-0.021,0.]
offset
=
[
0.
,
-
0.021
,
0.
]
normal
=
[
0
,
1
,
0
]
legx
=
0.02
;
legy
=
0.02
...
...
@@ -72,11 +71,6 @@ fullBody.addLimb(larmId,larm,lHand,offset,normal, legx, legy, nbSamples, "random
#~ fullBody.runLimbSampleAnalysis(rarmId, "jointLimitsDistance", True)
#~ fullBody.runLimbSampleAnalysis(larmId, "jointLimitsDistance", True)
#~ q_init = hyq_ref[:]; q_init[0:7] = tp.q_init[0:7];
#~ q_goal = hyq_ref[:]; q_goal[0:7] = tp.q_goal[0:7];
q_init
=
hyq_ref
[:];
q_init
[
0
:
7
]
=
tp
.
q_init
[
0
:
7
];
q_init
[
2
]
=
hyq_ref
[
2
]
q_goal
=
hyq_ref
[:];
q_goal
[
0
:
7
]
=
tp
.
q_goal
[
0
:
7
];
q_goal
[
2
]
=
hyq_ref
[
2
]
+
0.02
q_init
=
[
-
2.0
,
0.0
,
0.6838277139631803
,
...
...
@@ -96,15 +90,10 @@ q_init = [-2.0,
0.03995660287873871
,
-
0.9577096766517215
,
0.9384602821326071
]
q_goal
=
hyq_ref
[:];
q_goal
[
0
:
7
]
=
tp
.
q_goal
[
0
:
7
];
q_goal
[
2
]
=
hyq_ref
[
2
]
+
0.02
# Randomly generating a contact configuration at q_init
#~ fullBody.setCurrentConfig (q_init)
#~ q_init = fullBody.generateContacts(q_init, [0,0,1])
# Randomly generating a contact configuration at q_end
#~ fullBody.setCurrentConfig (q_goal)
#~ q_goal = fullBody.generateContacts(q_goal, [0,0,1])
# specifying the full body configurations as start and goal state of the problem
fullBody
.
setStartState
(
q_init
,[
rLegId
,
lLegId
,
rarmId
,
larmId
])
...
...
@@ -169,90 +158,7 @@ def contactPlan(step = 0.5):
r
(
configs
[
i
]);
time
.
sleep
(
step
)
def
contactPlanDontMove
(
step
=
0.5
):
r
.
client
.
gui
.
setVisibility
(
"hyq"
,
"ON"
)
tp
.
cl
.
problem
.
selectProblem
(
"default"
)
tp
.
r
.
client
.
gui
.
setVisibility
(
"toto"
,
"OFF"
)
tp
.
r
.
client
.
gui
.
setVisibility
(
"hyq_trunk_large"
,
"OFF"
)
for
i
in
range
(
0
,
len
(
configs
)):
a
=
configs
[
i
]
a
[:
6
]
=
[
0
for
_
in
range
(
6
)]
a
[
6
]
=
1
#~ r(configs[i]);
r
(
a
);
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.06
):
print
"computing contact plan"
genPlan
(
step
)
def
e
(
step
=
0.5
):
print
"displaying contact plan"
contactPlan
(
step
)
def
f
(
step
=
0.5
):
print
"displaying static contact plan"
contactPlanDontMove
(
step
)
print
"Root path generated in "
+
str
(
tp
.
t
)
+
" ms."
#~ d();e()
d
(
0.01
);
e
(
0.01
)
#~ d(0.004);e(0.01)
from
hpp.corbaserver.rbprm.rbprmstate
import
*
com
=
fullBody
.
getCenterOfMass
()
s
=
None
def
d1
():
global
s
s
=
State
(
fullBody
,
q
=
q_init
,
limbsIncontact
=
[
larmId
])
a
=
s
.
q
()
a
[
2
]
=
a
[
2
]
+
0.01
s
.
setQ
(
a
)
return
s
.
projectToCOM
([
0.01
,
0.
,
0.
],
maxNumSample
=
0
)
def
d2
():
global
s
s
=
State
(
fullBody
,
q
=
q_init
,
limbsIncontact
=
[
larmId
,
rarmId
])
a
=
s
.
q
()
a
[
2
]
=
a
[
2
]
+
0.05
a
[
0
]
=
a
[
0
]
+
0.05
s
.
setQ
(
a
)
return
s
.
projectToCOM
([
0.01
,
0.
,
0.
],
maxNumSample
=
0
)
def
d3
():
global
s
s
=
State
(
fullBody
,
q
=
q_init
,
limbsIncontact
=
[
rarmId
])
a
=
s
.
q
()
a
[
2
]
=
a
[
2
]
+
0.01
s
.
setQ
(
a
)
return
s
.
projectToCOM
([
0.01
,
0.
,
0.
],
maxNumSample
=
0
)
def
d4
():
global
s
s
=
State
(
fullBody
,
q
=
q_init
,
limbsIncontact
=
[
rarmId
])
a
=
s
.
q
()
a
[
2
]
=
a
[
2
]
-
0.01
s
.
setQ
(
a
)
print
s
.
projectToCOM
([
0.01
,
0.
,
0.
],
maxNumSample
=
0
)
s
=
State
(
fullBody
,
q
=
s
.
q
(),
limbsIncontact
=
[
larmId
])
return
s
.
projectToCOM
([
0.01
,
0.
,
0.
],
maxNumSample
=
0
)
genPlan
(
0.01
);
contactPlan
(
0.01
)
Write
Preview
Supports
Markdown
0%
Try again
or
attach a new file
.
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Cancel
Please
register
or
sign in
to comment