Skip to content
Snippets Groups Projects
Commit 1e3d6469 authored by Thomas Moulard's avatar Thomas Moulard
Browse files

Factorize initialization in tools, make display optional.

parent 6edb1c23
No related branches found
No related tags found
1 merge request!1[major][cpp] starting point to integrate pinocchio
...@@ -57,6 +57,9 @@ class Hrp2(AbstractHumanoidRobot): ...@@ -57,6 +57,9 @@ class Hrp2(AbstractHumanoidRobot):
0.261799, 0.17453, 0., -0.523599, 0., 0., 0.1, 0.261799, 0.17453, 0., -0.523599, 0., 0., 0.1,
) )
def smallToFull(self, config):
res = (config + 10*(0.,))
return res
def __init__(self, name, def __init__(self, name,
simu, simu,
......
...@@ -204,6 +204,17 @@ class AbstractHumanoidRobot (object): ...@@ -204,6 +204,17 @@ class AbstractHumanoidRobot (object):
else: else:
self.simu = False self.simu = False
def restart(self):
if self.robotSimu:
self.robotSimu.set(self.halfSitting)
self.dynamicRobot.signal('position').value = self.halfSitting
self.dynamicRobot.signal('velocity').value = self.dimension * (0.,)
self.dynamicRobot.signal('acceleration').value = self.dimension * (0.,)
if self.robotSimu:
self.robotSimu.increment(.001)
class HumanoidRobot(AbstractHumanoidRobot): class HumanoidRobot(AbstractHumanoidRobot):
......
...@@ -21,57 +21,9 @@ from dynamic_graph.sot.dynamics.hrp2 import Hrp2 ...@@ -21,57 +21,9 @@ from dynamic_graph.sot.dynamics.hrp2 import Hrp2
from dynamic_graph import enableTrace, plug from dynamic_graph import enableTrace, plug
# Robotviewer is optional from tools import *
enableRobotViewer = True
try:
import robotviewer
except ImportError:
enableRobotViewer = False
def displayHomogeneousMatrix(matrix):
"""
Display nicely a 4x4 matrix (usually homogeneous matrix).
"""
import itertools
matrix_tuple = tuple(itertools.chain.from_iterable(matrix))
formatStr = ''
for i in xrange(4*4):
formatStr += '{0[' + str(i) + ']: <10} '
if i != 0 and (i + 1) % 4 == 0:
formatStr += '\n'
print formatStr.format(matrix_tuple)
def toList(tupleOfTuple):
result = [[0, 0, 0, 0],
[0, 0, 0, 0],
[0, 0, 0, 0],
[0, 0, 0, 0]]
for i in xrange(4):
for j in xrange(4):
result[i][j] = tupleOfTuple[i][j]
return result
def toTuple(listOfList):
return ((listOfList[0][0], listOfList[0][1],
listOfList[0][2], listOfList[0][3]),
(listOfList[1][0], listOfList[1][1],
listOfList[1][2], listOfList[1][3]),
(listOfList[2][0], listOfList[2][1],
listOfList[2][2], listOfList[2][3]),
(listOfList[3][0], listOfList[3][1],
listOfList[3][2], listOfList[3][3]))
def smallToFull(config):
res = (config + 10*(0.,))
return res
class QuasiStaticWalking: class QuasiStaticWalking:
leftFoot = 0 leftFoot = 0
rightFoot = 1 rightFoot = 1
...@@ -200,34 +152,6 @@ class QuasiStaticWalking: ...@@ -200,34 +152,6 @@ class QuasiStaticWalking:
# Trigger actions to move to next state. # Trigger actions to move to next state.
self.do(self.state) self.do(self.state)
robot = Hrp2("robot", True)
# Initialize robotviewer is possible.
clt = None
if enableRobotViewer:
try:
clt = robotviewer.client()
except:
enableRobotViewer = False
timeStep = .02
class Solver:
robot = None
sot = None
def __init__(self, robot):
self.robot = robot
self.sot = SOT('solver')
self.sot.signal('damping').value = 1e-6
self.sot.setNumberDofs(self.robot.dimension)
if robot.robotSimu:
plug(self.sot.signal('control'), robot.robotSimu.signal('control'))
plug(self.robot.robotSimu.signal('state'),
self.robot.dynamicRobot.signal('position'))
solver = Solver (robot)
# Push tasks # Push tasks
# Feet tasks. # Feet tasks.
...@@ -262,7 +186,7 @@ for i in xrange(totalSteps + 1): ...@@ -262,7 +186,7 @@ for i in xrange(totalSteps + 1):
if clt: if clt:
clt.updateElementConfig( clt.updateElementConfig(
'hrp', smallToFull(robot.robotSimu.signal('state').value)) 'hrp', robot.smallToFull(robot.robotSimu.signal('state').value))
# Security: switch back to double support. # Security: switch back to double support.
quasiStaticWalking.moveCoM('origin') quasiStaticWalking.moveCoM('origin')
...@@ -274,7 +198,7 @@ for i in xrange(int(duration / timeStep)): ...@@ -274,7 +198,7 @@ for i in xrange(int(duration / timeStep)):
if clt: if clt:
clt.updateElementConfig( clt.updateElementConfig(
'hrp', smallToFull(robot.robotSimu.signal('state').value)) 'hrp', robot.smallToFull(robot.robotSimu.signal('state').value))
print "FINISHED" print "FINISHED"
#!/usr/bin/env python
# -*- coding: utf-8 -*-
# Copyright 2011, Florent Lamiraux, Thomas Moulard, JRL, CNRS/AIST
#
# This file is part of dynamic-graph.
# dynamic-graph is free software: you can redistribute it and/or
# modify it under the terms of the GNU Lesser General Public License
# as published by the Free Software Foundation, either version 3 of
# the License, or (at your option) any later version.
#
# dynamic-graph is distributed in the hope that it will be useful, but
# WITHOUT ANY WARRANTY; without even the implied warranty of
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
# General Lesser Public License for more details. You should have
# received a copy of the GNU Lesser General Public License along with
# dynamic-graph. If not, see <http://www.gnu.org/licenses/>.
from dynamic_graph.sot.core import FeaturePoint6d, FeatureGeneric, SOT
from dynamic_graph.sot.dynamics.humanoid_robot import HumanoidRobot
from dynamic_graph.sot.dynamics.hrp2 import Hrp2
from dynamic_graph import enableTrace, plug
from tools import *
# Move right wrist
sdes = toList(robot.dynamicRobot.signal('right-wrist').value)
sdes[0][3] += 0.25 # Move reference point forward.
sdes[2][3] += 0.1 # Increment reference point altitude.
robot.features['right-wrist'].reference.signal('position').value = toTuple(sdes)
# Select translation only.
robot.features['right-wrist'].feature.signal('selec').value = '000111'
robot.tasks['right-wrist'].signal('controlGain').value = 1.
# Push tasks
# Operational points tasks
solver.sot.push(robot.name + '.task.right-ankle')
solver.sot.push(robot.name + '.task.left-ankle')
solver.sot.push(robot.name + '.task.right-wrist')
# Center of mass
solver.sot.push(robot.name + '.task.com')
# Main.
# Main loop
for i in xrange(500):
robot.robotSimu.increment(timeStep)
if clt:
clt.updateElementConfig(
'hrp', robot.smallToFull(robot.robotSimu.signal('state').value))
print "FINISHED"
#!/usr/bin/env python
# -*- coding: utf-8 -*-
# Copyright 2011, Florent Lamiraux, Thomas Moulard, JRL, CNRS/AIST
#
# This file is part of dynamic-graph.
# dynamic-graph is free software: you can redistribute it and/or
# modify it under the terms of the GNU Lesser General Public License
# as published by the Free Software Foundation, either version 3 of
# the License, or (at your option) any later version.
#
# dynamic-graph is distributed in the hope that it will be useful, but
# WITHOUT ANY WARRANTY; without even the implied warranty of
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
# General Lesser Public License for more details. You should have
# received a copy of the GNU Lesser General Public License along with
# dynamic-graph. If not, see <http://www.gnu.org/licenses/>.
from optparse import OptionParser
from dynamic_graph import plug
from dynamic_graph.sot.core import SOT
from dynamic_graph.sot.dynamics.hrp2 import Hrp2
# Robotviewer is optional
hasRobotViewer = True
try:
import robotviewer
except ImportError:
hasRobotViewer = False
####################
# Helper functions #
####################
def toList(tupleOfTuple):
result = [[0, 0, 0, 0],
[0, 0, 0, 0],
[0, 0, 0, 0],
[0, 0, 0, 0]]
for i in xrange(4):
for j in xrange(4):
result[i][j] = tupleOfTuple[i][j]
return result
def toTuple(listOfList):
return ((listOfList[0][0], listOfList[0][1],
listOfList[0][2], listOfList[0][3]),
(listOfList[1][0], listOfList[1][1],
listOfList[1][2], listOfList[1][3]),
(listOfList[2][0], listOfList[2][1],
listOfList[2][2], listOfList[2][3]),
(listOfList[3][0], listOfList[3][1],
listOfList[3][2], listOfList[3][3]))
def displayHomogeneousMatrix(matrix):
"""
Display nicely a 4x4 matrix (usually homogeneous matrix).
"""
import itertools
matrix_tuple = tuple(itertools.chain.from_iterable(matrix))
formatStr = ''
for i in xrange(4*4):
formatStr += '{0[' + str(i) + ']: <10} '
if i != 0 and (i + 1) % 4 == 0:
formatStr += '\n'
print formatStr.format(matrix_tuple)
def initRobotViewer():
"""Initialize robotviewer is possible."""
clt = None
if hasRobotViewer:
try:
clt = robotviewer.client()
except:
print "failed to connect to robotviewer"
return clt
##################
# Helper classes #
##################
class Solver:
robot = None
sot = None
def __init__(self, robot):
self.robot = robot
self.sot = SOT('solver')
self.sot.signal('damping').value = 1e-6
self.sot.setNumberDofs(self.robot.dimension)
if robot.robotSimu:
plug(self.sot.signal('control'), robot.robotSimu.signal('control'))
plug(self.robot.robotSimu.signal('state'),
self.robot.dynamicRobot.signal('position'))
##################
# Initialization #
##################
# Parse options and enable robotviewer client if wanted.
clt = None
parser = OptionParser()
parser.add_option("-d", "--display",
action="store_true", dest="display", default=False,
help="enable display using robotviewer")
(options, args) = parser.parse_args()
if options.display:
if not hasRobotViewer:
print "Failed to import robotviewer client library."
clt = initRobotViewer()
if not clt:
print "Failed to initialize robotviewer client library."
# Initialize the stack of tasks.
robot = Hrp2("robot", True)
timeStep = .02
solver = Solver(robot)
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