Skip to content
This repository was archived by the owner on Nov 28, 2025. It is now read-only.

Commit 1fd2005

Browse files
nim65snim65s
committed
[Python 3] Format
1 parent ddf649f commit 1fd2005

File tree

13 files changed

+544
-562
lines changed

13 files changed

+544
-562
lines changed

‎python/kine_romeo.py‎

Lines changed: 29 additions & 25 deletions
Original file line numberDiff line numberDiff line change
@@ -1,44 +1,49 @@
11
# -*- coding: utf-8 -*-
22
# Copyright 2011, Florent Lamiraux, Thomas Moulard, JRL, CNRS/AIST
3+
# flake8: noqa
34

4-
from dynamic_graph.sot.core.matrix_util import matrixToTuple, vectorToTuple,rotate, matrixToRPY
5+
# Binds with ROS. assert that roscore is running.
6+
from dynamic_graph.ros import *
7+
# Create a simple kinematic solver.
8+
from dynamic_graph.sot.application.velocity.precomputed_tasks import initialize
9+
from dynamic_graph.sot.core.matrix_util import matrixToRPY, matrixToTuple, rotate, vectorToTuple
510
from dynamic_graph.sot.core.meta_tasks_kine import *
6-
from numpy import *
7-
11+
# -------------------------------------------------------------------------------
12+
# ----- MAIN LOOP ---------------------------------------------------------------
13+
# -------------------------------------------------------------------------------
14+
# define the macro allowing to run the simulation.
15+
from dynamic_graph.sot.core.utils.thread_interruptible_loop import loopInThread, loopShortcuts
816
# Create the robot romeo.
917
from dynamic_graph.sot.romeo.robot import *
18+
from numpy import *
19+
1020
robot = Robot('romeo', device=RobotSimu('romeo'))
1121

12-
# Binds with ROS. assert that roscore is running.
13-
from dynamic_graph.ros import *
1422
ros = Ros(robot)
1523

16-
# Create a simple kinematic solver.
17-
from dynamic_graph.sot.application.velocity.precomputed_tasks import initialize
18-
solver = initialize ( robot )
24+
solver = initialize(robot)
25+
26+
dt = 5e-3
27+
1928

20-
#-------------------------------------------------------------------------------
21-
#----- MAIN LOOP ---------------------------------------------------------------
22-
#-------------------------------------------------------------------------------
23-
# define the macro allowing to run the simulation.
24-
from dynamic_graph.sot.core.utils.thread_interruptible_loop import loopInThread,loopShortcuts
25-
dt=5e-3
2629
@loopInThread
2730
def inc():
2831
robot.device.increment(dt)
2932

30-
runner=inc()
31-
[go,stop,next,n]=loopShortcuts(runner)
33+
34+
runner = inc()
35+
[go, stop, next, n] = loopShortcuts(runner)
3236

3337
# ---- TASKS -------------------------------------------------------------------
3438
# ---- TASKS -------------------------------------------------------------------
3539
# ---- TASKS -------------------------------------------------------------------
3640

3741
# ---- TASK GRIP ---
3842
# Defines a task for the right hand.
39-
taskRH=MetaTaskKine6d('rh',robot.dynamic,'right-wrist','right-wrist')
43+
taskRH = MetaTaskKine6d('rh', robot.dynamic, 'right-wrist', 'right-wrist')
4044
# Move the operational point.
41-
handMgrip=eye(4); handMgrip[0:3,3] = (0.1,0,0)
45+
handMgrip = eye(4)
46+
handMgrip[0:3, 3] = (0.1, 0, 0)
4247
taskRH.opmodif = matrixToTuple(handMgrip)
4348
taskRH.feature.frame('desired')
4449
# robot.tasks['right-wrist'].add(taskRH.feature.name)
@@ -51,12 +56,12 @@ def inc():
5156

