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

Jump to bottom

Add trunk control for Tiago #336

Open
wants to merge 5 commits into
base: master
Choose a base branch
from
+14 −22
Open

Add trunk control for Tiago #336

Changes from all commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
d49546c
add trunk control for tiago
sujaygarlanka Aug 24, 2023
dfd8fdd
Update controller_base.py
sujaygarlanka Aug 24, 2023
0b320b1
Update robot_base.py
sujaygarlanka Aug 24, 2023
a58f420
Update robot_base.py
sujaygarlanka Aug 24, 2023
2e93c5d
Update robot_base.py
sujaygarlanka Aug 24, 2023
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
36 changes: 14 additions & 22 deletions igibson/robots/tiago.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -362,28 +362,20 @@ def _default_controller_config(self):
cfg["base"] = {"JointController": self._default_base_controller_configs}

for arm in self.arm_names:
# Use default IK controller -- also need to override joint idx being controlled to include trunk in default
# IK arm controller
cfg["arm_{}".format(arm)]["InverseKinematicsController"]["joint_idx"] = np.concatenate(
[self.trunk_control_idx, self.arm_control_idx[arm]]
)
# TODO: is this not assigning the trunk to both arms?

# If using rigid trunk, we also clamp its limits
if self.rigid_trunk:
cfg["arm_{}".format(arm)]["InverseKinematicsController"]["control_limits"]["position"][0][
self.trunk_control_idx
] = self.untucked_default_joint_pos[self.trunk_control_idx]
cfg["arm_{}".format(arm)]["InverseKinematicsController"]["control_limits"]["position"][1][
self.trunk_control_idx
] = self.untucked_default_joint_pos[self.trunk_control_idx]

cfg["arm_{}".format(arm)]["InverseKinematicsController"]["ik_joint_idx"] = np.array(
[
self.joint_idx_to_ik_joint_idx[x]
for x in cfg["arm_{}".format(arm)]["InverseKinematicsController"]["joint_idx"]
]
)
for arm_cfg in cfg["arm_{}".format(arm)].values():

if arm == "left":
# Need to override joint idx being controlled to include trunk in default arm controller configs
arm_cfg["joint_idx"] = np.concatenate([self.trunk_control_idx, self.arm_control_idx[arm]])

# If using rigid trunk, we also clamp its limits
# TODO: How to handle for right arm which has a fixed trunk internally even though the trunk is moving
# via the left arm??
if self.rigid_trunk:
arm_cfg["control_limits"]["position"][0][self.trunk_control_idx] = \
self.untucked_default_joint_pos[self.trunk_control_idx]
arm_cfg["control_limits"]["position"][1][self.trunk_control_idx] = \
self.untucked_default_joint_pos[self.trunk_control_idx]

return cfg

Expand Down
Loading