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
790286d4
Commit
790286d4
authored
Nov 10, 2016
by
Steve Tonneau
Browse files
unfinished projection
parent
c5fc6077
Changes
5
Hide whitespace changes
Inline
Side-by-side
src/hpp/corbaserver/rbprm/rbprmfullbody.py
View file @
790286d4
...
...
@@ -253,6 +253,14 @@ class FullBody (object):
# \param contacts the array of limbs in contact
def
setStartState
(
self
,
configuration
,
contacts
):
return
self
.
client
.
rbprm
.
rbprm
.
setStartState
(
configuration
,
contacts
)
## Create a state given a configuration and contacts
#
# \param configuration the desired start configuration
# \param contacts the array of limbs in contact
# \return id of created state
def
createState
(
self
,
configuration
,
contacts
):
return
self
.
client
.
rbprm
.
rbprm
.
createState
(
configuration
,
contacts
)
## Initialize the last configuration of the path discretization
# with a balanced configuration for the interpolation problem;
...
...
src/hpp/corbaserver/rbprm/tools/cwc_trajectory.py
View file @
790286d4
...
...
@@ -159,6 +159,7 @@ reduce_ineq = True, verbose = False, limbsCOMConstraints = None, profile = False
def
draw_trajectory
(
fullBody
,
states
,
state_id
,
computeCones
=
False
,
mu
=
1
,
dt
=
0.2
,
phase_dt
=
[
0.4
,
1
],
reduce_ineq
=
True
,
verbose
=
False
,
limbsCOMConstraints
=
None
,
use_window
=
0
):
var_final
,
params
,
elapsed
=
gen_trajectory
(
fullBody
,
states
,
state_id
,
computeCones
,
mu
,
dt
,
phase_dt
,
reduce_ineq
,
verbose
,
limbsCOMConstraints
,
False
,
use_window
=
use_window
)
p
,
N
=
fullBody
.
computeContactPoints
(
state_id
)
print
"p"
,
p
fig
=
plt
.
figure
()
ax
=
fig
.
add_subplot
(
111
,
projection
=
'3d'
)
n
=
100
...
...
@@ -180,8 +181,8 @@ def draw_trajectory(fullBody, states, state_id, computeCones = False, mu = 1, d
ax
.
set_ylabel
(
'Y Label'
)
ax
.
set_zlabel
(
'Z Label'
)
#~
plt.show()
plt
.
savefig
(
'/tmp/c'
+
str
(
state_id
)
+
'.png'
)
plt
.
show
()
#~
plt.savefig('/tmp/c'+ str(state_id)+ '.png')
print
"plotting speed "
print
"end target "
,
params
[
'x_end'
]
...
...
@@ -199,8 +200,8 @@ def draw_trajectory(fullBody, states, state_id, computeCones = False, mu = 1, d
ax
.
scatter
(
xs
,
ys
,
c
=
'b'
)
#~
plt.show()
plt
.
savefig
(
'/tmp/dc'
+
str
(
state_id
)
+
'.png'
)
plt
.
show
()
#~
plt.savefig('/tmp/dc'+ str(state_id)+ '.png')
print
"plotting acceleration "
fig
=
plt
.
figure
()
...
...
@@ -215,7 +216,7 @@ def draw_trajectory(fullBody, states, state_id, computeCones = False, mu = 1, d
ax
.
scatter
(
xs
,
ys
,
c
=
'b'
)
#~
plt.show()
plt
.
show
()
plt
.
savefig
(
'/tmp/ddc'
+
str
(
state_id
)
+
'.png'
)
print
"plotting Dl "
...
...
@@ -227,7 +228,7 @@ def draw_trajectory(fullBody, states, state_id, computeCones = False, mu = 1, d
ax
.
scatter
(
xs
,
ys
,
c
=
'b'
)
#~
plt.show()
plt
.
show
()
plt
.
savefig
(
'/tmp/dL'
+
str
(
state_id
)
+
'.png'
)
return
var_final
,
params
,
elapsed
...
...
src/hpp/corbaserver/rbprm/tools/cwc_trajectory_helper.py
View file @
790286d4
...
...
@@ -158,16 +158,16 @@ trackedEffectors = []):
global
trajec
global
trajec_mil
frame_rate
=
1.
/
24.
frame_rate_andrea
=
1.
/
1000.
#~
frame_rate_andrea = 1./1000.
#~ if(len(trajec) > 0):
#~ frame_rate = 1./25.
#~ frame_rate_andrea = 1./1001.
new_traj
=
gen_trajectory_to_play
(
fullBody
,
pp
,
trajectory
,
time_per_path
,
frame_rate
)
new_traj_andrea
=
gen_trajectory_to_play
(
fullBody
,
pp
,
trajectory
,
time_per_path
,
frame_rate_andrea
)
#~
new_traj_andrea = gen_trajectory_to_play(fullBody, pp, trajectory, time_per_path,frame_rate_andrea)
#~ new_contacts = gencontactsPerFrame(fullBody, i, limbsCOMConstraints, pp, trajectory, times, frame_rate_andrea)
Ps
,
Ns
,
freeEffectorsPerPhase
=
genPandNperFrame
(
fullBody
,
i
,
limbsCOMConstraints
,
pp
,
trajectory
,
time_per_path
,
frame_rate_andrea
)
NPeffs
=
genPEffperFrame
(
fullBody
,
freeEffectorsPerPhase
,
new_traj_andrea
,
pp
,
time_per_path
,
frame_rate_andrea
)
com
=
genComPerFrame
(
final_state
,
dt
,
frame_rate_andrea
)
#~
Ps, Ns, freeEffectorsPerPhase = genPandNperFrame(fullBody, i, limbsCOMConstraints, pp, trajectory, time_per_path, frame_rate_andrea)
#~
NPeffs = genPEffperFrame(fullBody, freeEffectorsPerPhase, new_traj_andrea, pp, time_per_path, frame_rate_andrea)
#~
com = genComPerFrame(final_state, dt, frame_rate_andrea)
#~ if(len(trajec) > 0):
#~ new_traj = new_traj[1:]
#~ new_traj_andrea = new_traj_andrea[1:]
...
...
@@ -176,19 +176,19 @@ trackedEffectors = []):
#~ com = com[1:]
#~ NPeffs = NPeffs[1:]
trajec
=
trajec
+
new_traj
trajec_mil
+=
new_traj_andrea
#~
trajec_mil += new_traj_andrea
#~ global contacts
#~ contacts += new_contacts
global
pos
pos
+=
Ps
global
normals
normals
+=
Ns
global
pEffs
pEffs
+=
NPeffs
global
coms
coms
+=
com
print
len
(
trajec_mil
),
" "
,
len
(
pos
),
" "
,
len
(
normals
),
" "
,
len
(
coms
),
" "
,
len
(
pEffs
)
assert
(
len
(
trajec_mil
)
==
len
(
pos
)
and
len
(
normals
)
==
len
(
pos
)
and
len
(
normals
)
==
len
(
coms
)
and
len
(
coms
)
==
len
(
pEffs
))
#~
global pos
#~
pos += Ps
#~
global normals
#~
normals+= Ns
#~
global pEffs
#~
pEffs+= NPeffs
#~
global coms
#~
coms+= com
#~
print len(trajec_mil), " ", len(pos), " ", len(normals), " ", len(coms), " ", len(pEffs)
#~
assert(len(trajec_mil) == len(pos) and len(normals) == len(pos) and len(normals) == len(coms) and len(coms) == len(pEffs))
stat_data
[
"num_success"
]
+=
1
else
:
print
"TODO, NO CONTACT VARIATION, LINEAR INTERPOLATION REQUIRED"
...
...
src/rbprmbuilder.impl.cc
View file @
790286d4
...
...
@@ -812,6 +812,61 @@ namespace hpp {
}
}
hpp
::
floatSeq
*
RbprmBuilder
::
getContactSamplesIdsAndProject
(
const
char
*
limbname
,
const
hpp
::
floatSeq
&
configuration
,
const
hpp
::
floatSeq
&
direction
,
unsigned
short
numSamples
)
throw
(
hpp
::
Error
)
{
if
(
!
fullBodyLoaded_
)
throw
Error
(
"No full body robot was loaded"
);
try
{
fcl
::
Vec3f
dir
;
for
(
std
::
size_t
i
=
0
;
i
<
3
;
++
i
)
{
dir
[
i
]
=
direction
[(
_CORBA_ULong
)
i
];
}
model
::
Configuration_t
config
=
dofArrayToConfig
(
fullBody_
->
device_
,
configuration
);
model
::
Configuration_t
save
=
fullBody_
->
device_
->
currentConfiguration
();
fullBody_
->
device_
->
currentConfiguration
(
config
);
sampling
::
T_OctreeReport
finalSet
;
rbprm
::
T_Limb
::
const_iterator
lit
=
fullBody_
->
GetLimbs
().
find
(
std
::
string
(
limbname
));
if
(
lit
==
fullBody_
->
GetLimbs
().
end
())
{
throw
std
::
runtime_error
(
"Impossible to find limb for joint "
+
std
::
string
(
limbname
)
+
" to robot; limb not defined."
);
}
const
RbPrmLimbPtr_t
&
limb
=
lit
->
second
;
fcl
::
Transform3f
transform
=
limb
->
limb_
->
robot
()
->
rootJoint
()
->
childJoint
(
0
)
->
currentTransformation
();
// get root transform from configuration
// TODO fix as in rbprm-fullbody.cc!!
std
::
vector
<
sampling
::
T_OctreeReport
>
reports
(
problemSolver_
->
collisionObstacles
().
size
());
std
::
size_t
i
(
0
);
//#pragma omp parallel for
for
(
model
::
ObjectVector_t
::
const_iterator
oit
=
problemSolver_
->
collisionObstacles
().
begin
();
oit
!=
problemSolver_
->
collisionObstacles
().
end
();
++
oit
,
++
i
)
{
sampling
::
GetCandidates
(
limb
->
sampleContainer_
,
transform
,
*
oit
,
dir
,
reports
[
i
]);
}
for
(
std
::
vector
<
sampling
::
T_OctreeReport
>::
const_iterator
cit
=
reports
.
begin
();
cit
!=
reports
.
end
();
++
cit
)
{
finalSet
.
insert
(
cit
->
begin
(),
cit
->
end
());
}
hpp
::
floatSeq
*
dofArray
=
new
hpp
::
floatSeq
();
dofArray
->
length
((
_CORBA_ULong
)
finalSet
.
size
());
sampling
::
T_OctreeReport
::
const_iterator
candCit
=
finalSet
.
begin
();
for
(
std
::
size_t
i
=
0
;
i
<
_CORBA_ULong
(
finalSet
.
size
());
++
i
,
++
candCit
)
{
(
*
dofArray
)[(
_CORBA_ULong
)
i
]
=
(
double
)
candCit
->
sample_
->
id_
;
}
fullBody_
->
device_
->
currentConfiguration
(
save
);
return
dofArray
;
}
catch
(
const
std
::
exception
&
exc
)
{
throw
hpp
::
Error
(
exc
.
what
());
}
}
hpp
::
floatSeq
*
RbprmBuilder
::
getSamplesIdsInOctreeNode
(
const
char
*
limb
,
double
octreeNodeId
)
throw
(
hpp
::
Error
)
{
...
...
src/rbprmbuilder.impl.hh
View file @
790286d4
...
...
@@ -136,6 +136,11 @@ namespace hpp {
const
hpp
::
floatSeq
&
configuration
,
const
hpp
::
floatSeq
&
direction
)
throw
(
hpp
::
Error
);
virtual
hpp
::
floatSeq
*
getContactSamplesIdsAndProject
(
const
char
*
limb
,
const
hpp
::
floatSeq
&
configuration
,
const
hpp
::
floatSeq
&
direction
,
unsigned
short
numSamples
)
throw
(
hpp
::
Error
);
virtual
hpp
::
floatSeq
*
getSamplesIdsInOctreeNode
(
const
char
*
limb
,
double
octreeNodeId
)
throw
(
hpp
::
Error
);
...
...
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