Skip to content

Commit dc5e02d

Browse files
committed
blacken everything but skip string normalization
1 parent a75f3e1 commit dc5e02d

File tree

111 files changed

+1393
-1006
lines changed

Some content is hidden

Large Commits have some content hidden by default. Use the searchbox below for content that may be hidden.

111 files changed

+1393
-1006
lines changed

src/compas_fab/backends/__init__.py

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -153,6 +153,7 @@
153153
SphericalWristKinematics,
154154
CartesianMotionError,
155155
)
156+
156157
# Robot-specific analytic IK
157158
from .kinematics import (
158159
UR3Kinematics,

src/compas_fab/backends/interfaces/client.py

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -7,13 +7,15 @@ def forward_docstring(backend_feature):
77
def dec(obj):
88
obj.__doc__ = backend_feature.__dict__[obj.__name__].__doc__
99
return obj
10+
1011
return dec
1112

1213

1314
class ClientInterface(object):
1415
"""Interface for all backend clients. Forwards all planning services and
1516
planning scene management to the planner.
1617
"""
18+
1719
def __init__(self):
1820
self.planner = PlannerInterface(self)
1921
# self.control = ControlInterface()
@@ -70,6 +72,7 @@ def remove_attached_collision_mesh(self, *args, **kwargs):
7072
"""Forwards call to appropriate method in the planner."""
7173
return self.planner.remove_attached_collision_mesh(*args, **kwargs)
7274

75+
7376
# # ==========================================================================
7477
# # executing
7578
# # ==========================================================================
@@ -100,6 +103,7 @@ class PlannerInterface(object):
100103
behavior for all planning services and planning scene management methods. To be
101104
use in conjunction with backend feature interfaces.
102105
"""
106+
103107
def __init__(self, client):
104108
super(PlannerInterface, self).__init__()
105109
self.client = client

src/compas_fab/backends/kinematics/__init__.py

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -17,6 +17,7 @@
1717
Staubli_TX260LKinematics,
1818
ABB_IRB4600_40_255Kinematics,
1919
)
20+
2021
__all__ = [
2122
# exceptions
2223
'CartesianMotionError',

src/compas_fab/backends/kinematics/analytical_plan_cartesian_motion.py

Lines changed: 3 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -8,8 +8,7 @@
88

99

1010
class AnalyticalPlanCartesianMotion(PlanCartesianMotion):
11-
"""
12-
"""
11+
""" """
1312

1413
def __init__(self, client=None):
1514
self.client = client
@@ -73,7 +72,7 @@ def plan_cartesian_motion(self, robot, frames_WCF, start_configuration=None, gro
7372
path = paths[idx]
7473
path = self.smooth_configurations(path)
7574
trajectory = JointTrajectory()
76-
trajectory.fraction = len(path)/len(frames_RCF)
75+
trajectory.fraction = len(path) / len(frames_RCF)
7776
trajectory.joint_names = path[0].joint_names
7877
trajectory.points = [JointTrajectoryPoint(config.joint_values, config.joint_types) for config in path]
7978
trajectory.start_configuration = robot.merge_group_with_full_configuration(path[0], start_configuration, group)
@@ -88,7 +87,7 @@ def smooth_configurations(self, configurations):
8887
curr = configurations[i].joint_values
8988
corrected = []
9089
for p, c in zip(prev, curr):
91-
c1 = c/abs(c) * (abs(c) % (2 * math.pi))
90+
c1 = c / abs(c) * (abs(c) % (2 * math.pi))
9291
c2 = c1 - 2 * math.pi
9392
c3 = c1 + 2 * math.pi
9493
values = [c1, c2, c3]

src/compas_fab/backends/kinematics/solvers/offset_wrist.py

Lines changed: 5 additions & 28 deletions
Original file line numberDiff line numberDiff line change
@@ -40,34 +40,15 @@ def forward_kinematics_offset_wrist(joint_values, params):
4040
T[0] = c234 * c1 * s5 - c5 * s1
4141
T[1] = c6 * (s1 * s5 + c234 * c1 * c5) - s234 * c1 * s6
4242
T[2] = -s6 * (s1 * s5 + c234 * c1 * c5) - s234 * c1 * c6
43-
T[3] = (
44-
d6 * c234 * c1 * s5
45-
- a3 * c23 * c1
46-
- a2 * c1 * c2
47-
- d6 * c5 * s1
48-
- d5 * s234 * c1
49-
- d4 * s1
50-
)
43+
T[3] = d6 * c234 * c1 * s5 - a3 * c23 * c1 - a2 * c1 * c2 - d6 * c5 * s1 - d5 * s234 * c1 - d4 * s1
5144
T[4] = c1 * c5 + c234 * s1 * s5
5245
T[5] = -c6 * (c1 * s5 - c234 * c5 * s1) - s234 * s1 * s6
5346
T[6] = s6 * (c1 * s5 - c234 * c5 * s1) - s234 * c6 * s1
54-
T[7] = (
55-
d6 * (c1 * c5 + c234 * s1 * s5)
56-
+ d4 * c1
57-
- a3 * c23 * s1
58-
- a2 * c2 * s1
59-
- d5 * s234 * s1
60-
)
47+
T[7] = d6 * (c1 * c5 + c234 * s1 * s5) + d4 * c1 - a3 * c23 * s1 - a2 * c2 * s1 - d5 * s234 * s1
6148
T[8] = -s234 * s5
6249
T[9] = -c234 * s6 - s234 * c5 * c6
6350
T[10] = s234 * c5 * s6 - c234 * c6
64-
T[11] = (
65-
d1
66-
+ a3 * s23
67-
+ a2 * s2
68-
- d5 * (c23 * c4 - s23 * s4)
69-
- d6 * s5 * (c23 * s4 + s23 * c4)
70-
)
51+
T[11] = d1 + a3 * s23 + a2 * s2 - d5 * (c23 * c4 - s23 * s4) - d6 * s5 * (c23 * s4 + s23 * c4)
7152
T[15] = 1.0
7253

7354
frame = Frame((T[3], T[7], T[11]), (T[0], T[4], T[8]), (T[1], T[5], T[9]))
@@ -186,9 +167,7 @@ def inverse_kinematics_offset_wrist(frame, params, q6_des=0.0):
186167
if fabs(s5) < ZERO_THRESH:
187168
q6 = q6_des
188169
else:
189-
q6 = atan2(
190-
sign(s5) * -(T01 * s1 - T11 * c1), sign(s5) * (T00 * s1 - T10 * c1)
191-
)
170+
q6 = atan2(sign(s5) * -(T01 * s1 - T11 * c1), sign(s5) * (T00 * s1 - T10 * c1))
192171
if fabs(q6) < ZERO_THRESH:
193172
q6 = 0.0
194173
if q6 < 0.0:
@@ -199,9 +178,7 @@ def inverse_kinematics_offset_wrist(frame, params, q6_des=0.0):
199178

200179
c6 = cos(q6)
201180
s6 = sin(q6)
202-
x04x = -s5 * (T02 * c1 + T12 * s1) - c5 * (
203-
s6 * (T01 * c1 + T11 * s1) - c6 * (T00 * c1 + T10 * s1)
204-
)
181+
x04x = -s5 * (T02 * c1 + T12 * s1) - c5 * (s6 * (T01 * c1 + T11 * s1) - c6 * (T00 * c1 + T10 * s1))
205182
x04y = c5 * (T20 * c6 - T21 * s6) - T22 * s5
206183
p13x = (
207184
d5 * (s6 * (T00 * c1 + T10 * s1) + c6 * (T01 * c1 + T11 * s1))

src/compas_fab/backends/kinematics/solvers/spherical_wrist.py

Lines changed: 6 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -238,4 +238,9 @@ def inverse_kinematics_spherical_wrist(target_frame, points):
238238
axis6_angle = math.atan2(endy, endx)
239239
axis6_angles.append(axis6_angle)
240240

241-
return [[a1, a2, a3, a4, a5, a6] for a1, a2, a3, a4, a5, a6 in zip(axis1_angles, axis2_angles, axis3_angles, axis4_angles, axis5_angles, axis6_angles)]
241+
return [
242+
[a1, a2, a3, a4, a5, a6]
243+
for a1, a2, a3, a4, a5, a6 in zip(
244+
axis1_angles, axis2_angles, axis3_angles, axis4_angles, axis5_angles, axis6_angles
245+
)
246+
]

src/compas_fab/backends/kinematics/solvers/spherical_wrist_kinematics.py

Lines changed: 15 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -7,8 +7,7 @@
77

88

99
class SphericalWristKinematics(object):
10-
"""
11-
"""
10+
""" """
1211

1312
def __init__(self, points):
1413
self.points = points
@@ -31,11 +30,14 @@ def _pre_process(self, joint_values):
3130

3231
class Staubli_TX260LKinematics(SphericalWristKinematics):
3332
"""Analytical IK solver for the Stäubli TX2 60L robot."""
33+
3434
def __init__(self):
35-
points = [Point(0.000, 0.000, 0.375),
36-
Point(0.000, 0.020, 0.775),
37-
Point(0.450, 0.020, 0.775),
38-
Point(0.520, 0.020, 0.775)]
35+
points = [
36+
Point(0.000, 0.000, 0.375),
37+
Point(0.000, 0.020, 0.775),
38+
Point(0.450, 0.020, 0.775),
39+
Point(0.520, 0.020, 0.775),
40+
]
3941
super(Staubli_TX260LKinematics, self).__init__(points)
4042

4143
def _pre_process(self, joint_values):
@@ -57,11 +59,14 @@ def _post_process(self, solutions):
5759

5860
class ABB_IRB4600_40_255Kinematics(SphericalWristKinematics):
5961
"""Analytical IK solver for the ABB IRB4600 40/255 robot."""
62+
6063
def __init__(self):
61-
points = [Point(0.175, 0.000, 0.495),
62-
Point(0.175, 0.000, 1.590),
63-
Point(1.446, 0.000, 1.765),
64-
Point(1.581, 0.000, 1.765)]
64+
points = [
65+
Point(0.175, 0.000, 0.495),
66+
Point(0.175, 0.000, 1.590),
67+
Point(1.446, 0.000, 1.765),
68+
Point(1.581, 0.000, 1.765),
69+
]
6570
super(ABB_IRB4600_40_255Kinematics, self).__init__(points)
6671

6772
def _pre_process(self, joint_values):

src/compas_fab/backends/kinematics/utils.py

Lines changed: 1 addition & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -23,10 +23,7 @@ def fit_within_bounds(angle, lower, upper):
2323

2424
def joint_angles_to_configurations(robot, solutions, group=None):
2525
joint_names = robot.get_configurable_joint_names(group=group)
26-
return [
27-
Configuration.from_revolute_values(q, joint_names=joint_names) if q else None
28-
for q in solutions
29-
]
26+
return [Configuration.from_revolute_values(q, joint_names=joint_names) if q else None for q in solutions]
3027

3128

3229
def try_to_fit_configurations_between_bounds(robot, configurations, group=None):
Lines changed: 6 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -1,10 +1,10 @@
11
from __future__ import absolute_import
22

3-
from .client import * # noqa: F401,F403
4-
from .const import * # noqa: F401,F403
5-
from .conversions import * # noqa: F401,F403
6-
from .exceptions import * # noqa: F401,F403
7-
from .planner import * # noqa: F401,F403
8-
from .utils import * # noqa: F401,F403
3+
from .client import * # noqa: F401,F403
4+
from .const import * # noqa: F401,F403
5+
from .conversions import * # noqa: F401,F403
6+
from .exceptions import * # noqa: F401,F403
7+
from .planner import * # noqa: F401,F403
8+
from .utils import * # noqa: F401,F403
99

1010
__all__ = [name for name in dir() if not name.startswith('_')]

src/compas_fab/backends/pybullet/backend_features/__init__.py

Lines changed: 6 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -1,11 +1,15 @@
11
from __future__ import absolute_import
22

3-
from compas_fab.backends.pybullet.backend_features.pybullet_add_attached_collision_mesh import PyBulletAddAttachedCollisionMesh
3+
from compas_fab.backends.pybullet.backend_features.pybullet_add_attached_collision_mesh import (
4+
PyBulletAddAttachedCollisionMesh,
5+
)
46
from compas_fab.backends.pybullet.backend_features.pybullet_add_collision_mesh import PyBulletAddCollisionMesh
57
from compas_fab.backends.pybullet.backend_features.pybullet_append_collision_mesh import PyBulletAppendCollisionMesh
68
from compas_fab.backends.pybullet.backend_features.pybullet_forward_kinematics import PyBulletForwardKinematics
79
from compas_fab.backends.pybullet.backend_features.pybullet_inverse_kinematics import PyBulletInverseKinematics
8-
from compas_fab.backends.pybullet.backend_features.pybullet_remove_attached_collision_mesh import PyBulletRemoveAttachedCollisionMesh
10+
from compas_fab.backends.pybullet.backend_features.pybullet_remove_attached_collision_mesh import (
11+
PyBulletRemoveAttachedCollisionMesh,
12+
)
913
from compas_fab.backends.pybullet.backend_features.pybullet_remove_collision_mesh import PyBulletRemoveCollisionMesh
1014

1115

0 commit comments

Comments
 (0)