Skip to content

Commit

Permalink
pre-commit run -a
Browse files Browse the repository at this point in the history
  • Loading branch information
nim65s committed Oct 4, 2024
1 parent 87e94eb commit b23ac5a
Show file tree
Hide file tree
Showing 13 changed files with 35 additions and 32 deletions.
10 changes: 5 additions & 5 deletions toolbox_parallel_robots/__init__.py
Original file line number Diff line number Diff line change
@@ -1,7 +1,7 @@
# ruff: noqa: F401
from .actuation_model import ActuationModel
from .actuation import mergeq, mergev, qfree, qmot, vfree, vmot
from .actuation_data import ActuationData
from .actuation import qfree, qmot, vfree, vmot, mergeq, mergev
from .actuation_model import ActuationModel
from .closures import partialLoopClosure, partialLoopClosureFrames
from .constraints import (
constraintResidual,
Expand All @@ -19,14 +19,14 @@
from .inverse_kinematics import (
closedLoopInverseKinematics,
closedLoopInverseKinematicsCasadi,
closedLoopInverseKinematicsScipy,
closedLoopInverseKinematicsProximal,
closedLoopInverseKinematicsScipy,
)
from .jacobian import (
separateConstraintJacobian,
computeClosedLoopFrameJacobian,
computeDerivative_dq_dqmot,
inverseConstraintKinematicsSpeed,
separateConstraintJacobian,
)
from .mounting import (
closedLoopMount,
Expand All @@ -35,8 +35,8 @@
closedLoopMountScipy,
)
from .projections import (
accelerationProjection,
configurationProjection,
configurationProjectionProximal,
velocityProjection,
accelerationProjection,
)
2 changes: 1 addition & 1 deletion toolbox_parallel_robots/actuation_data.py
Original file line number Diff line number Diff line change
@@ -1,5 +1,5 @@
import pinocchio as pin
import numpy as np
import pinocchio as pin


class ActuationData:
Expand Down
7 changes: 4 additions & 3 deletions toolbox_parallel_robots/closures.py
Original file line number Diff line number Diff line change
Expand Up @@ -6,11 +6,12 @@
It is used to get configurations that minimizes the pinocchio constraints residuals under joints configurations constraints.
"""

import pinocchio as pin
from warnings import warn

import casadi
import numpy as np
import pinocchio as pin
from pinocchio import casadi as caspin
import casadi
from warnings import warn

from .constraints import constraintsResidual

Expand Down
4 changes: 2 additions & 2 deletions toolbox_parallel_robots/constraints.py
Original file line number Diff line number Diff line change
Expand Up @@ -5,9 +5,9 @@
Tool functions to compute the constraints residuals from a robot constraint model.
"""

import pinocchio as pin
import numpy as np
import casadi
import numpy as np
import pinocchio as pin


## Constraints residuals
Expand Down
8 changes: 4 additions & 4 deletions toolbox_parallel_robots/forward_kinematics.py
Original file line number Diff line number Diff line change
Expand Up @@ -5,21 +5,21 @@
This module provides tools to perform the forward kinematics of a closed-loop system.
"""

import pinocchio as pin
import numpy as np
import pinocchio as pin

try:
from pinocchio import casadi as caspin
import casadi
from pinocchio import casadi as caspin

_WITH_CASADI = True
except ImportError:
_WITH_CASADI = False
from scipy.optimize import fmin_slsqp
from numpy.linalg import norm
from scipy.optimize import fmin_slsqp

from .constraints import constraintsResidual
from .actuation import mergeq, mergev
from .constraints import constraintsResidual


def closedLoopForwardKinematicsCasadi(
Expand Down
2 changes: 1 addition & 1 deletion toolbox_parallel_robots/inverse_dynamics.py
Original file line number Diff line number Diff line change
Expand Up @@ -5,8 +5,8 @@
This module provides tools to perform the inverse dynamics of a closed-loop system.
"""

import pinocchio as pin
import numpy as np
import pinocchio as pin
from qpsolvers import solve_qp


Expand Down
8 changes: 4 additions & 4 deletions toolbox_parallel_robots/inverse_kinematics.py
Original file line number Diff line number Diff line change
Expand Up @@ -5,18 +5,18 @@
Tools to compute the inverse kinematics of a closed loop system.
"""

import pinocchio as pin
import numpy as np
import pinocchio as pin

try:
from pinocchio import casadi as caspin
import casadi
from pinocchio import casadi as caspin

_WITH_CASADI = True
except ImportError:
_WITH_CASADI = False
from scipy.optimize import fmin_slsqp
from numpy.linalg import norm
from scipy.optimize import fmin_slsqp

from .constraints import constraintsResidual

Expand Down Expand Up @@ -289,7 +289,7 @@ def closedLoopInverseKinematicsProximal(
for cm in constraint_model:
constraint_dim += cm.size()

y = np.ones((constraint_dim))
y = np.ones(constraint_dim)
data.M = np.eye(model.nv) * rho
kkt_constraint = pin.ContactCholeskyDecomposition(model, constraint_model)

Expand Down
2 changes: 1 addition & 1 deletion toolbox_parallel_robots/jacobian.py
Original file line number Diff line number Diff line change
Expand Up @@ -6,8 +6,8 @@
"""

import pinocchio as pin
import numpy as np
import pinocchio as pin


def separateConstraintJacobian(actuation_data, Jn):
Expand Down
8 changes: 4 additions & 4 deletions toolbox_parallel_robots/mounting.py
Original file line number Diff line number Diff line change
Expand Up @@ -6,18 +6,18 @@
Contains three methods to solve this problem, methode selection is done by setting global variables or through imports
"""

import pinocchio as pin
import numpy as np
import pinocchio as pin

try:
from pinocchio import casadi as caspin
import casadi
from pinocchio import casadi as caspin

_WITH_CASADI = True
except ImportError:
_WITH_CASADI = False
from scipy.optimize import fmin_slsqp
from numpy.linalg import norm
from scipy.optimize import fmin_slsqp

from .constraints import constraintsResidual

Expand Down Expand Up @@ -229,7 +229,7 @@ def closedLoopMountProximal(
for cm in cmodels:
constraint_dim += cm.size()

y = np.ones((constraint_dim))
y = np.ones(constraint_dim)
rdata.M = np.eye(rmodel.nv) * rho
kkt_constraint = pin.ContactCholeskyDecomposition(rmodel, cmodels)

Expand Down
6 changes: 3 additions & 3 deletions toolbox_parallel_robots/projections.py
Original file line number Diff line number Diff line change
Expand Up @@ -6,10 +6,10 @@
Contains three methods to solve this problem, methode selection is done by setting global variables or through imports
"""

import pinocchio as pin
import casadi
import numpy as np
import pinocchio as pin
from pinocchio import casadi as caspin
import casadi
from qpsolvers import solve_qp

from .constraints import constraintsResidual
Expand Down Expand Up @@ -72,7 +72,7 @@ def configurationProjectionProximal(
for cm in cmodels:
constraint_dim += cm.size()

y = np.ones((constraint_dim))
y = np.ones(constraint_dim)
rdata.M = np.eye(rmodel.nv) * rho
kkt_constraint = pin.ContactCholeskyDecomposition(rmodel, cmodels)

Expand Down
5 changes: 3 additions & 2 deletions toolbox_parallel_robots/sliders/foo.py
Original file line number Diff line number Diff line change
Expand Up @@ -6,8 +6,9 @@
"""

import meshcat
import tkinter as tk

import meshcat
import pinocchio as pin
from toolbox_parallel_robots.tk_robot_sliders import SlidersFrame
from toolbox_parallel_robots.tk_sliders_manager import SlidersManager
Expand Down Expand Up @@ -71,9 +72,9 @@ def createSlidersInterface(
# constraint_models = []
# mot_ids_q = [model.getJointId(joint_name) for joint_name in ["FL_HAA", "FL_HFE", "FL_KFE", "FR_HAA", "FR_HFE", "FR_KFE", "HL_HAA", "HL_HFE", "HL_KFE", "HR_HAA", "HR_HFE", "HR_KFE"]]
# * Create the visualizer
import meshcat
import pinocchio as pin
from pinocchio.visualize import MeshcatVisualizer
import meshcat

viz = MeshcatVisualizer(model, collision_model, visual_model)
viz.viewer = meshcat.Visualizer(zmq_url="tcp://127.0.0.1:6000")
Expand Down
3 changes: 2 additions & 1 deletion toolbox_parallel_robots/sliders/tk_robot_sliders.py
Original file line number Diff line number Diff line change
@@ -1,7 +1,8 @@
import tkinter as tk
from functools import partial

import numpy as np
import pinocchio as pin
from functools import partial


class SlidersFrame:
Expand Down
2 changes: 1 addition & 1 deletion toolbox_parallel_robots/sliders/util_frames.py
Original file line number Diff line number Diff line change
@@ -1,5 +1,5 @@
import pinocchio as pin
import hppfcl
import pinocchio as pin


def addXYZAxisToFrames(rm, vm, basename="XYZ", scale=1, world=False):
Expand Down

0 comments on commit b23ac5a

Please sign in to comment.