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

Add IK Test Cases for Non serial chains #43

Merged
merged 3 commits into from
Aug 29, 2024
Merged

Add IK Test Cases for Non serial chains #43

Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
666fd06
Update test_inverse_kinematics.py
StoneT2000 Aug 29, 2024
6a4e8e9
more tests
StoneT2000 Aug 29, 2024
bdb1853
Merge branch 'UM-ARM-Lab:master' into add-ik-test-cases
StoneT2000 Aug 29, 2024
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
46 changes: 28 additions & 18 deletions tests/test_inverse_kinematics.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -24,16 +24,27 @@ def make_transparent(link):
for link in visual_data:
make_transparent(link)


def test_jacobian_follower():
def create_test_chain(robot="kuka_iiwa", device="cpu"):
if robot == "kuka_iiwa":
urdf = "kuka_iiwa/model.urdf"
search_path = pybullet_data.getDataPath()
full_urdf = os.path.join(search_path, urdf)
chain = pk.build_serial_chain_from_urdf(open(full_urdf).read(), "lbr_iiwa_link_7")
chain = chain.to(device=device)
elif robot == "widowx":
urdf = "widowx/wx250s.urdf"
full_urdf = urdf
chain = pk.build_serial_chain_from_urdf(open(full_urdf, "rb").read(), "ee_gripper_link")
chain = chain.to(device=device)
else:
raise NotImplementedError(f"Robot {robot} not implemented")
return chain, urdf

def test_jacobian_follower(robot="kuka_iiwa"):
pytorch_seed.seed(2)
device = "cuda" if torch.cuda.is_available() else "cpu"
# device = "cpu"
urdf = "kuka_iiwa/model.urdf"
search_path = pybullet_data.getDataPath()
full_urdf = os.path.join(search_path, urdf)
chain = pk.build_serial_chain_from_urdf(open(full_urdf).read(), "lbr_iiwa_link_7")
chain = chain.to(device=device)
chain, urdf = create_test_chain(robot=robot, device=device)

# robot frame
pos = torch.tensor([0.0, 0.0, 0.0], device=device)
Expand All @@ -45,7 +56,7 @@ def test_jacobian_follower():
# generate random goal joint angles (so these are all achievable)
# use the joint limits to generate random joint angles
lim = torch.tensor(chain.get_joint_limits(), device=device)
goal_q = torch.rand(M, 7, device=device) * (lim[1] - lim[0]) + lim[0]
goal_q = torch.rand(M, lim.shape[1], device=device) * (lim[1] - lim[0]) + lim[0]

# get ee pose (in robot frame)
goal_in_rob_frame_tf = chain.forward_kinematics(goal_q)
Expand Down Expand Up @@ -159,24 +170,19 @@ def test_jacobian_follower():
p.stepSimulation()


def test_ik_in_place_no_err():
def test_ik_in_place_no_err(robot="kuka_iiwa"):
pytorch_seed.seed(2)
device = "cuda" if torch.cuda.is_available() else "cpu"
# device = "cpu"
urdf = "kuka_iiwa/model.urdf"
search_path = pybullet_data.getDataPath()
full_urdf = os.path.join(search_path, urdf)
chain = pk.build_serial_chain_from_urdf(open(full_urdf).read(), "lbr_iiwa_link_7")
chain = chain.to(device=device)

chain, urdf = create_test_chain(robot=robot, device=device)
# robot frame
pos = torch.tensor([0.0, 0.0, 0.0], device=device)
rot = torch.tensor([0.0, 0.0, 0.0], device=device)
rob_tf = pk.Transform3d(pos=pos, rot=rot, device=device)

# goal equal to current configuration
lim = torch.tensor(chain.get_joint_limits(), device=device)
cur_q = torch.rand(7, device=device) * (lim[1] - lim[0]) + lim[0]
cur_q = torch.rand(lim.shape[1], device=device) * (lim[1] - lim[0]) + lim[0]
M = 1
goal_q = cur_q.unsqueeze(0).repeat(M, 1)

Expand Down Expand Up @@ -209,5 +215,9 @@ def test_ik_in_place_no_err():


if __name__ == "__main__":
test_jacobian_follower()
test_ik_in_place_no_err()
print("Testing kuka_iiwa IK")
test_jacobian_follower(robot="kuka_iiwa")
test_ik_in_place_no_err(robot="kuka_iiwa")
print("Testing widowx IK")
test_jacobian_follower(robot="widowx")
test_ik_in_place_no_err(robot="widowx")
Loading