diff --git a/CMakeLists.txt b/CMakeLists.txt index 16f5c66663b690195377d48b67aabfa93addeca6..7e132f852d1a29a78d86b225eb4a977fe3940e52 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -76,6 +76,7 @@ SEARCH_FOR_BOOST() ADD_SUBDIRECTORY(include) ADD_SUBDIRECTORY(src) ADD_SUBDIRECTORY(doc) +ADD_SUBDIRECTORY(python) ADD_SUBDIRECTORY(unitTesting) SETUP_PROJECT_FINALIZE() diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt new file mode 100644 index 0000000000000000000000000000000000000000..cd9354cc8ff277048258631e588d32e8cf69e88f --- /dev/null +++ b/python/CMakeLists.txt @@ -0,0 +1,8 @@ +INCLUDE(../cmake/python.cmake) +FINDPYTHON() + +INSTALL( + FILES kine_romeo.py + DESTINATION ${PYTHON_SITELIB}/dynamic_graph/tutorial +) + diff --git a/python/kine_romeo.py b/python/kine_romeo.py new file mode 100644 index 0000000000000000000000000000000000000000..401ca0fc76cadd545df7172ba7439b795a12585b --- /dev/null +++ b/python/kine_romeo.py @@ -0,0 +1,99 @@ +# -*- 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.matrix_util import matrixToTuple, vectorToTuple,rotate, matrixToRPY +from dynamic_graph.sot.core.meta_tasks_kine import * +from numpy import * + +# Create the robot romeo. +from dynamic_graph.sot.romeo.romeo import * +robot = Robot('romeo', device=RobotSimu('romeo')) + +plug(robot.device.state, robot.dynamic.position) + +# Binds with ROS. assert that roscore is running. +from dynamic_graph.ros import * +ros = Ros(robot) + +# Create a simple kinematic solver. +from dynamic_graph.sot.dynamics.solver import Solver +solver = Solver(robot) + +# Alternate visualization tool +from dynamic_graph.sot.core.utils.viewer_helper import addRobotViewer +addRobotViewer(robot.device,small=True,small_extra=24,verbose=False) + +#------------------------------------------------------------------------------- +#----- 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) + +runner=inc() +[go,stop,next,n]=loopShortcuts(runner) + +# ---- TASKS ------------------------------------------------------------------- +# ---- TASKS ------------------------------------------------------------------- +# ---- TASKS ------------------------------------------------------------------- + +# ---- TASK GRIP --- +# Defines a task for the right hand. +taskRH=MetaTaskKine6d('rh',robot.dynamic,'right-wrist','right-wrist') +# Move the operational point. +handMgrip=eye(4); handMgrip[0:3,3] = (0.1,0,0) +taskRH.opmodif = matrixToTuple(handMgrip) +taskRH.feature.frame('desired') +# robot.tasks['right-wrist'].add(taskRH.feature.name) + +# --- STATIC COM (if not walking) +robot.createCenterOfMassFeatureAndTask( + 'featureCom', 'featureComDef', 'comTask', + selec = '011', + gain = 10) + + +# --- CONTACTS +# define contactLF and contactRF +for name,joint in [ ['LF','left-ankle'], ['RF','right-ankle' ] ]: + contact = MetaTaskKine6d('contact'+name,robot.dynamic,name,joint) + contact.feature.frame('desired') + contact.gain.setConstant(10) + contact.keep() + locals()['contact'+name] = contact + +# --- RUN ---------------------------------------------------------------------- + +# Move the desired position of the right hand +# 1st param: task concerned +# 2st param: objective +# 3rd param: selected parameters +# 4th param: gain +target=(0.5,-0.2,0.8) +gotoNd(taskRH,target,'111',(4.9,0.9,0.01,0.9)) + +# Fill the stack with some tasks. +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() +