Commit 1fd2005f authored by Guilhem Saurel's avatar Guilhem Saurel

[Python 3] Format

parent ddf649f3
# -*- coding: utf-8 -*-
# Copyright 2011, Florent Lamiraux, Thomas Moulard, JRL, CNRS/AIST
# flake8: noqa
from dynamic_graph.sot.core.matrix_util import matrixToTuple, vectorToTuple,rotate, matrixToRPY
# Binds with ROS. assert that roscore is running.
from dynamic_graph.ros import *
# Create a simple kinematic solver.
from dynamic_graph.sot.application.velocity.precomputed_tasks import initialize
from dynamic_graph.sot.core.matrix_util import matrixToRPY, matrixToTuple, rotate, vectorToTuple
from dynamic_graph.sot.core.meta_tasks_kine import *
from numpy import *
# -------------------------------------------------------------------------------
# ----- MAIN LOOP ---------------------------------------------------------------
# -------------------------------------------------------------------------------
# define the macro allowing to run the simulation.
from dynamic_graph.sot.core.utils.thread_interruptible_loop import loopInThread, loopShortcuts
# Create the robot romeo.
from dynamic_graph.sot.romeo.robot import *
from numpy 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.application.velocity.precomputed_tasks import initialize
solver = initialize ( robot )
solver = initialize(robot)
dt = 5e-3
#-------------------------------------------------------------------------------
#----- 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)
runner = inc()
[go, stop, next, n] = loopShortcuts(runner)
# ---- TASKS -------------------------------------------------------------------
# ---- TASKS -------------------------------------------------------------------
......@@ -36,9 +40,10 @@ runner=inc()
# ---- TASK GRIP ---
# Defines a task for the right hand.
taskRH=MetaTaskKine6d('rh',robot.dynamic,'right-wrist','right-wrist')
taskRH = MetaTaskKine6d('rh', robot.dynamic, 'right-wrist', 'right-wrist')
# Move the operational point.
handMgrip=eye(4); handMgrip[0:3,3] = (0.1,0,0)
handMgrip = eye(4)
handMgrip[0:3, 3] = (0.1, 0, 0)
taskRH.opmodif = matrixToTuple(handMgrip)
taskRH.feature.frame('desired')
# robot.tasks['right-wrist'].add(taskRH.feature.name)
......@@ -51,12 +56,12 @@ solver.sot.clear()
# --- CONTACTS
# define contactLF and contactRF
for name,joint in [ ['LF','left-ankle'], ['RF','right-ankle' ] ]:
contact = MetaTaskKine6d('contact'+name,robot.dynamic,name,joint)
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
locals()['contact' + name] = contact
# --- RUN ----------------------------------------------------------------------
......@@ -65,16 +70,15 @@ for name,joint in [ ['LF','left-ankle'], ['RF','right-ankle' ] ]:
# 2st param: objective
# 3rd param: selected parameters
# 4th param: gain
target=(0.5,-0.2,0.8)
gotoNd(taskRH,target,'111',(4.9,0.9,0.01,0.9))
target = (0.5, -0.2, 0.8)
gotoNd(taskRH, target, '111', (4.9, 0.9, 0.01, 0.9))
# Fill the stack with some tasks.
solver.push(contactRF.task)
solver.push(contactLF.task)
solver.push (robot.tasks ['com'])
solver.push(robot.tasks['com'])
solver.push(taskRH.task)
# type inc to run one iteration, or go to run indefinitely.
# go()
# -*- coding: utf-8 -*-
# Copyright 2011, Florent Lamiraux, Thomas Moulard, JRL, CNRS/AIST
# flake8: noqa
from dynamic_graph.sot.core.matrix_util import matrixToTuple, vectorToTuple,rotate, matrixToRPY
# Binds with ROS. assert that roscore is running.
from dynamic_graph.ros import *
# Create a simple kinematic solver.
from dynamic_graph.sot.application.velocity.precomputed_tasks import initialize
from dynamic_graph.sot.core.matrix_util import matrixToRPY, matrixToTuple, rotate, vectorToTuple
from dynamic_graph.sot.core.meta_tasks_kine import *
from numpy import *
# -------------------------------------------------------------------------------
# ----- MAIN LOOP ---------------------------------------------------------------
# -------------------------------------------------------------------------------
# define the macro allowing to run the simulation.
from dynamic_graph.sot.core.utils.thread_interruptible_loop import loopInThread, loopShortcuts
# Create the robot romeo.
from dynamic_graph.sot.romeo.robot import *
from numpy 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 )
solver = initialize(robot)
dt = 5e-3
#-------------------------------------------------------------------------------
#----- 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)
runner = inc()
[go, stop, next, n] = loopShortcuts(runner)
# ---- TASKS -------------------------------------------------------------------
# ---- TASKS -------------------------------------------------------------------
......@@ -36,9 +40,10 @@ runner=inc()
# ---- TASK GRIP ---
# Defines a task for the right hand.
taskRH=MetaTaskKine6d('rh',robot.dynamic,'right-wrist','right-wrist')
taskRH = MetaTaskKine6d('rh', robot.dynamic, 'right-wrist', 'right-wrist')
# Move the operational point.
handMgrip=eye(4); handMgrip[0:3,3] = (0.1,0,0)
handMgrip = eye(4)
handMgrip[0:3, 3] = (0.1, 0, 0)
taskRH.opmodif = matrixToTuple(handMgrip)
taskRH.feature.frame('desired')
# robot.tasks['right-wrist'].add(taskRH.feature.name)
......@@ -51,12 +56,12 @@ solver.sot.clear()
# --- CONTACTS
# define contactLF and contactRF
for name,joint in [ ['LF','left-ankle'], ['RF','right-ankle' ] ]:
contact = MetaTaskKine6d('contact'+name,robot.dynamic,name,joint)
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
locals()['contact' + name] = contact
# --- RUN ----------------------------------------------------------------------
......@@ -65,16 +70,15 @@ for name,joint in [ ['LF','left-ankle'], ['RF','right-ankle' ] ]:
# 2st param: objective
# 3rd param: selected parameters
# 4th param: gain
target=(0.5,-0.2,0.8)
gotoNd(taskRH,target,'111',(4.9,0.9,0.01,0.9))
target = (0.5, -0.2, 0.8)
gotoNd(taskRH, target, '111', (4.9, 0.9, 0.01, 0.9))
# Fill the stack with some tasks.
solver.push(contactRF.task)
solver.push(contactLF.task)
solver.push (robot.tasks ['com'])
solver.push(robot.tasks['com'])
solver.push(taskRH.task)
# type inc to run one iteration, or go to run indefinitely.
# go()
from dynamic import DynamicPinocchio as DynamicCpp
from angle_estimator import AngleEstimator
from zmp_from_forces import ZmpFromForces
import numpy as np
from numpy import arctan2, arcsin, sin, cos, sqrt
from numpy import cos, sin, sqrt
from dynamic import DynamicPinocchio as DynamicCpp
#DynamicOld = Dynamic
# DynamicOld = Dynamic
class DynamicPinocchio (DynamicCpp):
class DynamicPinocchio(DynamicCpp):
def __init__(self, name):
DynamicCpp.__init__(self, name)
self.model = None
self.data = None
def setData(self, pinocchio_data):
dynamic.wrap.set_pinocchio_data(self.obj,pinocchio_data)
dynamic.wrap.set_pinocchio_data(self.obj, pinocchio_data) # noqa TODO
self.data = pinocchio_data
return
def setModel(self, pinocchio_model):
dynamic.wrap.set_pinocchio_model(self.obj,pinocchio_model)
dynamic.wrap.set_pinocchio_model(self.obj, pinocchio_model) # noqa TODO
self.model = pinocchio_model
return
def fromSotToPinocchio(q_sot, freeflyer=True):
if freeflyer:
[r,p,y] = q_sot[3:6]
[r, p, y] = q_sot[3:6]
cr = cos(r)
cp = cos(p)
cy = cos(y)
......@@ -32,43 +33,42 @@ def fromSotToPinocchio(q_sot, freeflyer=True):
sp = sin(p)
sy = sin(y)
rotmat = np.matrix([[cy*cp, cy*sp*sr-sy*cr, cy*sp*cr+sy*sr],
[sy*cp, sy*sp*sr+cy*cr, sy*sp*cr-cy*sr],
[-sp, cp*sr, cp*cr]])
rotmat = np.matrix([[cy * cp, cy * sp * sr - sy * cr, cy * sp * cr + sy * sr],
[sy * cp, sy * sp * sr + cy * cr, sy * sp * cr - cy * sr], [-sp, cp * sr, cp * cr]])
d0 = rotmat[0,0]
d1 = rotmat[1,1]
d2 = rotmat[2,2]
rr = 1.0+d0+d1+d2
d0 = rotmat[0, 0]
d1 = rotmat[1, 1]
d2 = rotmat[2, 2]
rr = 1.0 + d0 + d1 + d2
if rr>0:
if rr > 0:
s = 0.5 / sqrt(rr)
_x = (rotmat[2,1] - rotmat[1,2]) * s
_y = (rotmat[0,2] - rotmat[2,0]) * s
_z = (rotmat[1,0] - rotmat[0,1]) * s
_x = (rotmat[2, 1] - rotmat[1, 2]) * s
_y = (rotmat[0, 2] - rotmat[2, 0]) * s
_z = (rotmat[1, 0] - rotmat[0, 1]) * s
_r = 0.25 / s
else:
#Trace is less than zero, so need to determine which
#major diagonal is largest
# Trace is less than zero, so need to determine which
# major diagonal is largest
if ((d0 > d1) and (d0 > d2)):
s = 0.5 / sqrt(1 + d0 - d1 - d2)
_x = 0.5 * s
_y = (rotmat[0,1] + rotmat[1,0]) * s
_z = (rotmat[0,2] + rotmat[2,0]) * s
_r = (rotmat[1,2] + rotmat[2,1]) * s
_y = (rotmat[0, 1] + rotmat[1, 0]) * s
_z = (rotmat[0, 2] + rotmat[2, 0]) * s
_r = (rotmat[1, 2] + rotmat[2, 1]) * s
elif (d1 > d2):
s = 0.5 / sqrt(1 + d0 - d1 - d2)
_x = (rotmat[0,1] + rotmat[1,0]) * s
_x = (rotmat[0, 1] + rotmat[1, 0]) * s
_y = 0.5 * s
_z = (rotmat[1,2] + rotmat[2,1]) * s
_r = (rotmat[0,2] + rotmat[2,0]) * s
_z = (rotmat[1, 2] + rotmat[2, 1]) * s
_r = (rotmat[0, 2] + rotmat[2, 0]) * s
else:
s = 0.5 / sqrt(1 + d0 - d1 - d2)
_x = (rotmat[0,2] + rotmat[2,0]) * s
_y = (rotmat[1,2] + rotmat[2,1]) * s
_x = (rotmat[0, 2] + rotmat[2, 0]) * s
_y = (rotmat[1, 2] + rotmat[2, 1]) * s
_z = 0.5 * s
_r = (rotmat[0,1] + rotmat[1,0]) * s
_r = (rotmat[0, 1] + rotmat[1, 0]) * s
return np.matrix([q_sot[0:3]+(_x,_y,_z,_r)+q_sot[6:]])
return np.matrix([q_sot[0:3] + (_x, _y, _z, _r) + q_sot[6:]])
else:
return np.matrix([q_sot[0:]])
......@@ -2,8 +2,10 @@
# -*- coding: utf-8 -*-
# Copyright 2011, Florent Lamiraux, Thomas Moulard, JRL, CNRS/AIST
from dynamic_graph import plug
from dynamic_graph.sot.core import SOT
from __future__ import print_function
import sys
from functools import reduce
# Robotviewer is optional
hasRobotViewer = True
......@@ -12,33 +14,25 @@ try:
except ImportError:
hasRobotViewer = False
####################
# Helper functions #
####################
def toList(tupleOfTuple):
result = [[0, 0, 0, 0],
[0, 0, 0, 0],
[0, 0, 0, 0],
[0, 0, 0, 0]]
for i in xrange(4):
for j in xrange(4):
result = [[0, 0, 0, 0], [0, 0, 0, 0], [0, 0, 0, 0], [0, 0, 0, 0]]
for i in range(4):
for j in range(4):
result[i][j] = tupleOfTuple[i][j]
return result
def toTuple(listOfList):
return ((listOfList[0][0], listOfList[0][1],
listOfList[0][2], listOfList[0][3]),
(listOfList[1][0], listOfList[1][1],
listOfList[1][2], listOfList[1][3]),
(listOfList[2][0], listOfList[2][1],
listOfList[2][2], listOfList[2][3]),
def toTuple(listOfList):
return ((listOfList[0][0], listOfList[0][1], listOfList[0][2],
listOfList[0][3]), (listOfList[1][0], listOfList[1][1], listOfList[1][2], listOfList[1][3]),
(listOfList[2][0], listOfList[2][1], listOfList[2][2],
listOfList[2][3]), (listOfList[3][0], listOfList[3][1], listOfList[3][2], listOfList[3][3]))
(listOfList[3][0], listOfList[3][1],
listOfList[3][2], listOfList[3][3]))
def displayHomogeneousMatrix(matrix):
"""
......@@ -49,40 +43,41 @@ def displayHomogeneousMatrix(matrix):
matrix_tuple = tuple(itertools.chain.from_iterable(matrix))
formatStr = ''
for i in xrange(4*4):
for i in range(4 * 4):
formatStr += '{0[' + str(i) + ']: <10} '
if i != 0 and (i + 1) % 4 == 0:
formatStr += '\n'
print formatStr.format(matrix_tuple)
print(formatStr.format(matrix_tuple))
def displayHrp2Configuration(cfg):
if len(cfg) != 36:
raise "bad configuration size"
str = ''
str += 'Free flyer:\n'
str += ' translation {0[0]: <+10f} {0[1]: <+10f} {0[2]: <+10f}\n'
str += ' rotation {0[3]: <+10f} {0[4]: <+10f} {0[5]: <+10f}\n'
str += 'Left leg:\n'
str += ' hip {0[6]: <+10f} {0[7]: <+10f} {0[8]: <+10f}\n'
str += ' knee {0[9]: <+10f}\n'
str += ' ankle {0[10]: <+10f} {0[11]: <+10f}\n'
str += 'Right leg:\n'
str += ' hip {0[12]: <+10f} {0[13]: <+10f} {0[14]: <+10f}\n'
str += ' knee {0[15]: <+10f}\n'
str += ' ankle {0[16]: <+10f} {0[17]: <+10f}\n'
str += 'Chest: {0[18]: <+10f} {0[19]: <+10f}\n'
str += 'Head: {0[20]: <+10f} {0[21]: <+10f}\n'
str += 'Left arm:\n'
str += ' shoulder {0[22]: <+10f} {0[23]: <+10f} {0[24]: <+10f}\n'
str += ' elbow {0[25]: <+10f}\n'
str += ' wrist {0[26]: <+10f} {0[27]: <+10f}\n'
str += ' clench {0[28]: <+10f}\n'
str += 'Left arm:\n'
str += ' shoulder {0[29]: <+10f} {0[30]: <+10f} {0[31]: <+10f}\n'
str += ' elbow {0[32]: <+10f}\n'
str += ' wrist {0[33]: <+10f} {0[34]: <+10f}\n'
str += ' clench {0[35]: <+10f}\n'
print str.format(cfg)
s = ''
s += 'Free flyer:\n'
s += ' translation {0[0]: <+10f} {0[1]: <+10f} {0[2]: <+10f}\n'
s += ' rotation {0[3]: <+10f} {0[4]: <+10f} {0[5]: <+10f}\n'
s += 'Left leg:\n'
s += ' hip {0[6]: <+10f} {0[7]: <+10f} {0[8]: <+10f}\n'
s += ' knee {0[9]: <+10f}\n'
s += ' ankle {0[10]: <+10f} {0[11]: <+10f}\n'
s += 'Right leg:\n'
s += ' hip {0[12]: <+10f} {0[13]: <+10f} {0[14]: <+10f}\n'
s += ' knee {0[15]: <+10f}\n'
s += ' ankle {0[16]: <+10f} {0[17]: <+10f}\n'
s += 'Chest: {0[18]: <+10f} {0[19]: <+10f}\n'
s += 'Head: {0[20]: <+10f} {0[21]: <+10f}\n'
s += 'Left arm:\n'
s += ' shoulder {0[22]: <+10f} {0[23]: <+10f} {0[24]: <+10f}\n'
s += ' elbow {0[25]: <+10f}\n'
s += ' wrist {0[26]: <+10f} {0[27]: <+10f}\n'
s += ' clench {0[28]: <+10f}\n'
s += 'Left arm:\n'
s += ' shoulder {0[29]: <+10f} {0[30]: <+10f} {0[31]: <+10f}\n'
s += ' elbow {0[32]: <+10f}\n'
s += ' wrist {0[33]: <+10f} {0[34]: <+10f}\n'
s += ' clench {0[35]: <+10f}\n'
print(s.format(cfg))
def initRobotViewer():
......@@ -91,10 +86,11 @@ def initRobotViewer():
if hasRobotViewer:
try:
clt = robotviewer.client()
except:
print "failed to connect to robotviewer"
except Exception:
print("failed to connect to robotviewer")
return clt
def reach(robot, op, tx, ty, tz):
sdes = toList(robot.dynamic.signal(op).value)
sdes[0][3] += tx
......@@ -105,28 +101,36 @@ def reach(robot, op, tx, ty, tz):
robot.features[op]._feature.signal('selec').value = '000111'
robot.tasks[op].signal('controlGain').value = 1.
def sqrDist(value, expectedValue):
"""Compute the square of the distance between two configurations."""
return reduce(lambda acc, (a, b): acc + abs(a - b) * abs(a - b),
zip(value, expectedValue), 0.)
def inner(acc, ab):
a, b = ab
return acc + abs(a - b) * abs(a - b)
return reduce(inner, zip(value, expectedValue), 0.)
def checkFinalConfiguration(position, finalPosition):
if sqrDist(position, finalPosition) >= 1e-3:
print "Wrong final position. Failing."
print "Value:"
print("Wrong final position. Failing.")
print("Value:")
displayHrp2Configuration(position)
print "Expected value:"
print("Expected value:")
displayHrp2Configuration(finalPosition)
print "Difference:"
displayHrp2Configuration(map(lambda (x, y): x - y,
zip(position, finalPosition)))
print("Difference:")
def diff(x, y):
return x - y
displayHrp2Configuration(map(diff, zip(position, finalPosition)))
sys.exit(1)
##################
# Initialization #
##################
import sys
if 'argv' in sys.__dict__:
from optparse import OptionParser
from dynamic_graph.sot.dynamics.solver import Solver
......@@ -134,29 +138,30 @@ if 'argv' in sys.__dict__:
# Parse options and enable robotviewer client if wanted.
clt = None
parser = OptionParser()
parser.add_option("-d", "--display",
action="store_true", dest="display", default=False,
parser.add_option("-d",
"--display",
action="store_true",
dest="display",
default=False,
help="enable display using robotviewer")
parser.add_option("-r", "--robot",
action="store", dest="robot", default="Hrp2Laas",
parser.add_option("-r",
"--robot",
action="store",
dest="robot",
default="Hrp2Laas",
help="Specify which robot model to use")
(options, args) = parser.parse_args()
if options.display:
if not hasRobotViewer:
print "Failed to import robotviewer client library."
print("Failed to import robotviewer client library.")
clt = initRobotViewer()
if not clt:
print "Failed to initialize robotviewer client library."
print("Failed to initialize robotviewer client library.")
# Initialize the stack of tasks.
robots = {
"Hrp2Laas" : Hrp2Laas,
"Hrp2Jrl" : Hrp2Jrl
}
if not options.robot in robots:
raise RuntimeError (
"invalid robot name (should by Hrp2Laas or Hrp2Jrl)")
robots = {"Hrp2Laas": Hrp2Laas, "Hrp2Jrl": Hrp2Jrl} # noqa TODO
if options.robot not in robots:
raise RuntimeError("invalid robot name (should by Hrp2Laas or Hrp2Jrl)")
robot = robots[options.robot]("robot")
solver = Solver(robot)
# flake8: noqa
# ______________________________________________________________________________
# ******************************************************************************
#
......@@ -5,54 +6,81 @@
#
# ______________________________________________________________________________
# ******************************************************************************
import pinocchio as pin
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.matrix_util import matrixToTuple
from dynamic_graph.sot.core.meta_tasks_kine import *
#from dynamic_graph.sot.core.utils.viewer_helper import addRobotViewer,VisualPinger,updateComDisplay
from dynamic_graph.sot.dynamics import *
# from dynamic_graph.sot.core.utils.viewer_helper import addRobotViewer,VisualPinger,updateComDisplay
from numpy import *
# Taking input from pinocchio
from pinocchio.romeo_wrapper import RomeoWrapper
set_printoptions(suppress=True, precision=7)
#-----------------------------------------------------------------------------
#---- ROBOT SPECIFICATIONS----------------------------------------------------
#-----------------------------------------------------------------------------
# -----------------------------------------------------------------------------
# ---- ROBOT SPECIFICATIONS----------------------------------------------------
# -----------------------------------------------------------------------------
#Define robotName, urdfpath and initialConfig
# Define robotName, urdfpath and initialConfig
#Taking input from pinocchio
from pinocchio.romeo_wrapper import RomeoWrapper
import pinocchio as pin
#SET THE PATH TO THE URDF AND MESHES
# SET THE PATH TO THE URDF AND MESHES
urdfPath = "~/git/sot/pinocchio/models/romeo.urdf"
urdfDir = ["~/git/sot/pinocchio/models"]
pinocchioRobot = RomeoWrapper(urdfPath, urdfDir, pin.JointModelFreeFlyer())
robotName = 'romeo'
pinocchioRobot.initDisplay(loadModel=True)
pinocchioRobot.display(pinocchioRobot.q0)
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
)
#-----------------------------------------------------------------------------
#---- PINOCCHIO MODEL AND DATA --------------------------------------------------------------------
#-----------------------------------------------------------------------------
#pinocchioModel = pin.buildModelFromUrdf(urdfpath, pin.JointModelFreeFlyer())
#pinocchioData = pinocchioModel.createData()
#-----------------------------------------------------------------------------
#---- DYN --------------------------------------------------------------------
#-----------------------------------------------------------------------------
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
)
# -----------------------------------------------------------------------------
# ---- PINOCCHIO MODEL AND DATA --------------------------------------------------------------------
# -----------------------------------------------------------------------------
# pinocchioModel = pin.buildModelFromUrdf(urdfpath, pin.JointModelFreeFlyer())
# pinocchioData = pinocchioModel.createData()
# -----------------------------------------------------------------------------
# ---- DYN --------------------------------------------------------------------
# -----------------------------------------------------------------------------
dyn = Dynamic("dyn")
dyn.setModel(pinocchioRobot.model)
dyn.setData(pinocchioRobot.data)
......@@ -60,37 +88,33 @@ dyn.setData(pinocchioRobot.data)
dyn.displayModel()
robotDim = dyn.getDimension()
inertiaRotor = (0,)*6+(5e-4,)*31
gearRatio = (0,)*6+(200,)*31
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.,)