5257
# --- CONTACTS
5358
# define contactLF and contactRF
54-
for name,joint in [ ['LF','left-ankle'], ['RF','right-ankle' ] ]:
55-
contact = MetaTaskKine6d('contact'+name,robot.dynamic,name,joint)
59+
for name, joint in [['LF', 'left-ankle'], ['RF', 'right-ankle']]:
60+
contact = MetaTaskKine6d('contact' + name, robot.dynamic, name, joint)
5661
contact.feature.frame('desired')
5762
contact.gain.setConstant(10)
5863
contact.keep()
59-
locals()['contact'+name] = contact
64+
locals()['contact' + name] = contact
6065

6166
# --- RUN ----------------------------------------------------------------------
6267

@@ -65,16 +70,15 @@ def inc():
6570
# 2st param: objective
6671
# 3rd param: selected parameters
6772
# 4th param: gain
68-
target=(0.5,-0.2,0.8)
69-
gotoNd(taskRH,target,'111',(4.9,0.9,0.01,0.9))
73+
target = (0.5, -0.2, 0.8)
74+
gotoNd(taskRH, target, '111', (4.9, 0.9, 0.01, 0.9))
7075

7176
# Fill the stack with some tasks.
7277
solver.push(contactRF.task)
7378
solver.push(contactLF.task)
74-
solver.push (robot.tasks ['com'])
79+
solver.push(robot.tasks['com'])
7580

7681
solver.push(taskRH.task)
7782

7883
# type inc to run one iteration, or go to run indefinitely.
7984
# go()
80-

‎python/kine_romeo_small.py‎

Lines changed: 29 additions & 25 deletions
Original file line numberDiff line numberDiff line change
@@ -1,44 +1,49 @@
11
# -*- coding: utf-8 -*-
22
# Copyright 2011, Florent Lamiraux, Thomas Moulard, JRL, CNRS/AIST
3+
# flake8: noqa
34

4-
from dynamic_graph.sot.core.matrix_util import matrixToTuple, vectorToTuple,rotate, matrixToRPY
5+
# Binds with ROS. assert that roscore is running.
6+
from dynamic_graph.ros import *
7+
# Create a simple kinematic solver.
8+
from dynamic_graph.sot.application.velocity.precomputed_tasks import initialize
9+
from dynamic_graph.sot.core.matrix_util import matrixToRPY, matrixToTuple, rotate, vectorToTuple
510
from dynamic_graph.sot.core.meta_tasks_kine import *
6-
from numpy import *
7-
11+
# -------------------------------------------------------------------------------
12+
# ----- MAIN LOOP ---------------------------------------------------------------
13+
# -------------------------------------------------------------------------------
14+
# define the macro allowing to run the simulation.
15+
from dynamic_graph.sot.core.utils.thread_interruptible_loop import loopInThread, loopShortcuts
816
# Create the robot romeo.
917
from dynamic_graph.sot.romeo.robot import *
18+
from numpy import *
19+
1020
robot = Robot('romeo_small', device=RobotSimu('romeo_small'))
1121

12-
# Binds with ROS. assert that roscore is running.
13-
from dynamic_graph.ros import *
1422
ros = Ros(robot)
1523

16-
# Create a simple kinematic solver.
17-
from dynamic_graph.sot.application.velocity.precomputed_tasks import initialize
18-
solver = initialize ( robot )
24+
solver = initialize(robot)
25+
26+
dt = 5e-3
27+
1928

20-
#-------------------------------------------------------------------------------
21-
#----- MAIN LOOP ---------------------------------------------------------------
22-
#-------------------------------------------------------------------------------
23-
# define the macro allowing to run the simulation.
24-
from dynamic_graph.sot.core.utils.thread_interruptible_loop import loopInThread,loopShortcuts
25-
dt=5e-3
2629
@loopInThread
2730
def inc():
2831
robot.device.increment(dt)
2932

30-
runner=inc()
31-
[go,stop,next,n]=loopShortcuts(runner)
33+
34+
runner = inc()
35+
[go, stop, next, n] = loopShortcuts(runner)
3236

3337
# ---- TASKS -------------------------------------------------------------------
3438
# ---- TASKS -------------------------------------------------------------------
3539
# ---- TASKS -------------------------------------------------------------------
3640

