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
Guilhem Saurel
ospi
Commits
cdd5ab7d
Commit
cdd5ab7d
authored
May 06, 2020
by
Guilhem Saurel
Browse files
update for pinocchio 2.4.3
parent
1e95a8ef
Changes
3
Hide whitespace changes
Inline
Side-by-side
ospi/model_builder.py
View file @
cdd5ab7d
import
pinocchio
as
se3
import
numpy
as
np
import
utils
from
IPython
import
embed
import
pinocchio
as
se3
import
utils
class
MS
:
def
__init__
(
self
,
name
=
"whole-body MS model"
):
# Pinocchio Model
self
.
model
=
se3
.
Model
.
BuildEmptyModel
()
# Skelette Meshes
self
.
visuals
=
[]
self
.
visuals
.
append
([
0
,
'ground'
,
'none'
])
self
.
forces
=
[]
self
.
joint_transformations
=
[]
self
.
name
=
name
#self.FrameType = se3.FrameType.OP_FRAME
def
buildModel
(
self
,
parent
,
joint_model
,
joint_placement
,
joint_name
,
joint_id
,
body_inertia
,
body_placement
,
body_name
):
''' Add a model to the kinematic three
def
__init__
(
self
,
name
=
"whole-body MS model"
):
# Pinocchio Model
self
.
model
=
se3
.
Model
()
# Skelette Meshes
self
.
visuals
=
[]
self
.
visuals
.
append
([
0
,
'ground'
,
'none'
])
self
.
forces
=
[]
self
.
joint_transformations
=
[]
self
.
name
=
name
#self.FrameType = se3.FrameType.OP_FRAME
def
buildModel
(
self
,
parent
,
joint_model
,
joint_placement
,
joint_name
,
joint_id
,
body_inertia
,
body_placement
,
body_name
):
''' Add a model to the kinematic three
TODO add with bounds, check model.hpp
'''
frame_parent
=
self
.
model
.
getFrameId
(
joint_name
)
#print 'frame_parent: ', frame_parent
#self.model.addFrame(joint_name, joint_id, frame_parent, joint_placement, se3.FrameType.JOINT)
self
.
model
.
addJoint
(
parent
,
joint_model
,
joint_placement
,
joint_name
)
self
.
model
.
addJointFrame
(
joint_id
,
frame_parent
)
''' Append a body to the given joint in the kinematic tree
frame_parent
=
self
.
model
.
getFrameId
(
joint_name
)
#print 'frame_parent: ', frame_parent
#self.model.addFrame(joint_name, joint_id, frame_parent, joint_placement, se3.FrameType.JOINT)
self
.
model
.
addJoint
(
parent
,
joint_model
,
joint_placement
,
joint_name
)
self
.
model
.
addJointFrame
(
joint_id
,
frame_parent
)
''' Append a body to the given joint in the kinematic tree
'''
self
.
model
.
appendBodyToJoint
(
joint_id
,
body_inertia
,
body_placement
)
self
.
model
.
addBodyFrame
(
body_name
,
joint_id
,
body_placement
,
parent
)
''' Add a frame to the frame three i.e. operational points
self
.
model
.
appendBodyToJoint
(
joint_id
,
body_inertia
,
body_placement
)
self
.
model
.
addBodyFrame
(
body_name
,
joint_id
,
body_placement
,
parent
)
''' Add a frame to the frame three i.e. operational points
'''
#self.model.addFrame(joint_name, parent, idx_f, body_placement, self.FrameType)
#return self.model
def
createVisuals
(
self
,
parent
,
joint_name
,
filename
,
scale_factors
=
None
,
transform
=
None
):
self
.
visuals
.
append
([
parent
,
joint_name
,
filename
,
scale_factors
,
transform
])
def
createForces
(
self
,
force_name
,
force_type
,
parent
,
points
):
#TODO
self
.
forces
.
append
([
force_name
,
force_type
,
parent
,
points
])
def
createConstraints
(
self
,
qRoM
):
self
.
model
.
lowerPositionLimit
=
utils
.
pinocchioCoordinates
(
self
.
model
,
self
.
joint_transformations
,
qRoM
[:,
0
])
self
.
model
.
upperPositionLimit
=
utils
.
pinocchioCoordinates
(
self
.
model
,
self
.
joint_transformations
,
qRoM
[:,
1
])
#TODO
#self.Model.effortLimit
#self.Model.velocityLimit
def
createData
(
self
):
self
.
data
=
self
.
model
.
createData
()
def
createJointTransformations
(
self
,
joint_transformations
):
self
.
joint_transformations
=
joint_transformations
#self.model.addFrame(joint_name, parent, idx_f, body_placement, self.FrameType)
#return self.model
def
createVisuals
(
self
,
parent
,
joint_name
,
filename
,
scale_factors
=
None
,
transform
=
None
):
self
.
visuals
.
append
([
parent
,
joint_name
,
filename
,
scale_factors
,
transform
])
def
createForces
(
self
,
force_name
,
force_type
,
parent
,
points
):
#TODO
self
.
forces
.
append
([
force_name
,
force_type
,
parent
,
points
])
def
createConstraints
(
self
,
qRoM
):
self
.
model
.
lowerPositionLimit
=
utils
.
pinocchioCoordinates
(
self
.
model
,
self
.
joint_transformations
,
qRoM
[:,
0
])
self
.
model
.
upperPositionLimit
=
utils
.
pinocchioCoordinates
(
self
.
model
,
self
.
joint_transformations
,
qRoM
[:,
1
])
#TODO
#self.Model.effortLimit
#self.Model.velocityLimit
def
createData
(
self
):
self
.
data
=
self
.
model
.
createData
()
def
createJointTransformations
(
self
,
joint_transformations
):
self
.
joint_transformations
=
joint_transformations
ospi/viewer_utils.py
View file @
cdd5ab7d
...
...
@@ -3,7 +3,7 @@ import gepetto.corbaserver
import
numpy
as
np
import
pinocchio
as
se3
import
os
from
pinocchio.utils
import
XYZQUATToViewerConfiguration
,
zero
,
se3ToXYZQUAT
from
pinocchio.utils
import
zero
,
se3ToXYZQUAT
from
pinocchio.utils
import
zero
as
mat_zeros
from
IPython
import
embed
ENABLE_VIEWER
=
"ON"
...
...
@@ -96,7 +96,6 @@ class Viewer(object):
layout. If multiple objects have to be placed at the same time, do the refresh only
at the end of the list
'''
#pinocchioConf = XYZQUATToViewerConfiguration(se3ToXYZQUAT(M))
pinocchioConf
=
se3
.
utils
.
se3ToXYZQUAT
(
M
)
self
.
viewer
.
gui
.
applyConfiguration
(
objName
,
pinocchioConf
)
if
refresh
:
self
.
viewer
.
gui
.
refresh
()
...
...
ospi/wrapper.py
View file @
cdd5ab7d
...
...
@@ -4,7 +4,7 @@ import pinocchio as se3
import
numpy
as
np
import
time
import
os
from
pinocchio.utils
import
XYZQUATToViewerConfiguration
,
zero
,
se3ToXYZQUAT
from
pinocchio.utils
import
zero
,
se3ToXYZQUAT
from
bmtools.algebra
import
quaternion_from_matrix
,
euler_matrix
from
bmtools.filters
import
*
...
...
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