Commit 46b0f606 authored by Francois Keith's avatar Francois Keith
Browse files

Clean and comment the script p105 with romeo.

Add the possibility to use both viewer.
parent a25df243
......@@ -20,17 +20,23 @@ from dynamic_graph.sot.core.meta_task_posture import MetaTaskKinePosture
from dynamic_graph.sot.core.meta_task_visual_point import MetaTaskVisualPoint
from dynamic_graph.sot.core.utils.attime import attime,ALWAYS,refset,sigset
from dynamic_graph.tracer import Tracer
from dynamic_graph.sot.core.utils.history import History
from numpy import *
from dynamic_graph.sot.core.utils.history import History
# create the robot and plug main signal.
from dynamic_graph.sot.romeo.romeo import *
robot = Robot('robot')
robot = Robot('romeo', device=RobotSimu('romeo'))
plug(robot.device.state, robot.dynamic.position)
# Binds with ROS. assert that roscore is running.
# if you prefer a ROS free installation, please comment those lines.
from dynamic_graph.ros import *
ros = Ros(robot)
# Alternate visualization tool
from dynamic_graph.sot.core.utils.viewer_helper import addRobotViewer
addRobotViewer(robot.device,small=True,small_extra=24,verbose=False)
# --- ROBOT SIMU ---------------------------------------------------------------
# --- ROBOT SIMU ---------------------------------------------------------------
......@@ -56,9 +62,11 @@ runner=inc()
#---- DYN --------------------------------------------------------------------
#-----------------------------------------------------------------------------
# initialize
robot.dynamic.velocity.value = robot.dimension*(0.,)
robot.dynamic.acceleration.value = robot.dimension*(0.,)
# Unplug the signals og the free floatting: it is not fixed in the scene anymore.
robot.dynamic.ffposition.unplug()
robot.dynamic.ffvelocity.unplug()
robot.dynamic.ffacceleration.unplug()
......@@ -66,8 +74,6 @@ robot.dynamic.ffacceleration.unplug()
robot.dynamic.setProperty('ComputeBackwardDynamics','true')
robot.dynamic.setProperty('ComputeAccelerationCoM','true')
robot.device.control.unplug()
# ---- SOT ---------------------------------------------------------------------
# ---- SOT ---------------------------------------------------------------------
# ---- SOT ---------------------------------------------------------------------
......@@ -78,6 +84,10 @@ def toList(sot):
SolverKine.toList = toList
sot = SolverKine('sot')
sot.setSize(robot.dimension)
# The control signal, previously plugged to the velocity (kinematic mode) is
# redirected.
robot.device.control.unplug()
plug(sot.control,robot.device.control)
# ---- TASKS -------------------------------------------------------------------
......@@ -87,10 +97,12 @@ plug(sot.control,robot.device.control)
# ---- TASK GRIP ---
taskRH=MetaTaskKine6d('rh',robot.dynamic,'rh','right-wrist')
# change the position of the operational point wrt the wrist
handMgrip=eye(4); handMgrip[0:3,3] = (0.07,-0.02,0)
taskRH.opmodif = matrixToTuple(handMgrip)
taskRH.feature.frame('desired')
taskLH=MetaTaskKine6d('lh',robot.dynamic,'lh','left-wrist')
taskLH.opmodif = matrixToTuple(handMgrip)
taskLH.feature.frame('desired')
......@@ -99,6 +111,7 @@ taskLH.feature.frame('desired')
taskCom = MetaTaskKineCom(robot.dynamic)
# --- TASK AVOID
# define a inequality task to reduce the shoulder range.
taskShoulder=MetaTaskKine6d('shoulder',robot.dynamic,'shoulder','LShoulderPitch')
taskShoulder.feature.frame('desired')
gotoNd(taskShoulder,(0,0,0),'010')
......@@ -109,6 +122,7 @@ taskShoulder.task.referenceSup.value = (0.20,) # Xmin, Ymin
taskShoulder.task.dt.value=dt
taskShoulder.task.controlGain.value = 0.9
# same thing for the elbow
taskElbow=MetaTaskKine6d('elbow',robot.dynamic,'elbow','LElbowYaw')
taskElbow.feature.frame('desired')
gotoNd(taskElbow,(0,0,0),'010')
......@@ -140,8 +154,6 @@ plug(robot.dynamic.Jcom,featureSupportSmall.jacobianIN)
taskSupportSmall=TaskInequality('taskSupportSmall')
taskSupportSmall.add(featureSupportSmall.name)
taskSupportSmall.selec.value = '011'
#taskSupportSmall.referenceInf.value = (-0.02,-0.02,0) # Xmin, Ymin
#askSupportSmall.referenceSup.value = (0.02,0.02,0) # Xmax, Ymax
taskSupportSmall.referenceInf.value = (-0.02,-0.05,0) # Xmin, Ymin
taskSupportSmall.referenceSup.value = (0.02,0.05,0) # Xmax, Ymax
taskSupportSmall.dt.value=dt
......@@ -297,21 +309,6 @@ class BallPosition:
b = BallPosition()
# tr.add(taskJL.name+".normalizedPosition","qn")
# tr.add('dyn.com','com')
# tr.add(taskRH.task.name+'.error','erh')
# tr.add(taskFoV.feature.name+'.error','fov')
# tr.add(taskFoV.task.name+'.normalizedPosition','fovn')
# tr.add(taskSupportSmall.name+".normalizedPosition",'comsmalln')
# tr.add(taskSupport.name+".normalizedPosition",'comn')
# robot.device.after.addSignal(taskJL.name+".normalizedPosition")
# robot.device.after.addSignal(taskSupportSmall.name+".normalizedPosition")
# robot.device.after.addSignal(taskFoV.task.name+".normalizedPosition")
# robot.device.after.addSignal(taskFoV.feature.name+'.error')
# robot.device.after.addSignal(taskSupport.name+".normalizedPosition")
# --- RUN ----------------------------------------------------------------------
# --- RUN ----------------------------------------------------------------------
# --- RUN ----------------------------------------------------------------------
......
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