...
 
Commits (22)
# Copyright 2016, Thomas Moulard, Olivier Stasse, JRL, CNRS/AIST
# Copyright 2016-2020, Thomas Moulard, Olivier Stasse, Guilhem Saurel, JRL, CNRS/AIST, LAAS-CNRS
#
CMAKE_MINIMUM_REQUIRED(VERSION 2.8)
CMAKE_MINIMUM_REQUIRED(VERSION 3.1)
SET(PROJECT_NAMESPACE stack-of-tasks)
# Project properties
SET(PROJECT_ORG stack-of-tasks)
SET(PROJECT_NAME sot-talos)
SET(PROJECT_DESCRIPTION "dynamic-graph package for Talos robot")
SET(PROJECT_URL "http://github.com/${PROJECT_NAMESPACE}/${PROJECT_NAME}")
SET(PROJECT_URL "https://github.com/${PROJECT_ORG}/${PROJECT_NAME}")
# Project options
OPTION(SUFFIX_SO_VERSION "Suffix library name with its version" ON)
# Project configuration
SET(PROJECT_USE_CMAKE_EXPORT TRUE)
SET(CUSTOM_HEADER_DIR "sot/talos")
SET(CXX_DISABLE_WERROR TRUE)
INCLUDE(cmake/base.cmake)
INCLUDE(cmake/boost.cmake)
INCLUDE(cmake/eigen.cmake)
INCLUDE(cmake/lapack.cmake)
INCLUDE(cmake/ros.cmake)
INCLUDE(cmake/python.cmake)
INCLUDE(cmake/ros.cmake)
SET(CUSTOM_HEADER_DIR "${PROJECT_NAME}")
SET(CXX_DISABLE_WERROR True)
SET(PKG_CONFIG_ADDITIONAL_VARIABLES
${PKG_CONFIG_ADDITIONAL_VARIABLES}
plugindirname
plugindir
)
CMAKE_POLICY(SET CMP0048 OLD)
PROJECT(${PROJECT_NAME} CXX)
# Search for dependencies.
ADD_REQUIRED_DEPENDENCY("pinocchio >= 2.0.0")
ADD_REQUIRED_DEPENDENCY("dynamic-graph >= 3.0.0")
ADD_REQUIRED_DEPENDENCY("sot-core >= 4.0.0")
ADD_REQUIRED_DEPENDENCY("sot-dynamic-pinocchio >= 3.1")
ADD_REQUIRED_DEPENDENCY("dynamic-graph-python >= 3.0.0")
ADD_REQUIRED_DEPENDENCY("dynamic_graph_bridge_msgs")
ADD_REQUIRED_DEPENDENCY("dynamic_graph_bridge >= 3.0")
# Project definition
COMPUTE_PROJECT_ARGS(PROJECT_ARGS LANGUAGES CXX C)
PROJECT(${PROJECT_NAME} ${PROJECT_ARGS})
ADD_OPTIONAL_DEPENDENCY("talos_data")
ADD_OPTIONAL_DEPENDENCY("pyrene-motions")
# Project dependencies
SET(BOOST_COMPONENTS filesystem system thread program_options
unit_test_framework python)
SET(CATKIN_REQUIRED_COMPONENTS roscpp std_msgs)
SET(CATKIN_DEPENDS_LIBRARIES ros_bridge sot_loader)
# Search for dependencies.
# Boost
SET(BOOST_COMPONENTS filesystem system thread)
SEARCH_FOR_BOOST()
SEARCH_FOR_EIGEN()
ADD_PROJECT_DEPENDENCY(sot-dynamic-pinocchio REQUIRED)
ADD_PROJECT_DEPENDENCY(dynamic-graph-python REQUIRED)
ADD_PROJECT_DEPENDENCY(dynamic_graph_bridge REQUIRED)
ADD_PROJECT_DEPENDENCY(roscpp REQUIRED)
ADD_PROJECT_DEPENDENCY(talos_data REQUIRED)
ADD_PROJECT_DEPENDENCY(pyrene-motions REQUIRED)
ADD_REQUIRED_DEPENDENCY(dynamic_graph_bridge_msgs)
FINDPYTHON()
INCLUDE_DIRECTORIES(SYSTEM ${PYTHON_INCLUDE_PATH})
# Handle rpath necessary to handle ROS multiplace packages
# libraries inclusion
SET(CMAKE_INSTALL_LIBDIR lib)
SET(CMAKE_SKIP_BUILD_RPATH FALSE)
SET(CMAKE_BUILD_WITH_INSTALL_RPATH FALSE)
SET(CMAKE_INSTALL_RPATH "${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_LIBDIR}")
SET(CMAKE_INSTALL_RPATH_USE_LINK_PATH TRUE)
SEARCH_FOR_BOOST()
FIND_PACKAGE(catkin REQUIRED COMPONENTS ${CATKIN_REQUIRED_COMPONENTS})
# Add subdirectories.
ADD_SUBDIRECTORY(src)
IF(TALOS_DATA_FOUND)
FOREACH(py_filename kinetalos katana_holding )
CONFIGURE_FILE(
${PROJECT_SOURCE_DIR}/tests/${py_filename}.py.cmake
${PROJECT_BINARY_DIR}/tests/${py_filename}.py
)
INSTALL(FILES
${PROJECT_BINARY_DIR}/tests/${py_filename}.py
DESTINATION
${CMAKE_INSTALL_PREFIX}/share/${PROJECT_NAME}/tests
)
ENDFOREACH(py_filename)
IF(PYRENE_MOTIONS_FOUND)
# Configure files using motions from pyrene-motions package
FOREACH(py_filename appli-test-simple-seq-play )
CONFIGURE_FILE(
${PROJECT_SOURCE_DIR}/tests/${py_filename}.py.cmake
${PROJECT_BINARY_DIR}/tests/${py_filename}.py
)
INSTALL(FILES
${PROJECT_BINARY_DIR}/tests/${py_filename}.py
DESTINATION
${CMAKE_INSTALL_PREFIX}/share/${PROJECT_NAME}/tests
)
ENDFOREACH(py_filename)
# Install python files starting the application
FOREACH(py_filename test-simple-seq-play )
INSTALL(FILES
${PROJECT_SOURCE_DIR}/tests/${py_filename}.py
DESTINATION
${CMAKE_INSTALL_PREFIX}/share/${PROJECT_NAME}/tests
)
ENDFOREACH(py_filename)
ENDIF(PYRENE_MOTIONS_FOUND)
FOREACH(py_filename kinetalos katana_holding)
CONFIGURE_FILE(
${PROJECT_SOURCE_DIR}/tests/${py_filename}.py.cmake
${PROJECT_BINARY_DIR}/tests/${py_filename}.py)
INSTALL(FILES
${PROJECT_SOURCE_DIR}/tests/test.py
${PROJECT_SOURCE_DIR}/tests/appli.py
DESTINATION
${CMAKE_INSTALL_PREFIX}/share/${PROJECT_NAME}/tests
)
ENDIF(TALOS_DATA_FOUND)
${PROJECT_BINARY_DIR}/tests/${py_filename}.py
DESTINATION share/${PROJECT_NAME}/tests)
ENDFOREACH(py_filename)
# Configure files using motions from pyrene-motions package
FOREACH(py_filename appli-test-simple-seq-play)
CONFIGURE_FILE(
${PROJECT_SOURCE_DIR}/tests/${py_filename}.py.cmake
${PROJECT_BINARY_DIR}/tests/${py_filename}.py)
INSTALL(FILES
${PROJECT_BINARY_DIR}/tests/${py_filename}.py
DESTINATION share/${PROJECT_NAME}/tests)
ENDFOREACH(py_filename)
# Install python files starting the application
FOREACH(py_filename test-simple-seq-play)
INSTALL(FILES
${PROJECT_SOURCE_DIR}/tests/${py_filename}.py
DESTINATION share/${PROJECT_NAME}/tests)
ENDFOREACH(py_filename)
INSTALL(FILES
${PROJECT_SOURCE_DIR}/tests/test.py
${PROJECT_SOURCE_DIR}/tests/appli.py
DESTINATION share/${PROJECT_NAME}/tests)
IF(NOT INSTALL_PYTHON_INTERFACE_ONLY)
INSTALL(FILES package.xml DESTINATION share/${PROJECT_NAME})
ENDIF(NOT INSTALL_PYTHON_INTERFACE_ONLY)
Subproject commit 046c3be5553c4ea340eb672d0289627f0c07b1a4
Subproject commit 321eb1ccf1d94570eb564f3659b13ef3ef82239e
<package format="2">
<name>sot-talos</name>
<version>1.1.1</version>
<description>
Stack of Tasks for Talos.
</description>
<maintainer email="ostasse@laas.fr">Olivier Stasse</maintainer>
<license>BSDv2</license>
<url>http://github.com/stack-of-tasks/sot-talos</url>
<author>Olivier Stasse</author>
<build_depend>roscpp</build_depend>
<build_depend>sot-dynamic-pinocchio</build_depend>
<build_depend>talos_data</build_depend>
<build_depend>pyrene-motions</build_depend>
<build_depend>dynamic_graph_bridge</build_depend>
<build_depend>dynamic_graph_bridge_msgs</build_depend>
<exec_depend>roscpp</exec_depend>
<exec_depend>sot-dynamic-pinocchio</exec_depend>
<exec_depend>talos_data</exec_depend>
<exec_depend>pyrene-motions</exec_depend>
<exec_depend>dynamic_graph_bridge</exec_depend>
<exec_depend>dynamic_graph_bridge_msgs</exec_depend>
<buildtool_depend>catkin</buildtool_depend>
<doc_depend>doxygen</doc_depend>
</package>
# Copyright 2016, R. Budhiraja, Olivier Stasse, CNRS
FUNCTION(COMPILE_PLUGIN NAME SOURCES ENTITIES)
ADD_LIBRARY(${NAME} SHARED ${SOURCES})
SET_TARGET_PROPERTIES(${lib} PROPERTIES
PREFIX ""
SOVERSION ${PROJECT_VERSION})
PKG_CONFIG_USE_DEPENDENCY(${NAME} dynamic-graph)
PKG_CONFIG_USE_DEPENDENCY(${NAME} sot-core)
PKG_CONFIG_USE_DEPENDENCY(${NAME} eigen3)
PKG_CONFIG_USE_DEPENDENCY(${NAME} pinocchio)
INSTALL(TARGETS ${NAME} DESTINATION ${CMAKE_INSTALL_LIBDIR}/plugin)
# build python submodule
STRING(REPLACE - _ PYTHON_LIBRARY_NAME ${NAME})
SET(NEW_ENTITY_CLASS ${ENTITIES})
DYNAMIC_GRAPH_PYTHON_MODULE("sot/talos/${PYTHON_LIBRARY_NAME}"
${NAME}
sot-talos-${PYTHON_LIBRARY_NAME}-wrap
)
ENDFUNCTION()
# Copyright 2016, 2020, R. Budhiraja, Olivier Stasse, Guilhem SaurelCNRS
# Install Python files.
SET(PYTHON_MODULE_DIR
${CMAKE_CURRENT_SOURCE_DIR}/dynamic_graph/sot/talos)
SET(PYTHON_MODULE_BUILD_DIR
${CMAKE_CURRENT_BINARY_DIR}/dynamic_graph/sot/talos)
SET(PYTHON_MODULE dynamic_graph/sot/talos)
PYTHON_INSTALL_ON_SITE("${PYTHON_MODULE}" "__init__.py" )
PYTHON_INSTALL_ON_SITE("${PYTHON_MODULE}" "talos.py" )
PYTHON_INSTALL_ON_SITE("${PYTHON_MODULE}" "__init__.py")
PYTHON_INSTALL_ON_SITE("${PYTHON_MODULE}" "talos.py")
SET(FILES __init__.py robot.py)
# Install dynamic_graph.sot.pyrene
SET(PYTHON_MODULE dynamic_graph/sot/pyrene)
PYTHON_INSTALL_ON_SITE("${PYTHON_MODULE}" "__init__.py" )
PYTHON_INSTALL_ON_SITE("${PYTHON_MODULE}" "robot.py" "${PYTHON_SITELIB}")
PYTHON_INSTALL_ON_SITE("${PYTHON_MODULE}" "__init__.py")
PYTHON_INSTALL_ON_SITE("${PYTHON_MODULE}" "prologue.py")
PYTHON_INSTALL_ON_SITE("${PYTHON_MODULE}" "robot.py")
# Add the library to wrap the device of Talos.
MACRO(build_talos_device)
SET(DEVICE_NAME sot-talos-device)
ADD_LIBRARY(${DEVICE_NAME}
SHARED
sot-talos-device.cpp
)
# Link the dynamic library containing the SoT with its dependencies.
PKG_CONFIG_USE_DEPENDENCY(${DEVICE_NAME} "dynamic-graph")
PKG_CONFIG_USE_DEPENDENCY(${DEVICE_NAME} "dynamic-graph-python")
PKG_CONFIG_USE_DEPENDENCY(${DEVICE_NAME} "sot-core")
PKG_CONFIG_USE_DEPENDENCY(${DEVICE_NAME} "pinocchio")
PKG_CONFIG_USE_DEPENDENCY(${DEVICE_NAME} "dynamic_graph_bridge")
IF(UNIX AND NOT APPLE)
TARGET_LINK_LIBRARIES(${DEVICE_NAME} ${Boost_LIBRARIES})
ENDIF(UNIX AND NOT APPLE)
SET(DEVICE_NAME sot-talos-device)
ADD_LIBRARY(${DEVICE_NAME} SHARED sot-talos-device.cpp)
INSTALL(TARGETS ${DEVICE_NAME} DESTINATION ${CMAKE_INSTALL_LIBDIR})
# Link the dynamic library containing the SoT with its dependencies.
TARGET_LINK_LIBRARIES(${DEVICE_NAME}
sot-core::sot-core
dynamic-graph-python::dynamic-graph-python
dynamic_graph_bridge::ros_bridge)
# build python submodule
STRING(REPLACE - _ PYTHON_LIBRARY_NAME ${DEVICE_NAME})
SET(NEW_ENTITY_CLASS ${ENTITIES})
DYNAMIC_GRAPH_PYTHON_MODULE("sot/talos/${PYTHON_LIBRARY_NAME}"
${DEVICE_NAME}
sot-talos-${PYTHON_LIBRARY_NAME}-wrap
)
INSTALL(TARGETS ${DEVICE_NAME} EXPORT ${TARGETS_EXPORT_NAME} DESTINATION lib)
ENDMACRO()
build_talos_device()
# build python submodule
STRING(REPLACE - _ PYTHON_LIBRARY_NAME ${DEVICE_NAME})
SET(NEW_ENTITY_CLASS ${ENTITIES})
DYNAMIC_GRAPH_PYTHON_MODULE("sot/talos/${PYTHON_LIBRARY_NAME}"
${DEVICE_NAME} sot-talos-${PYTHON_LIBRARY_NAME}-wrap)
# Add the library to wrap the controller of Pyrene.
MACRO(build_talos_controller)
SET(CONTROLLER_NAME sot-pyrene-controller)
ADD_LIBRARY(${CONTROLLER_NAME}
SHARED
sot-talos-controller.cpp
sot-pyrene-controller.cpp
)
# Link the dynamic library containing the SoT with its dependencies.
PKG_CONFIG_USE_DEPENDENCY(${CONTROLLER_NAME} "dynamic-graph")
PKG_CONFIG_USE_DEPENDENCY(${CONTROLLER_NAME} "dynamic-graph-python")
PKG_CONFIG_USE_DEPENDENCY(${CONTROLLER_NAME} "sot-core")
PKG_CONFIG_USE_DEPENDENCY(${CONTROLLER_NAME} "dynamic_graph_bridge")
ADD_DEPENDENCIES(${CONTROLLER_NAME} "sot-talos-device")
TARGET_LINK_LIBRARIES(${CONTROLLER_NAME} "sot-talos-device")
TARGET_LINK_LIBRARIES(${CONTROLLER_NAME} "ros_bridge")
TARGET_LINK_LIBRARIES(${CONTROLLER_NAME} "ros_interpreter")
IF(UNIX AND NOT APPLE)
TARGET_LINK_LIBRARIES(${CONTROLLER_NAME} ${Boost_LIBRARIES})
ENDIF(UNIX AND NOT APPLE)
INSTALL(TARGETS ${CONTROLLER_NAME} DESTINATION ${CMAKE_INSTALL_LIBDIR})
SET(CONTROLLER_NAME sot-pyrene-controller)
ADD_LIBRARY(${CONTROLLER_NAME} SHARED ${CONTROLLER_NAME}.cpp
sot-talos-controller.cpp)
ENDMACRO()
# Link the dynamic library containing the SoT with its dependencies.
TARGET_LINK_LIBRARIES(${CONTROLLER_NAME} ${DEVICE_NAME}
dynamic_graph_bridge::ros_interpreter
dynamic_graph_bridge::ros_bridge)
build_talos_controller()
INSTALL(TARGETS ${CONTROLLER_NAME} EXPORT ${TARGETS_EXPORT_NAME} DESTINATION lib)
# -*- coding: utf-8 -*-
# Copyright 2011, Florent Lamiraux, Thomas Moulard, JRL, CNRS/AIST
# sys.argv is not defined when running the remove interpreter, but it is
# required by rospy
import sys
from dynamic_graph.entity import PyEntityFactoryClass
from dynamic_graph.sot.pyrene.robot import Robot
if not hasattr(sys, 'argv'):
sys.argv = [
"dynamic_graph",
]
print("Prologue Pyrene TALOS Robot")
......@@ -19,8 +28,7 @@ def makeRobot():
DeviceTalos = PyEntityFactoryClass('DeviceTalos')
# Create the robot using the device.
robot = Robot(name='robot', device=DeviceTalos('PYRENE'))
robot = Robot(name='robot', device=DeviceTalos('PYRENE'), fromRosParam=True)
robot.dynamic.com.recompute(0)
_com = robot.dynamic.com.value
robot.device.zmp.value = (_com[0], _com[1], 0.)
......
......@@ -50,7 +50,7 @@ class Robot(Talos):
0. # Head
)
def __init__(self, name, device=None, tracer=None):
def __init__(self, name, device=None, tracer=None, fromRosParam=None):
Talos.__init__(self, name, self.halfSitting, device, tracer)
"""
TODO:Confirm these values
......
......@@ -2,7 +2,8 @@
# Copyright 2012, CNRS-LAAS, Florent Lamiraux
import numpy as np
from dynamic_graph.sot.core import RPYToMatrix
from dynamic_graph.sot.core.matrix_util import RPYToMatrix
from dynamic_graph.sot.pyrene.robot import Robot
from dynamic_graph.sot.tools.se3 import R3, SE3
......
......@@ -3,12 +3,13 @@
from __future__ import print_function
import pinocchio as se3
from dynamic_graph import plug
from dynamic_graph.sot.core.math_small_entities import Derivator_of_Vector
from dynamic_graph.sot.dynamics_pinocchio import DynamicPinocchio
from dynamic_graph.sot.dynamics_pinocchio.humanoid_robot import AbstractHumanoidRobot
from dynamic_graph.sot.dynamic_pinocchio import DynamicPinocchio
from dynamic_graph.sot.dynamic_pinocchio.humanoid_robot import AbstractHumanoidRobot
from pinocchio import JointModelFreeFlyer, buildModelFromXML, buildReducedModel, neutral
from pinocchio.robot_wrapper import RobotWrapper
from rospkg import RosPack
# Internal helper tool.
......@@ -27,6 +28,7 @@ class Talos(AbstractHumanoidRobot):
forceSensorInLeftAnkle = ((1., 0., 0., 0.), (0., 1., 0., 0.), (0., 0., 1., -0.107), (0., 0., 0., 1.))
forceSensorInRightAnkle = ((1., 0., 0., 0.), (0., 1., 0., 0.), (0., 0., 1., -0.107), (0., 0., 0., 1.))
defaultFilename = "talos_reduced_v2.urdf"
"""
TODO: Confirm the position and existence of these sensors
accelerometerPosition = np.matrix ((
......@@ -50,7 +52,7 @@ class Talos(AbstractHumanoidRobot):
res = config[0:27] + 7 * (0., ) + config[27:34] + 7 * (0., ) + config[34:]
return res
def __init__(self, name, initialConfig, device=None, tracer=None):
def __init__(self, name, initialConfig, device=None, tracer=None, fromRosParam=False):
self.OperationalPointsMap = {
'left-wrist': 'arm_left_7_joint',
'right-wrist': 'arm_right_7_joint',
......@@ -61,14 +63,40 @@ class Talos(AbstractHumanoidRobot):
'chest': 'torso_2_joint'
}
from rospkg import RosPack
rospack = RosPack()
urdfPath = rospack.get_path('talos_data') + "/urdf/talos_reduced.urdf"
urdfDir = [rospack.get_path('talos_data') + "/../"]
if fromRosParam:
print("Using ROS parameter \"/robot_description\"")
rosParamName = "/robot_description"
import rospy
if rosParamName not in rospy.get_param_names():
raise RuntimeError('"' + rosParamName + '" is not a ROS parameter.')
s = rospy.get_param(rosParamName)
model = buildModelFromXML(s, JointModelFreeFlyer())
# get mimic joints
mimicJoints = list()
import xml.etree.ElementTree as ET
root = ET.fromstring(s)
for e in root.iter('joint'):
if 'name' in e.attrib:
name = e.attrib['name']
for c in e._children:
if hasattr(c, 'tag') and c.tag == 'mimic':
mimicJoints.append(name)
jointIds = list()
for j in mimicJoints:
jointIds.append(model.getJointId(j))
q = neutral(model)
reducedModel = buildReducedModel(model, jointIds, q)
pinocchioRobot = RobotWrapper(model=reducedModel)
else:
# Create a wrapper to access the dynamic model provided
# through an urdf file.
pinocchioRobot = RobotWrapper()
rospack = RosPack()
urdfPath = rospack.get_path('talos_data') + "/urdf/" + self.defaultFilename
urdfDir = [rospack.get_path('talos_data') + "/../"]
pinocchioRobot.initFromURDF(urdfPath, urdfDir, JointModelFreeFlyer())
# Create a wrapper to access the dynamic model provided through an urdf file.
pinocchioRobot = RobotWrapper()
pinocchioRobot.initFromURDF(urdfPath, urdfDir, se3.JointModelFreeFlyer())
self.pinocchioModel = pinocchioRobot.model
self.pinocchioData = pinocchioRobot.data
......@@ -90,12 +118,12 @@ class Talos(AbstractHumanoidRobot):
# TODO For position limit, we remove the first value to get
# a vector of the good size because SoT use euler angles and not
# quaternions...
self.device.setPositionBounds(self.pinocchioModel.lowerPositionLimit.T.tolist()[0][1:],
self.pinocchioModel.upperPositionLimit.T.tolist()[0][1:])
self.device.setVelocityBounds((-self.pinocchioModel.velocityLimit).T.tolist()[0],
self.pinocchioModel.velocityLimit.T.tolist()[0])
self.device.setTorqueBounds((-self.pinocchioModel.effortLimit).T.tolist()[0],
self.pinocchioModel.effortLimit.T.tolist()[0])
self.device.setPositionBounds(self.pinocchioModel.lowerPositionLimit.tolist()[1:],
self.pinocchioModel.upperPositionLimit.tolist()[1:])
self.device.setVelocityBounds((-self.pinocchioModel.velocityLimit).tolist(),
self.pinocchioModel.velocityLimit.tolist())
self.device.setTorqueBounds((-self.pinocchioModel.effortLimit).tolist(),
self.pinocchioModel.effortLimit.tolist())
self.halfSitting = initialConfig
self.device.set(self.halfSitting)
plug(self.device.state, self.dynamic.position)
......
# flake8: noqa
# from dynamic_graph.sot.core.meta_task_posture import MetaTaskKinePosture
from numpy import eye, hstack, identity, zeros
from dynamic_graph import plug
# Create the solver
# Connects the sequence player to the posture task
from dynamic_graph.sot.core import SOT, FeatureGeneric, GainAdaptive, Selec_of_vector, Task
from dynamic_graph.sot.core.feature_generic import FeatureGeneric
from dynamic_graph.sot.core.gain_adaptive import GainAdaptive
from dynamic_graph.sot.core.matrix_util import matrixToTuple
from dynamic_graph.sot.core.meta_tasks import setGain
from dynamic_graph.sot.core.operator import Selec_of_vector
# Create the solver
# Connects the sequence player to the posture task
from dynamic_graph.sot.core.sot import SOT, Task
from dynamic_graph.sot.tools import SimpleSeqPlay
from numpy import eye, hstack, identity, zeros
# Create the posture task
task_name = "posture_task"
......
# flake8: noqa
from dynamic_graph import plug
from dynamic_graph.ros import RosPublish
from dynamic_graph.sot.core import SOT
from dynamic_graph.sot.core.matrix_util import matrixToTuple
from dynamic_graph.sot.core.meta_tasks_kine import MetaTaskKine6d, MetaTaskKineCom, gotoNd
from dynamic_graph.sot.core.meta_tasks_kine import (MetaTaskKine6d, MetaTaskKineCom, gotoNd)
from dynamic_graph.sot.core.sot import SOT
from numpy import eye
taskRH = MetaTaskKine6d('rh', robot.dynamic, 'rh', robot.OperationalPointsMap['right-wrist'])
......@@ -23,13 +23,11 @@ contactLF = MetaTaskKine6d('contactLF', robot.dynamic, 'LF', robot.OperationalPo
contactLF.feature.frame('desired')
contactLF.gain.setConstant(10)
contactLF.keep()
locals()['contactLF'] = contactLF
contactRF = MetaTaskKine6d('contactRF', robot.dynamic, 'RF', robot.OperationalPointsMap['right-ankle'])
contactRF.feature.frame('desired')
contactRF.gain.setConstant(10)
contactRF.keep()
locals()['contactRF'] = contactRF
sot = SOT('sot')
sot.setSize(robot.dynamic.getDimension())
......
#!/usr/bin/python
# flake8: noqa
import rospy
from dynamic_graph_bridge.srv import *
from dynamic_graph_bridge_msgs.srv import *
from std_srvs.srv import *
......
......@@ -3,7 +3,6 @@
import sys
import rospy
from dynamic_graph_bridge.srv import *
from dynamic_graph_bridge_msgs.srv import *
from std_srvs.srv import *
......
#!/usr/bin/python
# flake8: noqa
import rospy
from dynamic_graph_bridge.srv import *
from dynamic_graph_bridge_msgs.srv import *
from std_srvs.srv import *
from dynamic_graph_bridge_msgs.srv import *
try:
# Python 2
input = raw_input
......@@ -38,12 +38,13 @@ try:
runCommandClient = rospy.ServiceProxy('run_command', RunCommand)
runCommandStartDynamicGraph = rospy.ServiceProxy('start_dynamic_graph', Empty)
initCode = open("appli.py", "r").read().split("\n")
with open("appli.py", "r") as f:
initCode = f.read().split("\n")
rospy.loginfo("Stack of Tasks launched")
# runCommandClient("from dynamic_graph import plug")
# runCommandClient("from dynamic_graph.sot.core import SOT")
# runCommandClient("from dynamic_graph.sot.core.sot import SOT")
# runCommandClient("sot = SOT('sot')")
# runCommandClient("sot.setSize(robot.dynamic.getDimension())")
# runCommandClient("plug(sot.control,robot.device.control)")
......
#!/usr/bin/python
# flake8: noqa
import rospy
from dynamic_graph_bridge.srv import *
from dynamic_graph_bridge_msgs.srv import *
from std_srvs.srv import *
......@@ -43,7 +42,7 @@ try:
rospy.loginfo("Stack of Tasks launched")
# runCommandClient("from dynamic_graph import plug")
# runCommandClient("from dynamic_graph.sot.core import SOT")
# runCommandClient("from dynamic_graph.sot.core.sot import SOT")
# runCommandClient("sot = SOT('sot')")
# runCommandClient("sot.setSize(robot.dynamic.getDimension())")
# runCommandClient("plug(sot.control,robot.device.control)")
......
......@@ -7,15 +7,19 @@
# ******************************************************************************
import pinocchio as se3
from numpy import hstack, identity, zeros
from pinocchio.robot_wrapper import RobotWrapper
from dynamic_graph import plug
from dynamic_graph.sot.core import SOT, FeatureGeneric, GainAdaptive, Selec_of_vector, Task
from dynamic_graph.sot.core.feature_generic import FeatureGeneric
from dynamic_graph.sot.core.gain_adaptive import GainAdaptive
from dynamic_graph.sot.core.matrix_util import matrixToTuple
from dynamic_graph.sot.core.meta_tasks import setGain
from dynamic_graph.sot.dynamics_pinocchio import fromSotToPinocchio
from dynamic_graph.sot.dynamics_pinocchio.humanoid_robot import HumanoidRobot
from dynamic_graph.sot.core.operator import Selec_of_vector
from dynamic_graph.sot.core.sot import SOT, Task
from dynamic_graph.sot.dynamic_pinocchio import fromSotToPinocchio
from dynamic_graph.sot.dynamic_pinocchio.humanoid_robot import HumanoidRobot
from dynamic_graph.sot.tools import SimpleSeqPlay
from numpy import hstack, identity, zeros
from pinocchio.robot_wrapper import RobotWrapper
# -----------------------------------------------------------------------------
# SET THE PATH TO THE URDF AND MESHES
......