Commit 8490f9de authored by Nicolas Mansard's avatar Nicolas Mansard Committed by Nicolas Mansard
Browse files

Update to new API.

Conflicts:

	unitTesting/python/dynamic_walk.py
	unitTesting/python/feet_follower.py
parent 439ad4c4
...@@ -2,6 +2,6 @@ This package was written by and with the assistance of: ...@@ -2,6 +2,6 @@ This package was written by and with the assistance of:
* Francois Bleibel fbleibel@gmail.com * Francois Bleibel fbleibel@gmail.com
* François Keith francois.keith@aist.go.jp * François Keith francois.keith@aist.go.jp
* Nicolas Mansard FIXME * Nicolas Mansard nicolas.mansard@laas.fr
* Olivier Stasse olivier.stasse@aist.go.jp * Olivier Stasse olivier.stasse@aist.go.jp
* Thomas Moulard thomas.moulard@gmail.com * Thomas Moulard thomas.moulard@gmail.com
#!/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 math import pi
from dynamic_graph.sot.core import FeaturePoint6d, FeatureGeneric, SOT
from dynamic_graph.sot.se3 import SE3
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 *
class HalfStep:
startX = 0.
startY = 0.
# startZ is always 0.
startTheta = 0.
# finalX is always 0.
finalY = 0.
finalZ = 0.
# finalTheta is always 0.
class DynamicWalking:
leftFoot = 0
rightFoot = 1
footAltitude = 0.1
robot = None
supportFoot = None
swingFoot = None
def __init__(self, robot):
self.robot = robot
self.supportFoot = self.leftFoot
self.swingFoot = HalfStep()
self.swingFoot.startX = -0.1
self.swingFoot.startY = 0.1
self.swingFoot.startTheta = pi / 4.
self.swingFoot.finalY = 0.1
self.swingFoot.finalZ = 0.
def computeFootPosition(op, swingFoot):
# Support foot position in the waist frame.
supportFootPos = self.robot.dynamicRobot.signal(op).value
# Swing foot in the support foot: swingFoot
# Swing foot in the waist frame.
return SE3(supportFootPos) * SE3(swingFoot)
res = \
SE3(robot.features['right-ankle'].reference.position.value) * \
SE3((0., 0., 0., -0.1),
(0., 0., 0., 0.1),
(0., 0., 0., 0.),
(0., 0., 0., 1.))
print res
# Push tasks
# Feet tasks.
solver.sot.push(robot.name + '.task.right-ankle')
solver.sot.push(robot.name + '.task.left-ankle')
# Center of mass
# FIXME: trigger segv at exit.
solver.sot.push(robot.name + '.task.com')
dynamicWalking = DynamicWalking(robot)
for i in xrange(100):
robot.simu.increment(timeStep)
if clt:
clt.updateElementConfig(
'hrp', robot.smallToFull(robot.simu.state.value))
#!/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 tools import *
from dynamic_graph.sot.dynamics.feet_follower import FeetFollowerFromFile
feetFollower = FeetFollowerFromFile('feet-follower')
feetFollower.feetToAnkleLeft = robot.dynamic.getAnklePositionInFootFrame()
feetFollower.feetToAnkleRight = robot.dynamic.getAnklePositionInFootFrame()
plug(feetFollower.signal('com'), robot.featureComDes.signal('errorIN'))
plug(feetFollower.signal('left-ankle'),
robot.features['left-ankle'].reference)
plug(feetFollower.signal('right-ankle'),
robot.features['right-ankle'].reference)
robot.comTask.signal('controlGain').value = 50.
robot.tasks['left-ankle'].signal('controlGain').value = 50.
robot.tasks['right-ankle'].signal('controlGain').value = 50.
# Push tasks
# Operational points tasks
solver.sot.push(robot.name + '.task.right-ankle')
solver.sot.push(robot.name + '.task.left-ankle')
# Center of mass
solver.sot.push(robot.name + '.task.com')
# Main.
# Main loop
for i in xrange(500):
robot.simu.increment(timeStep)
if clt:
clt.updateElementConfig(
'hrp', robot.smallToFull(robot.simu.state.value))
finalPosition = (
-0.015361, -0.0049075500000000001, -0.00047065200000000001, -0.0172946,
-0.020661800000000001, 0.0374547, -0.037641599999999997,
0.025434399999999999, -0.45398100000000002, 0.86741800000000002,
-0.39213799999999999, -0.0089269499999999995, -0.037646100000000002,
0.025648199999999999, -0.46715499999999999, 0.87717599999999996,
-0.38872200000000001, -0.0091408199999999992, 0.080488199999999996,
-0.18355399999999999, -0.00036695100000000002, -0.0056776600000000002,
-0.12173299999999999, -0.23972599999999999, -0.00637303,
-0.56908000000000003, 0.00296262, 0.19108900000000001, 0.100088,
0.23896800000000001, 0.21485599999999999, -0.18973400000000001,
-0.49457699999999999, 0.040646799999999997, 0.16970299999999999, 0.100067)
checkFinalConfiguration(robot.simu.state.value, finalPosition)
print "Exiting."
...@@ -57,14 +57,14 @@ class QuasiStaticWalking: ...@@ -57,14 +57,14 @@ class QuasiStaticWalking:
self.nextStateSwitch = 0 # Next switch is now! self.nextStateSwitch = 0 # Next switch is now!
self.initialFootPose['left-ankle'] = \ self.initialFootPose['left-ankle'] = \
self.robot.dynamicRobot.signal('left-ankle').value self.robot.dynamic.signal('left-ankle').value
self.initialFootPose['right-ankle'] = \ self.initialFootPose['right-ankle'] = \
self.robot.dynamicRobot.signal('right-ankle').value self.robot.dynamic.signal('right-ankle').value
self.t = None # Will be updated through the update method. self.t = None # Will be updated through the update method.
self.robot.tasks['left-ankle'].signal('controlGain').value = 1. self.robot.tasks['left-ankle'].controlGain.value = 1.
self.robot.tasks['right-ankle'].signal('controlGain').value = 1. self.robot.tasks['right-ankle'].controlGain.value = 1.
# Move CoM to a particular operational point (usually left or right ankle). # Move CoM to a particular operational point (usually left or right ankle).
...@@ -77,19 +77,19 @@ class QuasiStaticWalking: ...@@ -77,19 +77,19 @@ class QuasiStaticWalking:
x = 0. x = 0.
y = 0. y = 0.
else: else:
x = robot.dynamicRobot.signal(op).value[0][3] x = robot.dynamic.signal(op).value[0][3]
y = robot.dynamicRobot.signal(op).value[1][3] y = robot.dynamic.signal(op).value[1][3]
z = robot.featureComDes.signal('errorIN').value[2] z = robot.featureComDes.errorIN.value[2]
self.robot.featureComDes.signal('errorIN').value = (x, y, z) self.robot.featureComDes.errorIN.value = (x, y, z)
def liftFoot(self, op): def liftFoot(self, op):
sdes = toList(robot.dynamicRobot.signal(op).value) sdes = toList(robot.dynamic.signal(op).value)
sdes[2][3] += self.footAltitude # Increment altitude. sdes[2][3] += self.footAltitude # Increment altitude.
robot.features[op].reference.signal('position').value = toTuple(sdes) robot.features[op].reference.value = toTuple(sdes)
def landFoot(self, op): def landFoot(self, op):
robot.features[op].reference.signal('position').value = \ robot.features[op].reference.value = \
self.initialFootPose[op] self.initialFootPose[op]
def supportFootStr(self): def supportFootStr(self):
...@@ -180,13 +180,13 @@ totalSteps = int((stepTime / timeStep) * steps) ...@@ -180,13 +180,13 @@ totalSteps = int((stepTime / timeStep) * steps)
t = 0 t = 0
for i in xrange(totalSteps + 1): for i in xrange(totalSteps + 1):
t += timeStep t += timeStep
robot.robotSimu.increment(timeStep) robot.simu.increment(timeStep)
quasiStaticWalking.update(t) quasiStaticWalking.update(t)
if clt: if clt:
clt.updateElementConfig( clt.updateElementConfig(
'hrp', robot.smallToFull(robot.robotSimu.signal('state').value)) 'hrp', robot.smallToFull(robot.simu.state.value))
# Security: switch back to double support. # Security: switch back to double support.
quasiStaticWalking.moveCoM('origin') quasiStaticWalking.moveCoM('origin')
...@@ -194,11 +194,11 @@ duration = quasiStaticWalking.time[quasiStaticWalking.stateCoM_singleToDouble] ...@@ -194,11 +194,11 @@ duration = quasiStaticWalking.time[quasiStaticWalking.stateCoM_singleToDouble]
for i in xrange(int(duration / timeStep)): for i in xrange(int(duration / timeStep)):
t += timeStep t += timeStep
robot.robotSimu.increment(timeStep) robot.simu.increment(timeStep)
if clt: if clt:
clt.updateElementConfig( clt.updateElementConfig(
'hrp', robot.smallToFull(robot.robotSimu.signal('state').value)) 'hrp', robot.smallToFull(robot.simu.state.value))
finalPosition = ( finalPosition = (
-0.0082169200000000008, -0.0126068, -0.00022860999999999999, -0.0082169200000000008, -0.0126068, -0.00022860999999999999,
...@@ -213,5 +213,5 @@ finalPosition = ( ...@@ -213,5 +213,5 @@ finalPosition = (
0.26377400000000001, 0.171155, -0.00065098499999999998, 0.26377400000000001, 0.171155, -0.00065098499999999998,
-0.52324700000000002, -1.23291e-05, 6.0469500000000001e-05, 0.100009) -0.52324700000000002, -1.23291e-05, 6.0469500000000001e-05, 0.100009)
checkFinalConfiguration(robot.robotSimu.signal('state').value, finalPosition) checkFinalConfiguration(robot.simu.state.value, finalPosition)
print "Exiting." print "Exiting."
...@@ -35,11 +35,11 @@ solver.sot.push(robot.name + '.task.com') ...@@ -35,11 +35,11 @@ solver.sot.push(robot.name + '.task.com')
# Main. # Main.
# Main loop # Main loop
for i in xrange(500): for i in xrange(500):
robot.robotSimu.increment(timeStep) robot.simu.increment(timeStep)
if clt: if clt:
clt.updateElementConfig( clt.updateElementConfig(
'hrp', robot.smallToFull(robot.robotSimu.signal('state').value)) 'hrp', robot.smallToFull(robot.simu.state.value))
finalPosition = ( finalPosition = (
-0.0357296, -0.0024092699999999998, 0.033870600000000001, -0.0357296, -0.0024092699999999998, 0.033870600000000001,
...@@ -55,5 +55,5 @@ finalPosition = ( ...@@ -55,5 +55,5 @@ finalPosition = (
-0.109959, -0.60156299999999996, -0.082422700000000002, -0.109959, -0.60156299999999996, -0.082422700000000002,
1.0207200000000001, 0.10037500000000001) 1.0207200000000001, 0.10037500000000001)
checkFinalConfiguration(robot.robotSimu.signal('state').value, finalPosition) checkFinalConfiguration(robot.simu.state.value, finalPosition)
print "Exiting." print "Exiting."
...@@ -34,11 +34,11 @@ solver.sot.push(robot.name + '.task.com') ...@@ -34,11 +34,11 @@ solver.sot.push(robot.name + '.task.com')
# Main. # Main.
# Main loop # Main loop
for i in xrange(500): for i in xrange(500):
robot.robotSimu.increment(timeStep) robot.simu.increment(timeStep)
if clt: if clt:
clt.updateElementConfig( clt.updateElementConfig(
'hrp', robot.smallToFull(robot.robotSimu.signal('state').value)) 'hrp', robot.smallToFull(robot.simu.state.value))
finalPosition = ( finalPosition = (
-0.015183500000000001, -0.00037148200000000002, -0.00065935600000000005, -0.015183500000000001, -0.00037148200000000002, -0.00065935600000000005,
...@@ -53,5 +53,5 @@ finalPosition = ( ...@@ -53,5 +53,5 @@ finalPosition = (
0.170987, 0.100064, -0.117492, 0.24870200000000001, 0.016264399999999998, 0.170987, 0.100064, -0.117492, 0.24870200000000001, 0.016264399999999998,
-0.56795700000000005, 0.0040012399999999997, 0.18956200000000001, 0.100089) -0.56795700000000005, 0.0040012399999999997, 0.18956200000000001, 0.100089)
checkFinalConfiguration(robot.robotSimu.signal('state').value, finalPosition) checkFinalConfiguration(robot.simu.state.value, finalPosition)
print "Exiting." print "Exiting."
...@@ -33,11 +33,11 @@ solver.sot.push(robot.name + '.task.com') ...@@ -33,11 +33,11 @@ solver.sot.push(robot.name + '.task.com')
# Main. # Main.
# Main loop # Main loop
for i in xrange(500): for i in xrange(500):
robot.robotSimu.increment(timeStep) robot.simu.increment(timeStep)
if clt: if clt:
clt.updateElementConfig( clt.updateElementConfig(
'hrp', robot.smallToFull(robot.robotSimu.signal('state').value)) 'hrp', robot.smallToFull(robot.simu.state.value))
finalPosition = ( finalPosition = (
...@@ -53,5 +53,5 @@ finalPosition = ( ...@@ -53,5 +53,5 @@ finalPosition = (
0.23896800000000001, 0.21485599999999999, -0.18973400000000001, 0.23896800000000001, 0.21485599999999999, -0.18973400000000001,
-0.49457699999999999, 0.040646799999999997, 0.16970299999999999, 0.100067) -0.49457699999999999, 0.040646799999999997, 0.16970299999999999, 0.100067)
checkFinalConfiguration(robot.robotSimu.signal('state').value, finalPosition) checkFinalConfiguration(robot.simu.state.value, finalPosition)
print "Exiting." print "Exiting."
...@@ -114,11 +114,11 @@ def initRobotViewer(): ...@@ -114,11 +114,11 @@ def initRobotViewer():
return clt return clt
def reach(robot, op, tx, ty, tz): def reach(robot, op, tx, ty, tz):
sdes = toList(robot.dynamicRobot.signal(op).value) sdes = toList(robot.dynamic.signal(op).value)
sdes[0][3] += tx sdes[0][3] += tx
sdes[1][3] += ty sdes[1][3] += ty
sdes[2][3] += tz sdes[2][3] += tz
robot.features[op].reference.signal('position').value = toTuple(sdes) robot.features[op].reference.value = toTuple(sdes)
# Select translation only. # Select translation only.
robot.features[op].feature.signal('selec').value = '000111' robot.features[op].feature.signal('selec').value = '000111'
robot.tasks[op].signal('controlGain').value = 1. robot.tasks[op].signal('controlGain').value = 1.
...@@ -154,10 +154,10 @@ class Solver: ...@@ -154,10 +154,10 @@ class Solver:
self.sot.signal('damping').value = 1e-6 self.sot.signal('damping').value = 1e-6
self.sot.setNumberDofs(self.robot.dimension) self.sot.setNumberDofs(self.robot.dimension)
if robot.robotSimu: if robot.simu:
plug(self.sot.signal('control'), robot.robotSimu.signal('control')) plug(self.sot.signal('control'), robot.simu.signal('control'))
plug(self.robot.robotSimu.signal('state'), plug(self.robot.simu.state,
self.robot.dynamicRobot.signal('position')) self.robot.dynamic.position)
################## ##################
...@@ -182,5 +182,5 @@ if options.display: ...@@ -182,5 +182,5 @@ if options.display:
# Initialize the stack of tasks. # Initialize the stack of tasks.
robot = Hrp2("robot", True) robot = Hrp2("robot", True)
timeStep = .02 timeStep = .005
solver = Solver(robot) 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