From 83fdbb2bc64c64cbb6fe0d7daf21073f264adc46 Mon Sep 17 00:00:00 2001
From: Rohan Budhiraja <budhiraja@laas.fr>
Date: Thu, 21 Jul 2016 12:30:43 +0200
Subject: [PATCH] [unitTesting] add simple python tutorial
---
unitTesting/kineromeo.py | 138 +++++++++++++++++++++++++++++++++++++++
1 file changed, 138 insertions(+)
create mode 100644 unitTesting/kineromeo.py
diff --git a/unitTesting/kineromeo.py b/unitTesting/kineromeo.py
new file mode 100644
index 0000000..d4345b0
--- /dev/null
+++ b/unitTesting/kineromeo.py
@@ -0,0 +1,138 @@
+# ______________________________________________________________________________
+# ******************************************************************************
+#
+# The simplest robot task: Just go and reach a point
+#
+# ______________________________________________________________________________
+# ******************************************************************************
+from dynamic_graph import plug
+from dynamic_graph.sot.core import *
+from dynamic_graph.sot.dynamics import *
+import dynamic_graph.script_shortcuts
+from dynamic_graph.script_shortcuts import optionalparentheses
+from dynamic_graph.sot.core.matrix_util import matrixToTuple, vectorToTuple,rotate, matrixToRPY
+from dynamic_graph.sot.core.meta_tasks_kine import *
+from dynamic_graph.sot.core.utils.viewer_helper import addRobotViewer,VisualPinger,updateComDisplay
+from numpy import *
+
+set_printoptions(suppress=True, precision=7)
+
+#-----------------------------------------------------------------------------
+#---- ROBOT SPECIFICATIONS----------------------------------------------------
+#-----------------------------------------------------------------------------
+
+#Define robotName, urdfpath and initialConfig
+
+#robotName = 'romeo'
+#urdfpath = ""
+
+#initialConfig = (0, 0, .840252, 0, 0, 0, # FF
+# 0, 0, -0.3490658, 0.6981317, -0.3490658, 0, # LLEG
+# 0, 0, -0.3490658, 0.6981317, -0.3490658, 0, # RLEG
+# 0, # TRUNK
+# 1.5, 0.6, -0.5, -1.05, -0.4, -0.3, -0.2, # LARM
+# 0, 0, 0, 0, # HEAD
+# 1.5, -0.6, 0.5, 1.05, -0.4, -0.3, -0.2, # RARM
+# )
+
+#-----------------------------------------------------------------------------
+#---- DYN --------------------------------------------------------------------
+#-----------------------------------------------------------------------------
+dyn = Dynamic("dyn")
+dyn.setFile(urdfpath)
+dyn.parse()
+dyn.displayModel()
+
+inertiaRotor = (0,)*6+(5e-4,)*31
+gearRatio = (0,)*6+(200,)*31
+dyn.inertiaRotor.value = inertiaRotor
+dyn.gearRatio.value = gearRatio
+dyn.velocity.value = robotDim*(0.,)
+dyn.acceleration.value = robotDim*(0.,)
+
+# ------------------------------------------------------------------------------
+# --- ROBOT SIMULATION ---------------------------------------------------------
+# ------------------------------------------------------------------------------
+
+
+
+robot = RobotSimu(robotName)
+robot.resize(robotDim)
+dt=5e-3
+
+robot.set( initialConfig )
+addRobotViewer(robot, small=False)
+
+plug(robot.state,dyn.position)
+
+# ------------------------------------------------------------------------------
+# ---- Kinematic Stack of Tasks (SoT) -----------------------------------------
+# ------------------------------------------------------------------------------
+sot = SOT('sot')
+sot.setSize(robotDim)
+plug(sot.control,robot.control)
+
+
+# ------------------------------------------------------------------------------
+# ---- TASKS -------------------------------------------------------------------
+# ------------------------------------------------------------------------------
+# ---- TASK -----------------------------
+
+# ---- TASK GRIP
+taskRH = MetaTaskKine6d('rh',dyn,'rh','RWristPitch')
+handMgrip = eye(4); handMgrip[0:3,3] = (0.1,0,0)
+taskRH.opmodif = matrixToTuple(handMgrip)
+taskRH.feature.frame('desired')
+# --- STATIC COM (if not walking)
+taskCom = MetaTaskKineCom(dyn)
+dyn.com.recompute(0)
+taskCom.featureDes.errorIN.value = dyn.com.value
+taskCom.task.controlGain.value = 10
+
+
+# --- CONTACTS
+#define contactLF and contactRF
+for name,joint in [ ['LF','LAnkleRoll'], ['RF','RAnkleRoll' ] ]:
+ contact = MetaTaskKine6d('contact'+name,dyn,name,joint)
+ contact.feature.frame('desired')
+ contact.gain.setConstant(10)
+ contact.keep()
+ locals()['contact'+name] = contact
+
+
+target = (0.5,-0.2,1.3)
+robot.viewer.updateElementConfig('zmp',target+(0,0,0))
+gotoNd(taskRH,target,'111',(4.9,0.9,0.01,0.9))
+sot.push(contactRF.task.name)
+sot.push(contactLF.task.name)
+sot.push(taskCom.task.name)
+sot.push(taskRH.task.name)
+
+#-------------------------------------------------------------------------------
+#----- MAIN LOOP ---------------------------------------------------------------
+#-------------------------------------------------------------------------------
+
+from dynamic_graph.sot.core.utils.thread_interruptible_loop import loopInThread,loopShortcuts
+@loopInThread
+def inc():
+ robot.increment(dt)
+# print "dyn_position_value_devel"
+# print asarray(dyn.position.value)
+# print "robot_control_value_devel"
+# print asarray(robot.control.value)
+# print "task_contactlf_jacobian_devel"
+# print transpose(asarray(contactLF.feature.signal("jacobian").value))
+# print "dyn_JLF_devel"
+# print transpose(asarray(dyn.signal("JLF").value))
+runner=inc()
+[go,stop,next,n]=loopShortcuts(runner)
+
+print "dyn_position_value_devel"
+print asarray(dyn.position.value)
+print "robot_control_value_devel"
+print asarray(robot.control.value)
+print "task_contactlf_jacobian_devel"
+print transpose(asarray(contactLF.feature.signal("jacobian").value))
+print "dyn_JLF_devel"
+print transpose(asarray(dyn.signal("JLF").value))
+go()
--
GitLab