1313
1414
1515def run_motion_planning (
16- robot : SingleArmPyBulletRobot , initial_positions : JointPositions ,
17- target_positions : JointPositions , collision_bodies : Collection [int ],
18- seed : int ,
19- physics_client_id : int ) -> Optional [Sequence [JointPositions ]]:
16+ robot : SingleArmPyBulletRobot ,
17+ initial_positions : JointPositions ,
18+ target_positions : JointPositions ,
19+ collision_bodies : Collection [int ],
20+ seed : int ,
21+ physics_client_id : int ,
22+ ) -> Optional [Sequence [JointPositions ]]:
2023 """Run BiRRT to find a collision-free sequence of joint positions.
2124
2225 Note that this function changes the state of the robot.
2326 """
2427 rng = np .random .default_rng (seed )
2528 joint_space = robot .action_space
2629 joint_space .seed (seed )
27- _sample_fn = lambda _ : joint_space .sample ()
2830 num_interp = CFG .pybullet_birrt_extend_num_interp
2931
32+ def _sample_fn (pt : JointPositions ) -> JointPositions :
33+ new_pt : JointPositions = list (joint_space .sample ())
34+ # Don't change the fingers.
35+ new_pt [robot .left_finger_joint_idx ] = pt [robot .left_finger_joint_idx ]
36+ new_pt [robot .right_finger_joint_idx ] = pt [robot .right_finger_joint_idx ]
37+ return new_pt
38+
39+ def _set_state (pt : JointPositions ) -> None :
40+ robot .set_joints (pt )
41+
3042 def _extend_fn (pt1 : JointPositions ,
3143 pt2 : JointPositions ) -> Iterator [JointPositions ]:
3244 pt1_arr = np .array (pt1 )
@@ -38,7 +50,7 @@ def _extend_fn(pt1: JointPositions,
3850 yield list (pt1_arr * (1 - i / num ) + pt2_arr * i / num )
3951
4052 def _collision_fn (pt : JointPositions ) -> bool :
41- robot . set_joints (pt )
53+ _set_state (pt )
4254 p .performCollisionDetection (physicsClientId = physics_client_id )
4355 for body in collision_bodies :
4456 if p .getContactPoints (robot .robot_id ,
@@ -48,6 +60,8 @@ def _collision_fn(pt: JointPositions) -> bool:
4860 return False
4961
5062 def _distance_fn (from_pt : JointPositions , to_pt : JointPositions ) -> float :
63+ # NOTE: only using positions to calculate distance. Should use
64+ # orientations as well in the near future.
5165 from_ee = robot .forward_kinematics (from_pt )
5266 to_ee = robot .forward_kinematics (to_pt )
5367 return sum (np .subtract (from_ee , to_ee )** 2 )
0 commit comments