Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Fixed: fail to turn due to low force limit #28

Open
wants to merge 1 commit into
base: main
Choose a base branch
from
Open

Fixed: fail to turn due to low force limit #28

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
4 changes: 4 additions & 0 deletions doc/changelog.md
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,5 +1,9 @@
# Changelog

## 2.2.11

- Fixed: The Magnebot fails to move or turn when its torso is higher than the default height. `WheelMotion` now sets force limits based on the height of the torso if `set_torso=False`.

## 2.2.10

- Fixed: Spherecast in `grasp` always targets the object at index 0 instead of the target object.
Expand Down
55 changes: 50 additions & 5 deletions magnebot/actions/wheel_motion.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -9,14 +9,18 @@
from magnebot.magnebot_static import MagnebotStatic
from magnebot.magnebot_dynamic import MagnebotDynamic
from magnebot.collision_detection import CollisionDetection
from magnebot.constants import DEFAULT_WHEEL_FRICTION
from magnebot.wheel import Wheel
from magnebot.constants import DEFAULT_WHEEL_FRICTION, COLUMN_Y


class WheelMotion(Action, ABC):
"""
Abstract base class for a motion action involving the Magnebot's wheels.
"""

# If `set_torso=False`, use these force limits (element 1), depending on the height of the torso (element 0).
TORSO_FORCE_LIMITS: List[List[int]] = [[0.6, 1.02], [0.7, 1.04], [0.8, 1.05], [0.9, 1.07], [1.0, 1.06]]

def __init__(self, dynamic: MagnebotDynamic, collision_detection: CollisionDetection, set_torso: bool,
previous: Action = None):
"""
Expand All @@ -43,6 +47,8 @@ def __init__(self, dynamic: MagnebotDynamic, collision_detection: CollisionDetec
self.status = ActionStatus.collision
elif previous.status == ActionStatus.tipping:
self.status = ActionStatus.tipping
self._default_wheel_force_limit: float = -1
self._wheel_force_limit: float = -1

@final
def get_initialization_commands(self, resp: List[bytes], static: MagnebotStatic, dynamic: MagnebotDynamic,
Expand All @@ -58,12 +64,37 @@ def get_initialization_commands(self, resp: List[bytes], static: MagnebotStatic,

commands: List[dict] = super().get_initialization_commands(resp=resp, image_frequency=image_frequency,
static=static, dynamic=dynamic)
# Make the robot moveable.
# Set force limits based on the height of the torso.
if not self._set_torso:
torso_y = round(float(dynamic.joints[static.arm_joints[ArmJoint.torso]].position[1]) + COLUMN_Y, 1)
# For heights below this value, the default force limit will work.
if torso_y >= WheelMotion.TORSO_FORCE_LIMITS[0][0]:
force_limit = 0
# Read the list of torso heights.
if torso_y < 1:
for forces in WheelMotion.TORSO_FORCE_LIMITS:
if abs(forces[0] - torso_y) < 0.1:
force_limit = forces[1]
break
# If the torso height is above 1, this value will always work.
else:
force_limit = WheelMotion.TORSO_FORCE_LIMITS[-1][1]
self._default_wheel_force_limit = static.joints[static.wheels[Wheel.wheel_left_front]].drives["x"].force_limit
self._wheel_force_limit = self._default_wheel_force_limit * force_limit
for wheel in static.wheels:
drive = static.joints[static.wheels[wheel]].drives["x"]
commands.append({"$type": "set_robot_joint_drive",
"joint_id": static.wheels[wheel],
"force_limit": self._wheel_force_limit,
"stiffness": drive.stiffness,
"damping": drive.damping,
"id": static.robot_id})
# Make the robot movable.
if dynamic.immovable:
self._resetting = True
commands.append({"$type": "set_immovable",
"id": static.robot_id,
"immovable": False})
"id": static.robot_id,
"immovable": False})
# Maybe reset the torso.
if self._set_torso:
commands.append({"$type": "set_prismatic_target",
Expand All @@ -78,9 +109,13 @@ def get_initialization_commands(self, resp: List[bytes], static: MagnebotStatic,
# Reset the drive values.
for wheel_id in static.wheels.values():
drive = static.joints[wheel_id].drives["x"]
if self._set_torso:
force_limit = drive.force_limit
else:
force_limit = self._wheel_force_limit
commands.extend([{"$type": "set_robot_joint_drive",
"joint_id": wheel_id,
"force_limit": drive.force_limit,
"force_limit": force_limit,
"stiffness": drive.stiffness,
"damping": drive.damping,
"id": static.robot_id},
Expand Down Expand Up @@ -127,6 +162,16 @@ def get_end_commands(self, resp: List[bytes], static: MagnebotStatic, dynamic: M
commands = self._get_stop_wheels_commands(static=static, dynamic=dynamic)
commands.extend(super().get_end_commands(resp=resp, static=static, dynamic=dynamic,
image_frequency=image_frequency))
# Reset the force limits.
if not self._set_torso:
for wheel_id in static.wheels.values():
drive = static.joints[wheel_id].drives["x"]
commands.extend([{"$type": "set_robot_joint_drive",
"joint_id": wheel_id,
"force_limit": self._default_wheel_force_limit,
"stiffness": drive.stiffness,
"damping": drive.damping,
"id": static.robot_id}])
return commands

@abstractmethod
Expand Down
2 changes: 1 addition & 1 deletion setup.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -10,7 +10,7 @@

setup(
name='magnebot',
version="2.2.10",
version="2.2.11",
description='High-level API for the Magnebot in TDW.',
long_description=readme,
long_description_content_type='text/markdown',
Expand Down