diff --git a/python/gym_jiminy/common/gym_jiminy/common/bases/compositions.py b/python/gym_jiminy/common/gym_jiminy/common/bases/compositions.py index 8ccda1d35..eb73aceca 100644 --- a/python/gym_jiminy/common/gym_jiminy/common/bases/compositions.py +++ b/python/gym_jiminy/common/gym_jiminy/common/bases/compositions.py @@ -8,7 +8,7 @@ """ from abc import ABC, abstractmethod from enum import Enum -from typing import Tuple, Sequence, Callable, Union, Optional, TypeVar +from typing import Tuple, Sequence, Callable, Union, Optional, Generic, TypeVar import numpy as np @@ -170,7 +170,7 @@ def __call__(self, terminated: bool, info: InfoType) -> float: return value -class QuantityReward(AbstractReward): +class QuantityReward(AbstractReward, Generic[ValueT]): """Convenience class making it easy to derive reward components from generic quantities. @@ -266,7 +266,7 @@ def compute(self, terminated: bool, info: InfoType) -> Optional[float]: return None # Evaluate raw quantity - value = self.env.quantities[self.name] + value = self.quantity.get() # Early return if quantity is None if value is None: @@ -426,13 +426,37 @@ class AbstractTerminationCondition(ABC): def __init__(self, env: InterfaceJiminyEnv, - name: str) -> None: + name: str, + grace_period: float = 0.0, + *, + is_truncation: bool = False, + is_training_only: bool = False) -> None: """ :param env: Base or wrapped jiminy environment. - :param name: Desired name of the reward. + :param name: Desired name of the termination condition. This name will + be used as key for storing the current episode state from + the perspective of this specific condition in 'info', and + to add the underlying quantity to the set of already + managed quantities by the environment. As a result, it + must be unique otherwise an exception will be raised. + :param grace_period: Grace period effective only at the very beginning + of the episode, during which the latter is bound + to continue whatever happens. + Optional: 0.0 by default. + :param is_truncation: Whether the episode should be considered + terminated or truncated whenever the termination + condition is triggered. + Optional: False by default. + :param is_training_only: Whether the termination condition should be + completely by-passed if the environment is in + evaluation mode. + Optional: False by default. """ self.env = env self._name = name + self.grace_period = grace_period + self.is_truncation = is_truncation + self.is_training_only = is_training_only @property def name(self) -> str: @@ -445,15 +469,12 @@ def name(self) -> str: return self._name @abstractmethod - def compute(self, info: InfoType) -> EpisodeState: - """Evaluate the termination condition. + def compute(self, info: InfoType) -> bool: + """Evaluate the termination condition at hands. :param info: Dictionary of extra information for monitoring. It will be updated in-place for storing terminated and truncated flags in 'info' as a tri-states `EpisodeState` value. - - :returns: Current episode state from the sole perspective of the - termination condition at hands. """ def __call__(self, info: InfoType) -> Tuple[bool, bool]: @@ -464,10 +485,10 @@ def __call__(self, info: InfoType) -> Tuple[bool, bool]: current state of the environment under the ongoing action. .. note:: - This method is a lightweight wrapper around `compute` to split the - episode state in two boolean flags to comply with Gym API. 'info' - will be updated to store either custom debug information if any - or the episode state otherwise. + This method is a lightweight wrapper around `compute` to return two + boolean flags 'terminated', 'truncated' complying with Gym API. + 'info' will be updated to store either custom debug information if + any, a tri-states episode state `EpisodeState` otherwise. .. warning:: This method is not meant to be overloaded. @@ -478,11 +499,17 @@ def __call__(self, info: InfoType) -> Tuple[bool, bool]: :returns: terminated and truncated flags. """ - # Evaluate the reward and store extra information + # Skip termination condition in eval mode or during grace period termination_info: InfoType = {} - episode_state = self.compute(termination_info) - is_terminated = episode_state == EpisodeState.TERMINATED - is_truncated = episode_state == EpisodeState.TRUNCATED + if (self.is_training_only and not self.env.is_training) or ( + self.env.stepper_state.t < self.grace_period): + # Always continue + is_terminated, is_truncated = False, False + else: + # Evaluate the reward and store extra information + is_done = self.compute(termination_info) + is_terminated = is_done and not self.is_truncation + is_truncated = is_done and self.is_truncation # Store episode state as info if self.name in info.keys(): @@ -492,13 +519,19 @@ def __call__(self, info: InfoType) -> Tuple[bool, bool]: if termination_info: info[self.name] = termination_info else: + if is_terminated: + episode_state = EpisodeState.TERMINATED + elif is_truncated: + episode_state = EpisodeState.TRUNCATED + else: + episode_state = EpisodeState.CONTINUED info[self.name] = episode_state # Returning terminated and truncated flags return is_terminated, is_truncated -class QuantityTermination(AbstractTerminationCondition): +class QuantityTermination(AbstractTerminationCondition, Generic[ValueT]): """Convenience class making it easy to derive termination conditions from generic quantities. @@ -547,14 +580,16 @@ def __init__(self, Optional: False by default. """ # Backup user argument(s) - self.low = np.asarray(low) if low is not None else None - self.high = np.asarray(high) if high is not None else None - self.grace_period = grace_period - self.is_truncation = is_truncation - self.is_training_only = is_training_only + self.low = low + self.high = high # Call base implementation - super().__init__(env, name) + super().__init__( + env, + name, + grace_period, + is_truncation=is_truncation, + is_training_only=is_training_only) # Add quantity to the set of quantities managed by the environment self.env.quantities[self.name] = quantity @@ -569,40 +604,26 @@ def __del__(self) -> None: # This method must not fail under any circumstances pass - def compute(self, info: InfoType) -> EpisodeState: + def compute(self, info: InfoType) -> bool: """Evaluate the termination condition. The underlying quantity is first evaluated. The episode continues if - its value is within bounds for all its components, otherwise the - episode is either truncated or terminated according to 'is_truncation'. + all the elements of its value are within bounds, otherwise the episode + is either truncated or terminated according to 'is_truncation'. .. warning:: This method is not meant to be overloaded. - - :returns: Current episode state from the sole perspective of the - termination condition at hands. """ - # Skip termination condition in eval mode or during grace period - if (self.is_training_only and not self.env.is_training) or ( - self.env.stepper_state.t < self.grace_period): - return EpisodeState.CONTINUED - # Evaluate the quantity - value = self.env.quantities[self.name] + value = self.quantity.get() - # Check if the quantity is within bound. + # Check if the quantity is out-of-bounds bound. # Note that it may be `None` if the quantity is ill-defined for the # current simulation state, which triggers termination unconditionally. - is_valid = value is not None - is_valid &= self.low is None or bool(np.all(self.low <= value)) - is_valid &= self.high is None or bool(np.all(value <= self.high)) - - # Determine the episode state to return - if is_valid: - return EpisodeState.CONTINUED - if self.is_truncation: - return EpisodeState.TRUNCATED - return EpisodeState.TERMINATED + is_done = value is None + is_done |= self.low is not None and bool(np.any(self.low > value)) + is_done |= self.high is not None and bool(np.any(value > self.high)) + return is_done QuantityTermination.name.__doc__ = \ diff --git a/python/gym_jiminy/common/gym_jiminy/common/bases/pipeline.py b/python/gym_jiminy/common/gym_jiminy/common/bases/pipeline.py index c4d93d503..e647c1a33 100644 --- a/python/gym_jiminy/common/gym_jiminy/common/bases/pipeline.py +++ b/python/gym_jiminy/common/gym_jiminy/common/bases/pipeline.py @@ -115,7 +115,8 @@ def __getattr__(self, name: str) -> Any: Calling this method in script mode while a simulation is already running would trigger a warning to avoid relying on it by mistake. """ - if self.is_simulation_running and not hasattr(sys, 'ps1'): + if (self.is_simulation_running and self.env.is_training and + not hasattr(sys, 'ps1')): # `hasattr(sys, 'ps1')` is used to detect whether the method was # called from an interpreter or within a script. For details, see: # https://stackoverflow.com/a/64523765/4820605 diff --git a/python/gym_jiminy/common/gym_jiminy/common/compositions/__init__.py b/python/gym_jiminy/common/gym_jiminy/common/compositions/__init__.py index 26c5e4357..289c68b6a 100644 --- a/python/gym_jiminy/common/gym_jiminy/common/compositions/__init__.py +++ b/python/gym_jiminy/common/gym_jiminy/common/compositions/__init__.py @@ -8,7 +8,8 @@ TrackingActuatedJointPositionsReward, SurviveReward, DriftTrackingQuantityTermination, - ShiftTrackingQuantityTermination) + ShiftTrackingQuantityTermination, + MechanicalSafetyTermination) from .locomotion import (TrackingBaseHeightReward, TrackingBaseOdometryVelocityReward, TrackingCapturePointReward, @@ -39,6 +40,7 @@ "SurviveReward", "DriftTrackingQuantityTermination", "ShiftTrackingQuantityTermination", + "MechanicalSafetyTermination", "BaseRollPitchTermination", "BaseHeightTermination", "FootCollisionTermination" diff --git a/python/gym_jiminy/common/gym_jiminy/common/compositions/generic.py b/python/gym_jiminy/common/gym_jiminy/common/compositions/generic.py index 14dd4097a..6267cc522 100644 --- a/python/gym_jiminy/common/gym_jiminy/common/compositions/generic.py +++ b/python/gym_jiminy/common/gym_jiminy/common/compositions/generic.py @@ -4,17 +4,21 @@ """ from operator import sub from functools import partial -from typing import Optional, Callable, TypeVar +from dataclasses import dataclass +from typing import Optional, Callable, Tuple, TypeVar import numpy as np import numba as nb +import pinocchio as pin from ..bases import ( - InfoType, QuantityCreator, InterfaceJiminyEnv, AbstractReward, - QuantityReward, QuantityEvalMode, QuantityTermination) + InfoType, QuantityCreator, InterfaceJiminyEnv, InterfaceQuantity, + AbstractQuantity, QuantityEvalMode, AbstractReward, QuantityReward, + AbstractTerminationCondition, QuantityTermination) from ..bases.compositions import ArrayOrScalar, ArrayLikeOrScalar from ..quantities import ( - StackedQuantity, UnaryOpQuantity, BinaryOpQuantity, ActuatedJointsPosition) + StackedQuantity, UnaryOpQuantity, BinaryOpQuantity, + MultiActuatedJointKinematic) from .mixin import radial_basis_function @@ -241,7 +245,7 @@ def __init__(self, is_training_only=is_training_only) -class ShiftTrackingQuantityTermination(QuantityTermination): +class ShiftTrackingQuantityTermination(QuantityTermination[np.ndarray]): """Base class to derive termination condition from the shift between the current and reference values of a given quantity. @@ -350,7 +354,7 @@ def min_norm(values: np.ndarray) -> float: name, shift_tracking_quantity, # type: ignore[arg-type] None, - thr, + np.array(thr), grace_period, is_truncation=is_truncation, is_training_only=is_training_only) @@ -376,3 +380,151 @@ def _compute_min_distance(self, passed as right-hand side of the binary operator 'op'. """ return self._min_norm(self.op(left, right)) + + +@dataclass(unsafe_hash=True) +class _MultiActuatedJointBoundDistance( + AbstractQuantity[Tuple[np.ndarray, np.ndarray]]): + """Distance of the actuated joints from their respective lower and upper + mechanical stops. + """ + + def __init__(self, + env: InterfaceJiminyEnv, + parent: Optional[InterfaceQuantity], + *, + mode: QuantityEvalMode = QuantityEvalMode.TRUE) -> None: + """ + :param env: Base or wrapped jiminy environment. + :param parent: Higher-level quantity from which this quantity is a + requirement if any, `None` otherwise. + :param mode: Desired mode of evaluation for this quantity. + """ + # Call base implementation + super().__init__( + env, + parent, + requirements=dict( + position=(MultiActuatedJointKinematic, dict( + kinematic_level=pin.KinematicLevel.POSITION))), + mode=mode, + auto_refresh=False) + + # Lower and upper bounds of the actuated joints + self.position_lower = np.array([]) + self.position_upper = np.array([]) + + def initialize(self) -> None: + # Call base implementation + super().initialize() + + # Initialize the motor position indices + quantity = self.requirements["position"] + quantity.initialize() + position_indices = quantity.kinematic_indices + + # Refresh mechanical joint position indices + position_limit_lower = self.robot.pinocchio_model.lowerPositionLimit + self.position_lower = position_limit_lower[position_indices] + position_limit_upper = self.robot.pinocchio_model.upperPositionLimit + self.position_upper = position_limit_upper[position_indices] + + def refresh(self) -> Tuple[np.ndarray, np.ndarray]: + return (self.position - self.position_lower, + self.position_upper - self.position) + + +class MechanicalSafetyTermination(AbstractTerminationCondition): + """Discouraging the agent from hitting the mechanical stops by immediately + terminating the episode if the articulated joints approach them at + excessive speed. + + Hitting the lower and upper mechanical stops is inconvenient but forbidding + it completely is not desirable as it induces safety margins that constrain + the problem too strictly. This is particularly true when the maximum motor + torque becomes increasingly limited and PD controllers are being used for + low-level motor control, which turns out to be the case in most instances. + Overall, such an hard constraint would impede performance while completing + the task successfully remains the highest priority. Still, the impact + velocity must be restricted to prevent destructive damage. It is + recommended to estimate an acceptable thresholdfrom real experimental data. + """ + def __init__(self, + env: InterfaceJiminyEnv, + position_margin: float, + velocity_max: float, + grace_period: float = 0.0, + *, + is_training_only: bool = False) -> None: + """ + :param env: Base or wrapped jiminy environment. + :param position_margin: Distance of actuated joints from their + respective mechanical bounds below which + their speed is being watched. + :param velocity_max: Maximum velocity above which further approaching + the mechanical stops triggers termination when + watched for being close from them. + :param grace_period: Grace period effective only at the very beginning + of the episode, during which the latter is bound + to continue whatever happens. + Optional: 0.0 by default. + :param is_training_only: Whether the termination condition should be + completely by-passed if the environment is in + evaluation mode. + Optional: False by default. + """ + # Backup user argument(s) + self.position_margin = position_margin + self.velocity_max = velocity_max + + # Call base implementation + super().__init__( + env, + "termination_mechanical_safety", + grace_period, + is_truncation=False, + is_training_only=is_training_only) + + # Add quantity to the set of quantities managed by the environment + self.env.quantities["_".join((self.name, "position_delta"))] = ( + _MultiActuatedJointBoundDistance, {}) + self.env.quantities["_".join((self.name, "velocity"))] = ( + MultiActuatedJointKinematic, dict( + kinematic_level=pin.KinematicLevel.VELOCITY)) + + # Keep track of the underlying quantities + registry = self.env.quantities.registry + self.position_delta = registry["_".join((self.name, "position_delta"))] + self.velocity = registry["_".join((self.name, "velocity"))] + + def __del__(self) -> None: + try: + for field in ("position_delta", "velocity"): + if hasattr(self, field): + del self.env.quantities["_".join((self.name, field))] + except Exception: # pylint: disable=broad-except + # This method must not fail under any circumstances + pass + + def compute(self, info: InfoType) -> bool: + """Evaluate the termination condition. + + The underlying quantity is first evaluated. The episode continues if + its value is within bounds, otherwise the episode is either truncated + or terminated according to 'is_truncation'. + + .. warning:: + This method is not meant to be overloaded. + """ + # Evaluate the quantity + position_delta_low, position_delta_high = self.position_delta.get() + velocity = self.velocity.get() + + # Check if the robot is going to hit the mechanical stops at high speed + is_done = any( + (position_delta_low < self.position_margin) & + (velocity < - self.velocity_max)) + is_done |= any( + (position_delta_high < self.position_margin) & + (velocity > self.velocity_max)) + return is_done diff --git a/python/gym_jiminy/common/gym_jiminy/common/quantities/generic.py b/python/gym_jiminy/common/gym_jiminy/common/quantities/generic.py index 34023792d..bfb811540 100644 --- a/python/gym_jiminy/common/gym_jiminy/common/quantities/generic.py +++ b/python/gym_jiminy/common/gym_jiminy/common/quantities/generic.py @@ -1582,11 +1582,18 @@ def initialize(self) -> None: # Call base implementation super().initialize() + # Make sure that the state data meet requirements + state = self.state + if ((self.kinematic_level == pin.ACCELERATION and state.a is None) or + (self.kinematic_level >= pin.VELOCITY and state.v is None)): + raise RuntimeError( + "Available state data do not meet requirements for kinematic " + f"level '{self.kinematic_level}'.") + # Refresh mechanical joint position indices self.kinematic_indices.clear() for motor in self.env.robot.motors: - joint_index = self.pinocchio_model.getJointId(motor.joint_name) - joint = self.pinocchio_model.joints[joint_index] + joint = self.pinocchio_model.joints[motor.joint_index] joint_type = jiminy.get_joint_type(joint) if joint_type == jiminy.JointModelType.ROTARY_UNBOUNDED: raise ValueError( @@ -1617,11 +1624,22 @@ def initialize(self) -> None: if self._must_refresh: self.data = np.full((len(self.kinematic_indices),), float("nan")) else: - self.data = self.state.q[slice(kin_first, kin_last + 1)] + if self.kinematic_level == pin.KinematicLevel.POSITION: + self.data = self.state.q[slice(kin_first, kin_last + 1)] + elif self.kinematic_level == pin.KinematicLevel.VELOCITY: + self.data = self.state.v[slice(kin_first, kin_last + 1)] + else: + self.data = self.state.a[slice(kin_first, kin_last + 1)] def refresh(self) -> np.ndarray: # Update mechanical joint positions only if necessary if self._must_refresh: - self.state.q.take(self.kinematic_indices, None, self.data, "clip") + if self.kinematic_level == pin.KinematicLevel.POSITION: + data = self.state.q + elif self.kinematic_level == pin.KinematicLevel.VELOCITY: + data = self.state.v + else: + data = self.state.a + data.take(self.kinematic_indices, None, self.data, "clip") return self.data diff --git a/python/gym_jiminy/common/gym_jiminy/common/quantities/locomotion.py b/python/gym_jiminy/common/gym_jiminy/common/quantities/locomotion.py index e57d7e009..1760a80ea 100644 --- a/python/gym_jiminy/common/gym_jiminy/common/quantities/locomotion.py +++ b/python/gym_jiminy/common/gym_jiminy/common/quantities/locomotion.py @@ -706,8 +706,8 @@ def __init__( :param env: Base or wrapped jiminy environment. :param parent: Higher-level quantity from which this quantity is a requirement if any, `None` otherwise. - :para kinematic_level: Desired kinematic level, ie position, velocity - or acceleration. + :param kinematic_level: Desired kinematic level, ie position, velocity + or acceleration. :param mode: Desired mode of evaluation for this quantity. Optional: 'QuantityEvalMode.TRUE' by default. """ diff --git a/python/gym_jiminy/common/gym_jiminy/common/quantities/transform.py b/python/gym_jiminy/common/gym_jiminy/common/quantities/transform.py index 603667e6d..fee9a79eb 100644 --- a/python/gym_jiminy/common/gym_jiminy/common/quantities/transform.py +++ b/python/gym_jiminy/common/gym_jiminy/common/quantities/transform.py @@ -438,7 +438,7 @@ class MultiAryOpQuantity(InterfaceQuantity[ValueT]): """Apply a given n-ary operator to the values of a given set of quantities. """ - quantities: Tuple[InterfaceQuantity[Any]] + quantities: Tuple[InterfaceQuantity[Any], ...] """Sequence of quantities that will be forwarded to the n-ary operator in this exact order. """ diff --git a/python/gym_jiminy/common/gym_jiminy/common/utils/math.py b/python/gym_jiminy/common/gym_jiminy/common/utils/math.py index f1ca42353..d425c610a 100644 --- a/python/gym_jiminy/common/gym_jiminy/common/utils/math.py +++ b/python/gym_jiminy/common/gym_jiminy/common/utils/math.py @@ -1189,7 +1189,7 @@ def quat_average(quat: np.ndarray, assert len(axes) > 0 and 0 not in axes q_perm = quat.transpose(( - *(i for i in range(1, quat.ndim) if i not in axes), 0, *axes)) + *[i for i in range(1, quat.ndim) if i not in axes], 0, *axes)) q_flat = q_perm.reshape((*q_perm.shape[:-len(axes)], -1)) _, eigvec = np.linalg.eigh(q_flat @ np.swapaxes(q_flat, -1, -2)) return np.moveaxis(eigvec[..., -1], -1, 0) diff --git a/python/gym_jiminy/toolbox/gym_jiminy/toolbox/math/qhull.py b/python/gym_jiminy/toolbox/gym_jiminy/toolbox/math/qhull.py index e5467b9ac..6572e4df6 100644 --- a/python/gym_jiminy/toolbox/gym_jiminy/toolbox/math/qhull.py +++ b/python/gym_jiminy/toolbox/gym_jiminy/toolbox/math/qhull.py @@ -354,7 +354,7 @@ def compute_convex_chebyshev_center( A = np.concatenate(( equations[:, :-1], np.ones((len(equations), 1))), axis=1) b = - equations[:, -1:] - c = np.array([*(0.0,) * (num_dims - 1), -1.0]) + c = np.array([*((0.0,) * (num_dims - 1)), -1.0]) res = linprog(c, A_ub=A, b_ub=b, bounds=(None, None)) return res.x[:-1], res.x[-1] diff --git a/python/gym_jiminy/unit_py/test_terminations.py b/python/gym_jiminy/unit_py/test_terminations.py index 3c8447575..d7cf80a56 100644 --- a/python/gym_jiminy/unit_py/test_terminations.py +++ b/python/gym_jiminy/unit_py/test_terminations.py @@ -18,7 +18,8 @@ ShiftTrackingQuantityTermination, BaseRollPitchTermination, BaseHeightTermination, - FootCollisionTermination) + FootCollisionTermination, + MechanicalSafetyTermination) class TerminationConditions(unittest.TestCase): @@ -257,16 +258,47 @@ def test_foot_collision(self): else: raise AssertionError("No collision detected.") - def test_misc(self): - """ TODO: Write documentation - """ - termination = BaseHeightTermination(self.env, 0.5) + def test_safety_limits(self): + POSITION_MARGIN, VELOCITY_MAX = 0.05, 1.0 + termination = MechanicalSafetyTermination(self.env, POSITION_MARGIN, VELOCITY_MAX) self.env.reset(seed=0) + + position_indices, velocity_indices = [], [] + pincocchio_model = self.env.robot.pinocchio_model + for motor in self.env.robot.motors: + joint = pincocchio_model.joints[motor.joint_index] + position_indices.append(joint.idx_q) + velocity_indices.append(joint.idx_v) + position_lower = pincocchio_model.lowerPositionLimit[position_indices] + position_lower += POSITION_MARGIN + position_upper = pincocchio_model.upperPositionLimit[position_indices] + position_upper -= POSITION_MARGIN + action = self.env.action_space.sample() for _ in range(20): _, _, terminated, _, _ = self.env.step(action) if terminated: break terminated, truncated = termination({}) - assert not truncated + position = self.env.robot_state.q[position_indices] + velocity = self.env.robot_state.v[velocity_indices] + is_valid = np.all( + (position_lower <= position) | (velocity >= - VELOCITY_MAX)) + is_valid = is_valid and np.all( + (position_upper >= position) | (velocity <= VELOCITY_MAX)) + assert terminated ^ is_valid + + def test_misc(self): + """ TODO: Write documentation + """ + for termination in ( + BaseHeightTermination(self.env, 0.5),): + self.env.reset(seed=0) + action = self.env.action_space.sample() + for _ in range(20): + _, _, terminated, _, _ = self.env.step(action) + if terminated: + break + terminated, truncated = termination({}) + assert not truncated diff --git a/python/jiminy_py/examples/collision_detection.py b/python/jiminy_py/examples/collision_detection.py index 423fb44d6..50e6061b7 100644 --- a/python/jiminy_py/examples/collision_detection.py +++ b/python/jiminy_py/examples/collision_detection.py @@ -25,9 +25,9 @@ def __init__(self, geom_model.getGeometryId, (geom_name_1, geom_name_2)) self.oMg1, self.oMg2 = ( geom_data.oMg[i] for i in (geom_index_1, geom_index_2)) - self.collide_functor = hppfcl.ComputeCollision(*( + self.collide_functor = hppfcl.ComputeCollision(*[ geom_model.geometryObjects[i].geometry - for i in (geom_index_1, geom_index_2))) + for i in (geom_index_1, geom_index_2)]) self.req = hppfcl.CollisionRequest() self.req.enable_cached_gjk_guess = True self.req.distance_upper_bound = 1e-6 diff --git a/python/jiminy_py/src/jiminy_py/viewer/viewer.py b/python/jiminy_py/src/jiminy_py/viewer/viewer.py index 4785ed12f..2b11964c5 100644 --- a/python/jiminy_py/src/jiminy_py/viewer/viewer.py +++ b/python/jiminy_py/src/jiminy_py/viewer/viewer.py @@ -1428,7 +1428,7 @@ def set_legend(labels: Optional[Sequence[str]] = None) -> None: for text, (robot_name, color) in zip( labels, Viewer._backend_robot_colors.items()): if color is not None: - rgba = (*(int(e * 255) for e in color[:3]), color[3]) + rgba = (*[int(e * 255) for e in color[:3]], color[3]) color_text = f"rgba({','.join(map(str, rgba))})" else: color_text = "black"