3741
# ---- TASK GRIP ---
3842
# Defines a task for the right hand.
39-
taskRH=MetaTaskKine6d('rh',robot.dynamic,'right-wrist','right-wrist')
43+
taskRH = MetaTaskKine6d('rh', robot.dynamic, 'right-wrist', 'right-wrist')
4044
# Move the operational point.
41-
handMgrip=eye(4); handMgrip[0:3,3] = (0.1,0,0)
45+
handMgrip = eye(4)
46+
handMgrip[0:3, 3] = (0.1, 0, 0)
4247
taskRH.opmodif = matrixToTuple(handMgrip)
4348
taskRH.feature.frame('desired')
4449
# robot.tasks['right-wrist'].add(taskRH.feature.name)
@@ -51,12 +56,12 @@ def inc():
5156

5257
# --- CONTACTS
5358
# define contactLF and contactRF
54-
for name,joint in [ ['LF','left-ankle'], ['RF','right-ankle' ] ]:
55-
contact = MetaTaskKine6d('contact'+name,robot.dynamic,name,joint)
59+
for name, joint in [['LF', 'left-ankle'], ['RF', 'right-ankle']]:
60+
contact = MetaTaskKine6d('contact' + name, robot.dynamic, name, joint)
5661
contact.feature.frame('desired')
5762
contact.gain.setConstant(10)
5863
contact.keep()
59-
locals()['contact'+name] = contact
64+
locals()['contact' + name] = contact
6065

6166
# --- RUN ----------------------------------------------------------------------
6267

@@ -65,16 +70,15 @@ def inc():
6570
# 2st param: objective
6671
# 3rd param: selected parameters
6772
# 4th param: gain
68-
target=(0.5,-0.2,0.8)
69-
gotoNd(taskRH,target,'111',(4.9,0.9,0.01,0.9))
73+
target = (0.5, -0.2, 0.8)
74+
gotoNd(taskRH, target, '111', (4.9, 0.9, 0.01, 0.9))
7075

7176
# Fill the stack with some tasks.
7277
solver.push(contactRF.task)
7378
solver.push(contactLF.task)
74-
solver.push (robot.tasks ['com'])
79+
solver.push(robot.tasks['com'])
7580

7681
solver.push(taskRH.task)
7782

7883
# type inc to run one iteration, or go to run indefinitely.
7984
# go()
80-

‎src/dynamic_graph/sot/dynamics_pinocchio/__init__.py‎

Lines changed: 33 additions & 33 deletions
Original file line numberDiff line numberDiff line change
@@ -1,74 +1,74 @@
1-
from dynamic import DynamicPinocchio as DynamicCpp
2-
from angle_estimator import AngleEstimator
3-
from zmp_from_forces import ZmpFromForces
41
import numpy as np
5-
from numpy import arctan2, arcsin, sin, cos, sqrt
2+
from numpy import cos, sin, sqrt
3+
4+
from dynamic import DynamicPinocchio as DynamicCpp
65

7-
#DynamicOld = Dynamic
6+
# DynamicOld = Dynamic
87

9-
class DynamicPinocchio (DynamicCpp):
8+
9+
class DynamicPinocchio(DynamicCpp):
1010
def __init__(self, name):
1111
DynamicCpp.__init__(self, name)
1212
self.model = None
1313
self.data = None
1414

1515
def setData(self, pinocchio_data):
16-
dynamic.wrap.set_pinocchio_data(self.obj,pinocchio_data)
16+
dynamic.wrap.set_pinocchio_data(self.obj, pinocchio_data) # noqa TODO
1717
self.data = pinocchio_data
1818
return
19-
19+
2020
def setModel(self, pinocchio_model):
21-
dynamic.wrap.set_pinocchio_model(self.obj,pinocchio_model)
21+
dynamic.wrap.set_pinocchio_model(self.obj, pinocchio_model) # noqa TODO
2222
self.model = pinocchio_model
2323
return
2424

25+
2526
def fromSotToPinocchio(q_sot, freeflyer=True):
2627
if freeflyer:
27-
[r,p,y] = q_sot[3:6]
28+
[r, p, y] = q_sot[3:6]
2829
cr = cos(r)
2930
cp = cos(p)
3031
cy = cos(y)
3132
sr = sin(r)
3233
sp = sin(p)
3334
sy = sin(y)
3435

