Commit 1e3d6469 authored by Thomas Moulard's avatar Thomas Moulard
Browse files

Factorize initialization in tools, make display optional.

parent 6edb1c23
......@@ -57,6 +57,9 @@ class Hrp2(AbstractHumanoidRobot):
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,
simu,
......
......@@ -204,6 +204,17 @@ class AbstractHumanoidRobot (object):
else:
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):
......
......@@ -21,57 +21,9 @@ from dynamic_graph.sot.dynamics.hrp2 import Hrp2
from dynamic_graph import enableTrace, plug
# Robotviewer is optional
enableRobotViewer = True
try:
import robotviewer
except ImportError:
enableRobotViewer = False
from tools import *
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:
leftFoot = 0
rightFoot = 1
......@@ -200,34 +152,6 @@ class QuasiStaticWalking:
# Trigger actions to move to next 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
# Feet tasks.
......@@ -262,7 +186,7 @@ for i in xrange(totalSteps + 1):
if clt:
clt.updateElementConfig(
'hrp', smallToFull(robot.robotSimu.signal('state').value))
'hrp', robot.smallToFull(robot.robotSimu.signal('state').value))
# Security: switch back to double support.
quasiStaticWalking.moveCoM('origin')
......@@ -274,7 +198,7 @@ for i in xrange(int(duration / timeStep)):
if clt:
clt.updateElementConfig(
'hrp', smallToFull(robot.robotSimu.signal('state').value))
'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 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)
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