Commit 703a31d3 authored by Francois Keith's avatar Francois Keith
Browse files

Use the Macro solver in ros scripts.

parent ed38f761
......@@ -14,6 +14,10 @@
# 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.matrix_util import matrixToTuple, vectorToTuple,rotate, matrixToRPY
from dynamic_graph.sot.core.meta_tasks_kine import *
from numpy import *
from dynamic_graph.sot.romeo.romeo import *
robot = Robot('robot')
plug(robot.device.state, robot.dynamic.position)
......@@ -23,34 +27,15 @@ ros = Ros(robot)
# Create a solver.
# from dynamic_graph.sot.dynamics.solver import Solver
# solver = Solver(robot)
from dynamic_graph.sot.core.matrix_util import matrixToTuple, vectorToTuple,rotate, matrixToRPY
from dynamic_graph.sot.core.meta_tasks_kine import *
from numpy import *
# --- ROBOT SIMU ---------------------------------------------------------------
# robotName = 'romeo'
# robotDim=robotDimension[robotName]
# robot = RobotSimu("romeo")
# robot.resize(robotDim)
dt=5e-3
# robot.set( initialConfig[robotName] )
# addRobotViewer(robot,small=True,small_extra=24,verbose=False)
# --- ROMEO HANDS ---------------------------------------------------------------
# robot.gripper=0.0
# RobotClass = RobotSimu
# RobotClass.stateFullSizeOld = RobotClass.stateFullSize
# RobotClass.stateFullSize = lambda x: [float(v) for v in x.state.value+24*(x.gripper,)]
from dynamic_graph.sot.dynamics.solver import Solver
solver = Solver(robot)
#-------------------------------------------------------------------------------
#----- MAIN LOOP ---------------------------------------------------------------
#-------------------------------------------------------------------------------
# define the macro allowing to run the simulation.
from dynamic_graph.sot.core.utils.thread_interruptible_loop import loopInThread,loopShortcuts
dt=5e-3
@loopInThread
def inc():
robot.device.increment(dt)
......@@ -58,14 +43,6 @@ def inc():
runner=inc()
[go,stop,next,n]=loopShortcuts(runner)
# ---- SOT ---------------------------------------------------------------------
# ---- SOT ---------------------------------------------------------------------
# ---- SOT ---------------------------------------------------------------------
sot = SOT('sot')
sot.setNumberDofs(robot.dimension)
plug(sot.control,robot.device.control)
# ---- TASKS -------------------------------------------------------------------
# ---- TASKS -------------------------------------------------------------------
# ---- TASKS -------------------------------------------------------------------
......@@ -79,11 +56,6 @@ taskRH.feature.frame('desired')
# robot.tasks['right-wrist'].add(taskRH.feature.name)
# --- STATIC COM (if not walking)
# taskCom = MetaTaskKineCom(robot.dynamic)
# robot.dynamic.com.recompute(0)
# taskCom.featureDes.errorIN.value = robot.dynamic.com.value
# taskCom.task.controlGain.value = 10
robot.createCenterOfMassFeatureAndTask(
'featureCom', 'featureComDef', 'comTask',
selec = '011',
......@@ -104,13 +76,11 @@ for name,joint in [ ['LF','left-ankle'], ['RF','right-ankle' ] ]:
target=(0.5,-0.2,0.8)
gotoNd(taskRH,target,'111',(4.9,0.9,0.01,0.9))
# sot.push(taskCom.task.name)
# sot.push(robot.tasks['right-wrist'].name)
sot.push(contactRF.task.name)
sot.push(contactLF.task.name)
sot.push((robot.comTask.name))
sot.push(taskRH.task.name)
solver.push(contactRF.task)
solver.push(contactLF.task)
solver.push(robot.comTask)
solver.push(taskRH.task)
# type inc to run one iteration, or go to run indefinitely.
# go()
......@@ -12,32 +12,30 @@ from dynamic_graph.sot.core.meta_tasks_kine import *
from dynamic_graph.sot.core.utils.thread_interruptible_loop import loopInThread,loopShortcuts
from dynamic_graph.sot.dyninv import SolverKine
# Create the robot.
from dynamic_graph.sot.romeo.romeo import *
robot = Robot('robot')
plug(robot.device.state, robot.dynamic.position)
# Binds with ros, export joint_state.
from dynamic_graph.ros import *
ros = Ros(robot)
# Create a solver.
# from dynamic_graph.sot.dynamics.solver import Solver
# solver = Solver(robot)
from dynamic_graph.sot.dynamics.solver import Solver
solver = Solver(robot)
from dynamic_graph.sot.core.matrix_util import matrixToTuple, vectorToTuple,rotate, matrixToRPY
from dynamic_graph.sot.core.meta_tasks_kine import *
from numpy import *
# --- ROBOT SIMU ---------------------------------------------------------------
dt=5e-3
#-------------------------------------------------------------------------------
#----- MAIN LOOP ---------------------------------------------------------------
#-------------------------------------------------------------------------------
dt=5e-3
from dynamic_graph.sot.core.utils.thread_interruptible_loop import loopInThread,loopShortcuts
@loopInThread
def inc():
......@@ -51,12 +49,6 @@ from dynamic_graph.sot.pattern_generator.meta_pg import MetaPG
pg = MetaPG(robot.dynamic)
pg.plugZmp(robot.device)
# ---- SOT ---------------------------------------------------------------------
# The basic SOT solver would work too.
sot = SolverKine('sot')
sot.setSize(robot.dimension)
plug(sot.control,robot.device.control)
# ---- TASKS -------------------------------------------------------------------
# ---- WAIST TASK ---
taskWaist=MetaTask6d('waist',robot.dynamic,'waist','waist')
......@@ -78,13 +70,22 @@ taskLF=MetaTask6d('lf',robot.dynamic,'lf','left-ankle')
plug(pg.pg.leftfootref,taskLF.featureDes.position)
taskLF.task.controlGain.value = 40
# ---- WAIST TASK ORIENTATION ---
# set the orientation of the waist to be the same as the one of the foot.
taskWaistOr=MetaTask6d('waistOr',robot.dynamic,'waist','waist')
plug(pg.pg.rightfootref,taskWaistOr.featureDes.position)
taskWaistOr.task.controlGain.value = 40
taskWaistOr.feature.selec.value = '100000'
# --- RUN ----------------------------------------------------------------------
# --- RUN ----------------------------------------------------------------------
# --- RUN ----------------------------------------------------------------------
sot.push(taskWaist.task.name)
sot.push(taskRF.task.name)
sot.push(taskLF.task.name)
sot.push(taskCom.task.name)
solver.push(taskWaist.task)
solver.push(taskRF.task)
solver.push(taskLF.task)
solver.push(taskCom.task)
solver.push(taskWaistOr.task)
# --- HERDT PG AND START -------------------------------------------------------
# Set the algorithm generating the ZMP reference trajectory to Herdt's one.
......@@ -92,6 +93,6 @@ pg.startHerdt(False)
# You can now modifiy the speed of the robot using set pg.pg.velocitydes [3]( x, y, yaw)
pg.pg.velocitydes.value =(0.1,0.0,0.0)
go()
# go()
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