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.robot import *
robot = Robot('romeo_small', device=RobotSimu('romeo_small'))
# 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.application.velocity.precomputed_tasks import initialize
solver = initialize ( 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)
runner=inc()
[go,stop,next,n]=loopShortcuts(runner)
# ---- TASKS -------------------------------------------------------------------
from dynamic_graph.sot.expression_graph.types import BaseElement
from dynamic_graph.sot.expression_graph.types import *
from dynamic_graph.sot.expression_graph.expression_graph import *
from dynamic_graph.sot.expression_graph.functions import *
from dynamic_graph.sot.robohow.superviser import *
# 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.dyninv import SolverKine
from dynamic_graph.sot.application.velocity.precomputed_tasks import initialize
solver = initialize ( robot, SolverKine )
# load the initialization prototype for the pr2
#from dynamic_graph.sot.pr2.pr2_tasks import *
#solver = initialize(robot, SolverKine)
# allows the publication of the velocity in the JointState message
plug(solver.jointLimitator.control, ros.rosJointState.velocity)
# Creates the superviser, that will handle the stack of task update
superviser = Superviser(robot, solver, ros.rosPublish)
# Additional frames.
robot.frames['l_gripper'] = robot.frames['leftGripper']
robot.frames['r_gripper'] = robot.frames['rightGripper']
robot.expressions={}
from dynamic_graph.sot.application.velocity.precomputed_tasks import initialize #deprecated
solver=initialize(robot) #deprecated
from dynamic_graph.sot.application.velocity.precomputed_tasks import Application
appli =Application(robot)
robot.initializeTracer()
#launch service start_dynamic_graph
#walk few steps
from dynamic_graph.sot.pattern_generator.walking import CreateEverythingForPG, walkFewSteps, walkFewStepsCircular
CreateEverythingForPG(robot,solver)
walkFewSteps(robot)
#-----------creation of a task---------------
from dynamic_graph.sot.core.meta_tasks import generic6dReference, Task, GainAdaptive
from dynamic_graph.sot.core.matrix_util import matrixToTuple
from dynamic_graph import plug
from numpy import *
#show tasks in solver controling the robot
print solver #deprecated
print appli.solver
#show library of precomputed tasks
robot.tasks
#
appli.gains['right-wrist'].setByPoint(4,0.2,0.01,0.8)
trunktask = Task ("trunk-task")
#----------------PINOCCHIO----------------------------#
import pinocchio as se3
from pinocchio.robot_wrapper import RobotWrapper
pinocchioRobot = RobotWrapper(_urdfPath, _urdfDir, se3.JointModelFreeFlyer())
pinocchioRobot.initDisplay(loadModel=True)
#----------------ROBOT - DEVICE AND DYNAMICS----------#
from dynamic_graph.sot.dynamics.humanoid_robot import HumanoidRobot
robot = HumanoidRobot(_robotName, pinocchioRobot.model, pinocchioRobot.data,
_initialConfig, _OperationalPointsMap)
#-----------------------------------------------------#
#----------------SOT (SOLVER)-------------------------#
from dynamic_graph.sot.application.velocity.precomputed_tasks import initialize
solver = initialize(robot)
#-----------------------------------------------------#
#----------------PG--------------------------------#
#from dynamic_graph.sot.pattern_generator.walking import initPg, initZMPRef, initWaistCoMTasks, initFeetTask, initPostureTask, pushTasks
from dynamic_graph.sot.pattern_generator.walking import walkNaveau, CreateEverythingForPG, walkFewSteps, walkAndrei
CreateEverythingForPG(robot, solver)
#------------------------------------------------#
#walkFewSteps ( robot )
walkNaveau(robot)
#walkAndrei( robot )
robot.pg.velocitydes.value = (0.1, 0.0, 0.0)
#-------------------------------------------------------------------------------
#----- MAIN LOOP ---------------------------------------------------------------
def __init__(self, robot):
ros = Ros(robot)
solver = initialize ( robot, SolverKine )
self.solver = solver
# --- 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
# ---- TASKS -------------------------------------------------------------------
# operational point used
ground_z = VersorElement('ground_z', robot, 'ground', versor = (0,0,1))
r_gripper_x = VersorElement('r_gripper_x', robot, 'rightGripper', versor = (1,0,0) )
(taskAngleBottle, angleBottleZ) = createTaskAndFeature('bottleZ', r_gripper_x, ground_z, 'angle')
angleBottleZ.reference.value = radians(90)
l_gripper_x = VersorElement('r_gripper_x', robot, 'leftGripper', versor = (1,0,0) )
(taskAngleCup, angleBottleCup) = createTaskAndFeature('cupZ', l_gripper_x, ground_z, 'angle')
angleBottleCup.reference.value = radians(90)
# # # # # # #
ground_plane = PlaneElement('ground_plane', robot, 'ground', normal = (0,0,1))
r_gripper_y = VersorElement('r_gripper_y', robot, 'rightGripper', versor = (0,1,0) )
(taskPlanBottleY, planBottleY) = createTaskAndFeature('bottleY', ground_plane, r_gripper_y, 'angle')
planBottleY.reference.value = 0
################################ #######################""
## position right hand above target
# heightZ = PointElement('heightZ', robot, 'ground', position = (0,0,1))
l_gripperZpos = PointElement('l_gripperZpos', robot, 'leftGripper')
r_gripperZpos = PointElement('r_gripperZpos', robot, 'rightGripper')
(taskGripperZ, positionZ) = createFeaturePointToPoint('positionZ', r_gripperZpos, l_gripperZpos)
positionZ.selec.value ='100'
positionZ.reference.value = (-0.1,)
#######################################################""
## position leftHand op above right hand
## -pi/8 << dot(bottle_normal, World_Z_axis) << pi/8
#posXY = PointElement('posXY', robot, 'ground')
l_gripperXY = PointElement('l_gripperXY', robot, 'leftGripper')
r_gripperXY = PointElement('r_gripperXY', robot, 'rightGripper')
(taskGripperXY, positionXY) = createFeaturePointToPoint('positionXY', l_gripperXY, r_gripperXY)
positionXY.selec.value = '011'
positionXY.reference.value = (0,0)
# define a task for the orientation of the fingertips : parallel to the handle
# line / line constraint
#tips = FeatureVersorToVersor('tips')
ground_x = VersorElement('ground_x', robot, 'ground', versor = (1,0,0))
(taskTips, tips) = createTaskAndFeature('tips', ground_x, r_gripper_y, 'angle')
tips.reference.value = 2.5
l_gripper_angle = Gripper('r_gripper_angle', robot, 38, 2)
r_gripper_angle = Gripper('r_gripper_angle', robot, 29, 2)
## TODO ...
self.angleBottleZ = angleBottleZ
self.taskAngleBottle = taskAngleBottle
self.taskAngleCup = taskAngleCup
self.l_gripper_angle = l_gripper_angle
self.r_gripper_angle = r_gripper_angle
self.taskGripperXY =taskGripperXY
self.taskGripperZ =taskGripperZ
self.taskPlanBottleY =taskPlanBottleY
self.taskTips =taskTips
from dynamic_graph.sot.core.meta_tasks_kine import *
## Create the robot romeo.
from dynamic_graph.sot.romeo.robot import *
robot = Robot('romeo', device=RobotSimu('romeo'))
# 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.dyninv import SolverKine
from dynamic_graph.sot.application.velocity.precomputed_tasks import initialize
solver = initialize ( robot, SolverKine )
from dynamic_graph.sot.core.meta_tasks_kine import *
from dynamic_graph.sot.core.meta_task_generic import MetaTaskGeneric
#-------------------------------------------------------------------------------
#----- 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()
