Skip to content

Commit

Permalink
fixed tests
Browse files Browse the repository at this point in the history
  • Loading branch information
@Lexseal
Lexseal committed Apr 1, 2024
1 parent 307497f commit 39ebca5
Show file tree
Hide file tree
Showing 3 changed files with 29 additions and 47 deletions.
16 changes: 6 additions & 10 deletions mplib/examples/detect_collision.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -49,32 +49,28 @@ def demo(self):
floor, [0, 0, -0.1], [1, 0, 0, 0]
)
# update the planning world with the floor collision object
self.planner.set_normal_object("floor", floor_fcl_collision_object)
self.planner.planning_world.add_normal_object(
"floor", floor_fcl_collision_object
)
# floor ankor end
print("\n----- self-collision-free qpos -----")
# if the joint qpos does not include the gripper joints,
# it will be set to the current gripper joint angle
self_collision_free_qpos = [0, 0.19, 0.0, -2.61, 0.0, 2.94, 0.78]
self.print_collisions(
self.planner.check_for_self_collision(
self.planner.robot, self_collision_free_qpos
)
self.planner.check_for_self_collision(self_collision_free_qpos)
)

print("\n----- self-collision qpos -----")
self_collision_qpos = [0, 1.36, 0, -3, -3, 3, -1]
self.print_collisions(
self.planner.check_for_self_collision(
self.planner.robot, self_collision_qpos
)
self.planner.check_for_self_collision(self_collision_qpos)
)

print("\n----- env-collision-free qpos -----")
env_collision_free_qpos = self_collision_free_qpos
self.print_collisions(
self.planner.check_for_env_collision(
self.planner.robot, env_collision_free_qpos
)
self.planner.check_for_env_collision(env_collision_free_qpos)
)

print("\n----- env-collision qpos -----")
Expand Down
4 changes: 1 addition & 3 deletions mplib/examples/two_stage_motion.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -118,7 +118,6 @@ def plan_without_base(self, pose, has_attach=False):
goal_qposes,
self.robot.get_qpos(),
use_point_cloud=True,
use_attach=has_attach,
time_step=1 / 250,
fixed_joint_indices=range(2),
)
Expand All @@ -140,7 +139,6 @@ def move_in_two_stage(self, pose, has_attach=False):
goal_qposes,
self.robot.get_qpos(),
use_point_cloud=True,
use_attach=has_attach,
time_step=1 / 250,
fixed_joint_indices=range(2, 9),
)
Expand All @@ -166,7 +164,7 @@ def demo(self):
collision_object = fcl.CollisionObject(
fcl_red_cube, [0.7, 0, 0.07], [1, 0, 0, 0]
)
self.planner.planning_world.set_normal_object("target", collision_object)
self.planner.planning_world.add_normal_object("target", collision_object)

# go above the target
pickup_pose[2] += 0.2
Expand Down
56 changes: 22 additions & 34 deletions tests/test_planner.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -124,25 +124,13 @@ def test_pad_move_group_qpos(self):

def test_self_collision(self):
self_collision_qpos = [0, 1.36, 0, -3, -3, 3, -1]
self.assertTrue(
self.planner.check_for_self_collision(
self.planner.robot, self_collision_qpos
)
)
self.assertTrue(self.planner.check_for_self_collision(self_collision_qpos))
self_collision_qpos[0] += (
1 # rotating the robot around the base should not cause self-collision to disappear
)
self.assertTrue(
self.planner.check_for_self_collision(
self.planner.robot, self_collision_qpos
)
)
self.assertTrue(self.planner.check_for_self_collision(self_collision_qpos))
collision_free_qpos = [0, 0.19, 0.0, -2.61, 0.0, 2.94, 0.78]
self.assertFalse(
self.planner.check_for_self_collision(
self.planner.robot, collision_free_qpos
)
)
self.assertFalse(self.planner.check_for_self_collision(collision_free_qpos))

def test_env_collision(self):
floor = fcl.Box([2, 2, 0.1]) # create a 2 x 2 x 0.1m box
Expand All @@ -151,7 +139,9 @@ def test_env_collision(self):
floor, [0, 0, -0.1], [1, 0, 0, 0]
)
# update the planning world with the floor collision object
self.planner.set_normal_object("floor", floor_fcl_collision_object)
self.planner.planning_world.add_normal_object(
"floor", floor_fcl_collision_object
)

env_collision_qpos = [
0,
Expand All @@ -162,15 +152,11 @@ def test_env_collision(self):
0,
0,
] # this qpos causes several joints to dip below the floor
self.assertTrue(
self.planner.check_for_env_collision(self.planner.robot, env_collision_qpos)
)
self.assertTrue(self.planner.check_for_env_collision(env_collision_qpos))

# remove the floor and check for env-collision returns no collision
self.planner.remove_normal_object("floor")
self.assertFalse(
self.planner.check_for_env_collision(self.planner.robot, env_collision_qpos)
)
self.assertFalse(self.planner.check_for_env_collision(env_collision_qpos))

def test_IK(self):
num_success = 0
Expand All @@ -196,7 +182,9 @@ def test_IK(self):
)
status, _ = self.planner.IK([0.4, 0.3, -0.1, 0, 1, 0, 0], self.init_qpos)
self.assertEqual(status, "Success")
self.planner.set_normal_object("floor", floor_fcl_collision_object)
self.planner.planning_world.add_normal_object(
"floor", floor_fcl_collision_object
)
status, _ = self.planner.IK([0.4, 0.3, -0.1, 0, 1, 0, 0], self.init_qpos)
self.assertNotEqual(status, "Success")

Expand Down Expand Up @@ -255,27 +243,27 @@ def add_point_cloud(self):
def test_update_point_cloud(self):
# use screw based planning. first succeeds but after point cloud obstacle fails
pose = [0.7, 0, 0.12, 0, 1, 0, 0]
self.add_point_cloud()
result_screw = self.planner.plan_screw(pose, self.init_qpos)
self.assertEqual(result_screw["status"], "Success")
result_screw = self.planner.plan_screw(
pose, self.init_qpos, use_point_cloud=True
)

# now add a point cloud and we should fail
self.add_point_cloud()

result_screw = self.planner.plan_screw(pose, self.init_qpos)
self.assertNotEqual(result_screw["status"], "Success")

def test_update_attach(self):
starting_qpos = [0, 0.48, 0, -1.48, 0, 1.96, 0.78]
target_pose = [0.4, 0.3, 0.33, 0, 1, 0, 0]
self.planner.update_attached_box([0.04, 0.04, 0.12], [0, 0, 0.14, 1, 0, 0, 0])
self.add_point_cloud()

result_screw = self.planner.plan_screw(
target_pose, starting_qpos, use_point_cloud=True
)
result_screw = self.planner.plan_screw(target_pose, starting_qpos)
self.assertEqual(result_screw["status"], "Success")
result_screw = self.planner.plan_screw(
target_pose, starting_qpos, use_point_cloud=True, use_attach=True
)

# now attach a box to the end effector and we should fail
self.planner.update_attached_box([0.04, 0.04, 0.12], [0, 0, 0.14, 1, 0, 0, 0])

result_screw = self.planner.plan_screw(target_pose, starting_qpos)
self.assertNotEqual(result_screw["status"], "Success")

def test_fixed_joint(self):
Expand Down

0 comments on commit 39ebca5

Please sign in to comment.