Commit 14cea02e authored by Guilhem Saurel's avatar Guilhem Saurel

[Python 3]

parent 183c961c
......@@ -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
......
# 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_msgs.srv import *
from std_srvs.srv import *
from dynamic_graph_bridge_msgs.srv import *
try:
# Python 2
input = raw_input
......@@ -37,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)")
......
......@@ -42,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.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
......
Markdown is supported
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