diff --git a/python/smc/robots/implementations/heron.py b/python/smc/robots/implementations/heron.py index f6f4da313fae979b9161e6f4bf1558e7ab16230d..a8b770b3f1aa2e5ebf0d0d2bafa581e816f33dcd 100644 --- a/python/smc/robots/implementations/heron.py +++ b/python/smc/robots/implementations/heron.py @@ -283,7 +283,9 @@ class RealHeronRobotManager(AbstractHeronRobotManager, AbstractRealRobotManager) def heron_approximation(): # arm + gripper - model_arm, collision_model_arm, visual_model_arm, _ = get_model() + model_arm, collision_model_arm, visual_model_arm, _ = get_model( + with_gripper_joints=True + ) # mobile base as planar joint (there's probably a better # option but whatever right now) @@ -297,6 +299,7 @@ def heron_approximation(): 1, pin.SE3.Identity(), ) + model = pin.buildReducedModel(model, [8, 9], np.zeros(model.nq)) data = model.createData() # fix gripper diff --git a/python/smc/robots/implementations/ur5e.py b/python/smc/robots/implementations/ur5e.py index a7ca94570b54b736156575e545fb6bc125414802..af934b8b2e3cd67474889c0bc715373cb4dc0cac 100644 --- a/python/smc/robots/implementations/ur5e.py +++ b/python/smc/robots/implementations/ur5e.py @@ -178,7 +178,9 @@ class RealUR5eRobotManager(AbstractUR5eRobotManager, AbstractRealRobotManager): self._rtde_control.endFreedriveMode() -def get_model() -> tuple[pin.Model, pin.GeometryModel, pin.GeometryModel, pin.Data]: +def get_model( + with_gripper_joints=False, +) -> tuple[pin.Model, pin.GeometryModel, pin.GeometryModel, pin.Data]: urdf_path_relative = files("smc.robots.robot_descriptions.urdf").joinpath( "ur5e_with_robotiq_hande_FIXED_PATHS.urdf" @@ -259,7 +261,8 @@ def get_model() -> tuple[pin.Model, pin.GeometryModel, pin.GeometryModel, pin.Da model.jointPlacements[5] = wrist_2_se3 model.jointPlacements[6] = wrist_3_se3 # TODO: fix where the fingers end up by setting a better position here (or maybe not here idk) - model = pin.buildReducedModel(model, [7, 8], np.zeros(model.nq)) + if not with_gripper_joints: + model = pin.buildReducedModel(model, [7, 8], np.zeros(model.nq)) data = pin.Data(model) return model, collision_model, visual_model, data