35-
rotmat = np.matrix([[cy*cp, cy*sp*sr-sy*cr, cy*sp*cr+sy*sr],
36-
[sy*cp, sy*sp*sr+cy*cr, sy*sp*cr-cy*sr],
37-
[-sp, cp*sr, cp*cr]])
36+
rotmat = np.matrix([[cy * cp, cy * sp * sr - sy * cr, cy * sp * cr + sy * sr],
37+
[sy * cp, sy * sp * sr + cy * cr, sy * sp * cr - cy * sr], [-sp, cp * sr, cp * cr]])
3838

39-
d0 = rotmat[0,0]
40-
d1 = rotmat[1,1]
41-
d2 = rotmat[2,2]
42-
rr = 1.0+d0+d1+d2
39+
d0 = rotmat[0, 0]
40+
d1 = rotmat[1, 1]
41+
d2 = rotmat[2, 2]
42+
rr = 1.0 + d0 + d1 + d2
4343

44-
if rr>0:
44+
if rr > 0:
4545
s = 0.5 / sqrt(rr)
46-
_x = (rotmat[2,1] - rotmat[1,2]) * s
47-
_y = (rotmat[0,2] - rotmat[2,0]) * s
48-
_z = (rotmat[1,0] - rotmat[0,1]) * s
46+
_x = (rotmat[2, 1] - rotmat[1, 2]) * s
47+
_y = (rotmat[0, 2] - rotmat[2, 0]) * s
48+
_z = (rotmat[1, 0] - rotmat[0, 1]) * s
4949
_r = 0.25 / s
5050
else:
51-
#Trace is less than zero, so need to determine which
52-
#major diagonal is largest
51+
# Trace is less than zero, so need to determine which
52+
# major diagonal is largest
5353
if ((d0 > d1) and (d0 > d2)):
5454
s = 0.5 / sqrt(1 + d0 - d1 - d2)
5555
_x = 0.5 * s
56-
_y = (rotmat[0,1] + rotmat[1,0]) * s
57-
_z = (rotmat[0,2] + rotmat[2,0]) * s
58-
_r = (rotmat[1,2] + rotmat[2,1]) * s
56+
_y = (rotmat[0, 1] + rotmat[1, 0]) * s
57+
_z = (rotmat[0, 2] + rotmat[2, 0]) * s
58+
_r = (rotmat[1, 2] + rotmat[2, 1]) * s
5959
elif (d1 > d2):
6060
s = 0.5 / sqrt(1 + d0 - d1 - d2)
61-
_x = (rotmat[0,1] + rotmat[1,0]) * s
61+
_x = (rotmat[0, 1] + rotmat[1, 0]) * s
6262
_y = 0.5 * s
63-
_z = (rotmat[1,2] + rotmat[2,1]) * s
64-
_r = (rotmat[0,2] + rotmat[2,0]) * s
63+
_z = (rotmat[1, 2] + rotmat[2, 1]) * s
64+
_r = (rotmat[0, 2] + rotmat[2, 0]) * s
6565
else:
6666
s = 0.5 / sqrt(1 + d0 - d1 - d2)
67-
_x = (rotmat[0,2] + rotmat[2,0]) * s
68-
_y = (rotmat[1,2] + rotmat[2,1]) * s
67+
_x = (rotmat[0, 2] + rotmat[2, 0]) * s
68+
_y = (rotmat[1, 2] + rotmat[2, 1]) * s
6969
_z = 0.5 * s
70-
_r = (rotmat[0,1] + rotmat[1,0]) * s
70+
_r = (rotmat[0, 1] + rotmat[1, 0]) * s
7171

72-
return np.matrix([q_sot[0:3]+(_x,_y,_z,_r)+q_sot[6:]])
72+
return np.matrix([q_sot[0:3] + (_x, _y, _z, _r) + q_sot[6:]])
7373
else:
7474
return np.matrix([q_sot[0:]])

0 commit comments

Comments
 (0)