diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..0ec87af --- /dev/null +++ b/.gitignore @@ -0,0 +1,11 @@ +gpt_rrt.py +pybullet/gettingStarted.py +pybullet/__pycache__/gen3lite_controller_collision_detection.cpython-313.pyc +pybullet/__pycache__/gen3lite_controller_collision_detection.cpython-313.pyc +pybullet/__pycache__/gen3lite_controller_collision_detection.cpython-313.pyc +pybullet/hardest_home.npy +.gitignore +pybullet/davespath.npy +.gitignore +pybullet/__pycache__/gen3lite_controller_collision_detection.cpython-313.pyc +pybullet/__pycache__/gen3lite_controller_collision_detection.cpython-38.pyc diff --git a/pybullet/__pycache__/gen3lite_controller_collision_detection.cpython-310.pyc b/pybullet/__pycache__/gen3lite_controller_collision_detection.cpython-310.pyc new file mode 100644 index 0000000..d49e6b5 Binary files /dev/null and b/pybullet/__pycache__/gen3lite_controller_collision_detection.cpython-310.pyc differ diff --git a/pybullet/__pycache__/gen3lite_controller_collision_detection.cpython-313.pyc b/pybullet/__pycache__/gen3lite_controller_collision_detection.cpython-313.pyc new file mode 100644 index 0000000..d367052 Binary files /dev/null and b/pybullet/__pycache__/gen3lite_controller_collision_detection.cpython-313.pyc differ diff --git a/pybullet/__pycache__/gen3lite_controller_collision_detection.cpython-38.pyc b/pybullet/__pycache__/gen3lite_controller_collision_detection.cpython-38.pyc new file mode 100644 index 0000000..96e446a Binary files /dev/null and b/pybullet/__pycache__/gen3lite_controller_collision_detection.cpython-38.pyc differ diff --git a/pybullet/arm_rrt.py b/pybullet/arm_rrt.py index 349d105..99b1b8e 100644 --- a/pybullet/arm_rrt.py +++ b/pybullet/arm_rrt.py @@ -1,161 +1,154 @@ import numpy as np from tqdm import tqdm from scipy.spatial import cKDTree -import matplotlib.pyplot as plt import random -import math -import gen3lite_controller_collision_detection +import argparse +import robots -# ----------------------------- -# RRT Node -# ----------------------------- class Node: - def __init__(self, point): + def __init__(self, point, parent=None): self.point = np.array(point) - self.parent = None + self.parent = parent + +class Tree: + def __init__(self, node_list, kdtree): + self.node_list = node_list + self.kdtree = kdtree + + def add(self,new_point): + self.node_list.append(new_point) + + if len(self.node_list) % 50 == 0: + data = [n.point for n in self.node_list] + self.kdtree = cKDTree(data) -# ----------------------------- -# RRT Planner -# ----------------------------- class RRT: - def __init__( - self, - start=None, - goal=None, - obstacles=None, - rand_area=[], - step_size=0.1, - goal_sample_rate=0.1, - max_iter=5000 - ): - self.controller = gen3lite_controller_collision_detection.Gen3LiteArmController() - self.controller.createBalloonMaze() + def __init__(self, step_size=0.1,max_iter=20,env_name="Free"): + self.controller = robots.Gen3LiteArmController(env_name=env_name) self.start = Node(self.controller.getCurrentJointAngles()) - self.goal = Node([0.1026325022237283, -0.2931188624740633, 1.2717083400432991, 0.048794139164578594, 0.07744723004754135, -0.8437927483158898, -0.024709326684397483]) + self.goal = Node(self.controller.goal_angles) self.rand_ranges = self.controller.getRanges() self.step_size = step_size - self.goal_sample_rate = goal_sample_rate self.max_iter = max_iter - self.node_list = [self.start] - self.kdtree = cKDTree([self.start.point]) - - # ----------------------------- - # Main planning loop - # ----------------------------- + self.start_tree = Tree([self.start],cKDTree([self.start.point])) + self.goal_tree = Tree([self.goal],cKDTree([self.goal.point])) + self.path_to_goal = [] + + # This a "stub" planning function. It adds random nodes always + # to the root of a start and goal tree. It shows many of the + # syntax elements and helper functions you could use, but + # you will have to fix and extend this to make it an RRT or + # RRT-Connect planner that can solve the harder environments. def plan(self): - for _ in tqdm(range(self.max_iter)): - + # Add max_iter nodes to each start and goal tree + for k in tqdm(range(self.max_iter)): rnd_point = self.sample() - nearest_node = self.nearest_node(rnd_point) - new_node = self.steer(nearest_node, rnd_point) + if self.collision_free(rnd_point, self.start.point): + new_node = Node(rnd_point,self.start) + self.start_tree.add(new_node) - if self.collision_free(nearest_node.point, new_node.point): - self.node_list.append(new_node) + rnd_point = self.sample() + if self.collision_free(rnd_point, self.goal.point): + new_node = Node(rnd_point,self.goal) + self.goal_tree.add(new_node) - if len(self.node_list) % 50 == 0: - data = [n.point for n in self.node_list] - self.kdtree = cKDTree(data) + # Now add the same rnd_pnt to both trees, making a connection + # and completing the path start->goal + rnd_point = self.sample() + if self.collision_free(rnd_point, self.start.point): + new_start_node = Node(rnd_point,self.start) + self.start_tree.add(new_start_node) - if self.reached_goal(new_node): - p = self.extract_path(new_node) - self.controller.execPath(p) - return p + if self.collision_free(rnd_point, self.goal.point): + new_goal_node = Node(rnd_point,self.goal) + self.goal_tree.add(new_goal_node) - return None # Failed + self.path_to_goal = self.extract_path(new_start_node,new_goal_node) + return True - # ----------------------------- - # Sampling - # ----------------------------- def sample(self): - if random.random() < self.goal_sample_rate: - return self.goal.point point = [] for i in range(0,len(self.rand_ranges[0])): point.append(random.uniform(self.rand_ranges[0][i], self.rand_ranges[1][i])) return np.array(point) - # ----------------------------- - # Nearest node - # ----------------------------- - def nearest_node(self, point): - _, idx = self.kdtree.query(point) - return self.node_list[idx] + def nearest_node(self, point, tree): + _, idx = tree.kdtree.query(point) + return tree.node_list[idx] - # ----------------------------- - # Steer - # ----------------------------- def steer(self, from_node, to_point): direction = to_point - from_node.point distance = np.linalg.norm(direction) - direction = direction / distance - new_point = from_node.point + self.step_size * direction + if distance < self.step_size: + new_point = to_point + else: + direction = direction / distance + new_point = from_node.point + self.step_size * direction + new_node = Node(new_point) new_node.parent = from_node return new_node - # ----------------------------- - # Collision checking - # ----------------------------- def collision_free(self, p1, p2): return self.controller.collision_free(p1,p2) - # ----------------------------- - # Goal check - # ----------------------------- - def reached_goal(self, node): - return np.linalg.norm(node.point - self.goal.point) < self.step_size - - # ----------------------------- - # Path extraction - # ----------------------------- - def extract_path(self, node): - path = [self.goal.point] - while node is not None: - path.append(node.point) - node = node.parent - return path[::-1] - - # ----------------------------- - # Visualization - # ----------------------------- - def draw(self, path=None): - plt.figure() - for node in self.node_list: - if node.parent: - plt.plot( - [node.point[0], node.parent.point[0]], - [node.point[1], node.parent.point[1]], - "-g" - ) - - for (ox, oy, r) in self.obstacles: - circle = plt.Circle((ox, oy), r, color="r") - plt.gca().add_patch(circle) - - plt.plot(self.start.point[0], self.start.point[1], "bo", label="Start") - plt.plot(self.goal.point[0], self.goal.point[1], "ro", label="Goal") - - if path: - px, py = zip(*path) - plt.plot(px, py, "-b", linewidth=2, label="Path") - - plt.axis("equal") - plt.grid(True) - plt.legend() - plt.show() - - -# ----------------------------- -# Example Usage -# ----------------------------- -if __name__ == "__main__": + def reached_goal(self, node,goal=None): + if goal is None: + goal = self.goal + + return np.linalg.norm(node.point - goal.point) < self.step_size and self.collision_free(node.point,goal.point) + + def extract_path(self, start_node,goal_node): + # Build the start-tree path, leaf to root (backwards) + start_tree_path = [] + while start_node is not None: + start_tree_path.append(start_node.point) + start_node = start_node.parent + + # Build the goal-tree path, leaf to root (forwards) + goal_tree_path = [] + while goal_node is not None: + goal_tree_path.append(goal_node.point) + goal_node = goal_node.parent + + # First add the start path, reversing + overall_path = start_tree_path[::-1] + # Add the goal path + overall_path.extend(goal_tree_path) + return overall_path - rrt = RRT() +if __name__ == "__main__": - path = rrt.plan() - print("Planning complete.") - print(path) - #rrt.draw(path) + parser = argparse.ArgumentParser( + prog='arm_rrt', + description='Plans and executes paths for arms around obstacles.') + parser.add_argument('--filename',default='rrt_path.npy') + parser.add_argument('-e', '--environment',default='middle') + parser.add_argument('-p', '--plan',action='store_true') + parser.add_argument('-r', '--run',action='store_true') + args = parser.parse_args() + + rrt = RRT(env_name=args.environment) + + if args.plan: + success = rrt.plan() + + if success: + print("Tree planning reached the goal.") + np.save(args.filename,rrt.path_to_goal) + else: + print("Failed to find a path to the goal.") + + rrt.controller.visTreesAndPaths( + [rrt.start_tree,rrt.goal_tree], + [rrt.path_to_goal], + rgbas_in=[[0.5,0.0,0.5,1.0],[0.902,0.106,0.714,1.0]] + ) + + if args.run: + path_to_goal = np.load(args.filename) + rrt.controller.execPath(path_to_goal) + \ No newline at end of file diff --git a/pybullet/gen3lite_controller_collision_detection.py b/pybullet/gen3lite_controller_collision_detection.py deleted file mode 100644 index a5be17b..0000000 --- a/pybullet/gen3lite_controller_collision_detection.py +++ /dev/null @@ -1,606 +0,0 @@ -import pybullet as pb -import time -import math -from typing import Dict, List, Optional -from enum import Enum -import numpy as np - - -class ControlModes(Enum): - """ - Pybullet control modes, only for the end effector. We use IK to translate - EF commands to joint velocities using a PID controller. - """ - - END_EFFECTOR_POSE = pb.POSITION_CONTROL - END_EFFECTOR_TWIST = pb.VELOCITY_CONTROL - - -class Gen3LiteArmController: - """ - A controller for the Kinova Gen3 Lite robotic arm in PyBullet. - - This class provides methods to control the arm's joints, move the end-effector - to desired positions and orientations using inverse kinematics, and operate the gripper. - """ - - def __init__(self, dt=1 / 50.0): - self.dt = dt - - self.LEFT_FINGER_JOINT = 7 # Example index; update if needed. - self.RIGHT_FINGER_JOINT = 9 # Example index; update if needed. - self.GRIPPER_OPEN_POS = 0.7 # Adjust as needed. - self.GRIPPER_CLOSED_POS = 0.0 # Adjust as needed. - - # End-effector link index as used in your URDF. - self.END_EFFECTOR_INDEX = 7 - - pb.connect(pb.GUI,) - pb.setGravity(0, 0, -9.8) - pb.setTimeStep(self.dt) - - # Load the Kinova Gen3 Lite URDF model. - # Ensure the path "gen3lite_urdf/gen3_lite.urdf" exists in your directory - self.__kinova_id = pb.loadURDF("gen3lite_urdf/gen3_lite.urdf", [0, 0, 0], useFixedBase=True) - pb.resetBasePositionAndOrientation(self.__kinova_id, [0, 0, 0.0], [0, 0, 0, 1]) - - self.__n_joints = 7 # pb.getNumJoints(self.__kinova_id) - 5, where -5 for the gripper - print(f'Found {self.__n_joints} active joints for the robot.') - - # Joint limits and rest/home poses. - self.__lower_limits: List = [-.967, -2, -2.96, 0.19, -2.96, -2.09, -3.05] - self.__upper_limits: List = [.967, 2, 2.96, 2.29, 2.96, 2.09, 3.05] - self.__joint_ranges: List = [5.8, 4, 5.8, 4, 5.8, 4, 6] - self.__rest_poses: List = [0, 0, 0, 0, 0, 0, 0] - self.__home_poses: List = [0, 0, 0.5 * math.pi, 0.5 * math.pi, 0.5 * math.pi, -math.pi * 0.5, 0] - - self.joint_ids = [pb.getJointInfo(self.__kinova_id, i) for i in range(self.__n_joints)] - self.joint_ids = [j[0] for j in self.joint_ids if j[2] == pb.JOINT_REVOLUTE] - - # Initialize to home position. - for i in range(self.__n_joints): - pb.resetJointState(self.__kinova_id, i, self.__home_poses[i]) - - self.default_ori = list(pb.getQuaternionFromEuler([0, -math.pi, 0])) - pb.configureDebugVisualizer(pb.COV_ENABLE_RENDERING, 0) - - def getRanges(self): - return (self.__lower_limits,self.__upper_limits) - - def getCurrentJointAngles(self): - angles = [] - for id in self.joint_ids: - joint_state = pb.getJointState(self.__kinova_id,id) - angles.append(joint_state[0]) - return angles - - def createBalloonMaze(self): - for y in [-0.125, 0.125]: - for z in [0.25, 0.5]: - col_box_id = pb.createCollisionShape(pb.GEOM_SPHERE, radius=0.1) - box_id = pb.createMultiBody(baseMass=0, baseCollisionShapeIndex=col_box_id, basePosition=[0.4, y, z]) - - def set_to_home(self): - """ - Resets the arm to its predefined home position. - """ - for i in range(self.__n_joints): - pb.resetJointState(self.__kinova_id, i, self.__home_poses[i]) - - def execPath(self,path): - pb.configureDebugVisualizer(pb.COV_ENABLE_RENDERING, 1) - for p in path: - self.move_to_joint_positions(p) - - def move_to_joint_positions(self, joints, max_steps=100): - """ - Move to target joint positions with position control. - - Args: - joints (list): Target joint positions. - max_steps (int): Maximum simulation steps to reach the target. - """ - for i in range(self.__n_joints): - pb.setJointMotorControl2( - bodyIndex=self.__kinova_id, - jointIndex=i, - controlMode=pb.POSITION_CONTROL, - targetPosition=joints[i], - force=2000, - positionGain=1.0, - velocityGain=1.0 - ) - - # Step the simulation for a short duration to allow movement. - for k in range(max_steps): - pb.stepSimulation() - curr = self.getCurrentJointAngles() - e = np.linalg.norm(np.array(joints) - np.array(curr)) - print("Error at iter ", k, " is ", e) - print("Target: ", joints) - print("Current ", curr) - if e < 0.1: - break - - time.sleep(self.dt) - - def move_to_cartesian(self, target_pos, target_ori, max_steps=240, error_threshold=0.01): - """ - Moves the arm using inverse kinematics and closed-loop control until the end effector - reaches the desired position and orientation within a threshold. - - Args: - target_pos (list or np.array): Desired end-effector position [x, y, z]. - target_ori (list or np.array): Desired end-effector orientation (quaternion). - max_steps (int): Maximum number of simulation steps to try. - error_threshold (float): Acceptable Euclidean distance (in meters) between - the current and target positions. - """ - - # Calculate the inverse kinematics solution. - jointPoses = pb.calculateInverseKinematics( - self.__kinova_id, - self.END_EFFECTOR_INDEX, - target_pos, - target_ori, - lowerLimits=self.__lower_limits, - upperLimits=self.__upper_limits, - jointRanges=self.__joint_ranges, - restPoses=self.__rest_poses, - maxNumIterations=100 - ) - - # Slice the IK solution so that only the controlled joints are used. - jointPoses = jointPoses[:self.__n_joints] - - for step in range(max_steps): - # Command each joint to the desired position. - for i in range(self.__n_joints): - pb.setJointMotorControl2( - bodyIndex=self.__kinova_id, - jointIndex=i, - controlMode=pb.POSITION_CONTROL, - targetPosition=jointPoses[i], - force=500, - positionGain=0.05, - velocityGain=1 - ) - - # Step the simulation. - pb.stepSimulation() - time.sleep(self.dt) - - # Get the current end-effector state. - ee_state = pb.getLinkState(self.__kinova_id, self.END_EFFECTOR_INDEX) - current_pos = np.array(ee_state[0]) - current_error = np.linalg.norm(np.array(target_pos) - current_pos) - - # If within threshold, break out. - if current_error < error_threshold: - print("Target reached within threshold.") - break - - # Final achieved state. - final_state = pb.getLinkState(self.__kinova_id, self.END_EFFECTOR_INDEX) - final_pos = final_state[0] - final_ori = final_state[1] - - print("Target end-effector position:", target_pos) - print("Final achieved end-effector position:", final_pos) - - def open_gripper(self): - """ - Opens the gripper. - """ - pb.setJointMotorControl2(self.__kinova_id, self.LEFT_FINGER_JOINT, pb.POSITION_CONTROL, - targetPosition=self.GRIPPER_OPEN_POS, force=500) - pb.setJointMotorControl2(self.__kinova_id, self.RIGHT_FINGER_JOINT, pb.POSITION_CONTROL, - targetPosition=-self.GRIPPER_OPEN_POS, force=500) - for _ in range(100): - pb.stepSimulation() - time.sleep(self.dt) - - print("Gripper opened.") - - def close_gripper(self): - """ - Closes the gripper. - """ - pb.setJointMotorControl2(self.__kinova_id, self.LEFT_FINGER_JOINT, pb.POSITION_CONTROL, - targetPosition=self.GRIPPER_CLOSED_POS, force=500) - pb.setJointMotorControl2(self.__kinova_id, self.RIGHT_FINGER_JOINT, pb.POSITION_CONTROL, - targetPosition=self.GRIPPER_CLOSED_POS, force=500) - for _ in range(100): - pb.stepSimulation() - time.sleep(self.dt) - - print("Gripper closed.") - - def set_joint_positions(self, joint_positions): - for joint_index, q in enumerate(joint_positions): - pb.resetJointState(self.__kinova_id, joint_index, q) - - def collision_free(self,p1,p2): - - self.set_joint_positions(p1) - if self.check_collision(): - return False - self.set_joint_positions(p2) - if self.check_collision(): - return False - return True - - # ------------------------------------------------------------------ - # UPDATED COLLISION FUNCTION - # ------------------------------------------------------------------ - def check_collision(self): - """ - Checks for collisions between the robot and ANY other body in the environment, - as well as self-collisions (robot links hitting each other). - - Returns: - bool: True if any collision is detected, False otherwise. - """ - # Ensure collision detection is up to date - pb.performCollisionDetection() - - # Iterate over all bodies in the PyBullet simulation - for i in range(pb.getNumBodies()): - other_body_id = pb.getBodyUniqueId(i) - - # Case 1: Self-collision (Body vs itself) - if other_body_id == self.__kinova_id: - contact_points = pb.getContactPoints(bodyA=self.__kinova_id, bodyB=self.__kinova_id) - - # Case 2: Environment collision (Body vs other object) - else: - contact_points = pb.getContactPoints(bodyA=self.__kinova_id, bodyB=other_body_id) - - # If contacts are found, return True immediately - if contact_points is None or len(contact_points) > 0: - return True - - # If loop completes without returning, no collisions were found - return False - -def main(): - """ - Test the Gen3Lite Arm moving, gripper functionalities, and collision detection. - """ - - # Initialize PyBullet simulation - pb.connect(pb.GUI) - pb.setGravity(0, 0, -9.8) - - # Create the controller - controller = Gen3LiteArmController() - pb.setTimeStep(controller.dt) - - # Test homing functionality - print("\nTesting Gen3Lite Arm controller homing...") - controller.move_to_cartesian([0.5, 0, 0.375], controller.default_ori) - #controller.set_joint_positions([0, 0, 0.5 * math.pi, 0.5 * math.pi, 0.5 * math.pi, -math.pi * 0.5, 0]) - - # --- COLLISION DETECTION TEST START --- - print("\nTesting Collision Detection...") - - for y in [-0.125, 0.125]: - for z in [0.25, 0.5]: - col_box_id = pb.createCollisionShape(pb.GEOM_SPHERE, radius=0.125) - box_id = pb.createMultiBody(baseMass=0, baseCollisionShapeIndex=col_box_id, basePosition=[0.4, y, z]) - - #col_box_id = pb.createCollisionShape(pb.GEOM_SPHERE, halfExtents=[0.2, 0.2, 0.2]) - #box_id = pb.createMultiBody(baseMass=0, baseCollisionShapeIndex=col_box_id, basePosition=[-1.0, 0, 0.5]) - - #col_box_id = pb.createCollisionShape(pb.GEOM_SPHERE, halfExtents=[0.2, 0.2, 0.2]) - #box_id = pb.createMultiBody(baseMass=0, baseCollisionShapeIndex=col_box_id, basePosition=[0.0, 1.0, 0.5]) - - print(controller.getCurrentJointAngles()) - for i in range (10000): - pb.stepSimulation() - time.sleep(1./240.) - - pb.disconnect() - - # Note: No arguments passed to check_collision - is_collision = controller.check_collision() - print(f"Box at [1.0, 0, 0.5]. Collision detected? {is_collision} (Expected: False)") - - return - - # 3. Move the box to where the arm currently is (approx [0.4, 0, 0.4]) - print("Moving box to collide with arm...") - pb.resetBasePositionAndOrientation(box_id, [0.4, 0, 0.4], [0, 0, 0, 1]) - pb.stepSimulation() - - is_collision = controller.check_collision() - print(f"Box at [0.4, 0, 0.4]. Collision detected? {is_collision} (Expected: True)") - - # Remove the box to continue other tests cleanly - pb.removeBody(box_id) - # --- COLLISION DETECTION TEST END --- - - controller.move_to_cartesian([-0.4, 0, 0.4], controller.default_ori) - - # Test gripper functionality - print("\nTesting gripper open/close...") - controller.open_gripper() - controller.close_gripper() - - # Test the direct joint control - print("\nTesting direct joint control...") - home_ = [0, 0, 0, 0, 0, -math.pi * 0.5, 0] - controller.move_to_joint_positions(home_) - - print("All tests completed.") - pb.disconnect() - - -if __name__ == "__main__": - main() - -# import pybullet as pb -# import time -# import math -# from typing import Dict, List -# from enum import Enum -# import numpy as np -# -# -# class ControlModes(Enum): -# """ -# Pybullet control modes, only for the end effector. We use IK to translate -# EF commands to joint velocities using a PID controller. -# """ -# -# END_EFFECTOR_POSE = pb.POSITION_CONTROL -# END_EFFECTOR_TWIST = pb.VELOCITY_CONTROL -# -# -# class Gen3LiteArmController: -# """ -# A controller for the Kinova Gen3 Lite robotic arm in PyBullet. -# -# This class provides methods to control the arm's joints, move the end-effector -# to desired positions and orientations using inverse kinematics, and operate the gripper. -# """ -# -# def __init__(self, dt=1 / 50.0): -# self.dt = dt -# -# self.LEFT_FINGER_JOINT = 7 # Example index; update if needed. -# self.RIGHT_FINGER_JOINT = 9 # Example index; update if needed. -# self.GRIPPER_OPEN_POS = 0.7 # Adjust as needed. -# self.GRIPPER_CLOSED_POS = 0.0 # Adjust as needed. -# -# # End-effector link index as used in your URDF. -# self.END_EFFECTOR_INDEX = 7 -# -# # Load the Kinova Gen3 Lite URDF model. -# # Ensure the path "gen3lite_urdf/gen3_lite.urdf" exists in your directory -# self.__kinova_id = pb.loadURDF("gen3lite_urdf/gen3_lite.urdf", [0, 0, 0], useFixedBase=True) -# pb.resetBasePositionAndOrientation(self.__kinova_id, [0, 0, 0.0], [0, 0, 0, 1]) -# -# self.__n_joints = 7 # pb.getNumJoints(self.__kinova_id) - 5, where -5 for the gripper -# print(f'Found {self.__n_joints} active joints for the robot.') -# -# # Joint limits and rest/home poses. -# self.__lower_limits: List = [-.967, -2, -2.96, 0.19, -2.96, -2.09, -3.05] -# self.__upper_limits: List = [.967, 2, 2.96, 2.29, 2.96, 2.09, 3.05] -# self.__joint_ranges: List = [5.8, 4, 5.8, 4, 5.8, 4, 6] -# self.__rest_poses: List = [0, 0, 0, 0, 0, 0, 0] -# self.__home_poses: List = [0, 0, 0.5 * math.pi, 0.5 * math.pi, 0.5 * math.pi, -math.pi * 0.5, 0] -# -# self.joint_ids = [pb.getJointInfo(self.__kinova_id, i) for i in range(self.__n_joints)] -# self.joint_ids = [j[0] for j in self.joint_ids if j[2] == pb.JOINT_REVOLUTE] -# -# # Initialize to rest position. -# for i in range(self.__n_joints): -# pb.resetJointState(self.__kinova_id, i, self.__rest_poses[i]) -# -# self.default_ori = list(pb.getQuaternionFromEuler([0, -math.pi, 0])) -# -# def set_to_home(self): -# """ -# Resets the arm to its predefined home position. -# """ -# for i in range(self.__n_joints): -# pb.resetJointState(self.__kinova_id, i, self.__home_poses[i]) -# -# def move_to_joint_positions(self, joints, max_steps=100): -# """ -# Move to target joint positions with position control. -# -# Args: -# joints (list): Target joint positions. -# max_steps (int): Maximum simulation steps to reach the target. -# """ -# for i in range(self.__n_joints): -# pb.setJointMotorControl2( -# bodyIndex=self.__kinova_id, -# jointIndex=i, -# controlMode=pb.POSITION_CONTROL, -# targetPosition=joints[i], -# force=500, -# positionGain=0.05, -# velocityGain=1 -# ) -# -# # Step the simulation for a short duration to allow movement. -# for _ in range(max_steps): -# pb.stepSimulation() -# time.sleep(self.dt) -# -# def move_to_cartesian(self, target_pos, target_ori, max_steps=240, error_threshold=0.01): -# """ -# Moves the arm using inverse kinematics and closed-loop control until the end effector -# reaches the desired position and orientation within a threshold. -# -# Args: -# target_pos (list or np.array): Desired end-effector position [x, y, z]. -# target_ori (list or np.array): Desired end-effector orientation (quaternion). -# max_steps (int): Maximum number of simulation steps to try. -# error_threshold (float): Acceptable Euclidean distance (in meters) between -# the current and target positions. -# """ -# -# # Calculate the inverse kinematics solution. -# jointPoses = pb.calculateInverseKinematics( -# self.__kinova_id, -# self.END_EFFECTOR_INDEX, -# target_pos, -# target_ori, -# lowerLimits=self.__lower_limits, -# upperLimits=self.__upper_limits, -# jointRanges=self.__joint_ranges, -# restPoses=self.__rest_poses, -# maxNumIterations=100 -# ) -# -# # Slice the IK solution so that only the controlled joints are used. -# jointPoses = jointPoses[:self.__n_joints] -# -# for step in range(max_steps): -# # Command each joint to the desired position. -# for i in range(self.__n_joints): -# pb.setJointMotorControl2( -# bodyIndex=self.__kinova_id, -# jointIndex=i, -# controlMode=pb.POSITION_CONTROL, -# targetPosition=jointPoses[i], -# force=500, -# positionGain=0.05, -# velocityGain=1 -# ) -# -# # Step the simulation. -# pb.stepSimulation() -# time.sleep(self.dt) -# -# # Get the current end-effector state. -# ee_state = pb.getLinkState(self.__kinova_id, self.END_EFFECTOR_INDEX) -# current_pos = np.array(ee_state[0]) -# current_error = np.linalg.norm(np.array(target_pos) - current_pos) -# -# # If within threshold, break out. -# if current_error < error_threshold: -# print("Target reached within threshold.") -# break -# -# # Final achieved state. -# final_state = pb.getLinkState(self.__kinova_id, self.END_EFFECTOR_INDEX) -# final_pos = final_state[0] -# final_ori = final_state[1] -# -# print("Target end-effector position:", target_pos) -# print("Final achieved end-effector position:", final_pos) -# -# def open_gripper(self): -# """ -# Opens the gripper. -# """ -# pb.setJointMotorControl2(self.__kinova_id, self.LEFT_FINGER_JOINT, pb.POSITION_CONTROL, -# targetPosition=self.GRIPPER_OPEN_POS, force=500) -# pb.setJointMotorControl2(self.__kinova_id, self.RIGHT_FINGER_JOINT, pb.POSITION_CONTROL, -# targetPosition=-self.GRIPPER_OPEN_POS, force=500) -# for _ in range(100): -# pb.stepSimulation() -# time.sleep(self.dt) -# -# print("Gripper opened.") -# -# def close_gripper(self): -# """ -# Closes the gripper. -# """ -# pb.setJointMotorControl2(self.__kinova_id, self.LEFT_FINGER_JOINT, pb.POSITION_CONTROL, -# targetPosition=self.GRIPPER_CLOSED_POS, force=500) -# pb.setJointMotorControl2(self.__kinova_id, self.RIGHT_FINGER_JOINT, pb.POSITION_CONTROL, -# targetPosition=self.GRIPPER_CLOSED_POS, force=500) -# for _ in range(100): -# pb.stepSimulation() -# time.sleep(self.dt) -# -# print("Gripper closed.") -# -# def check_collision(self, other_object_id): -# """ -# Determines if there is a collision between the Kinova Gen3 Lite robotic arm -# and another object based on their current positions in the PyBullet simulation. -# -# Args: -# other_object_id (int): The PyBullet body unique ID of the other object. -# -# Returns: -# bool: True if a collision is detected, False otherwise. -# """ -# # Ensure collision detection is up to date -# pb.performCollisionDetection() -# -# # Check for contact points between the arm and the specified object -# contact_points = pb.getContactPoints(bodyA=self.__kinova_id, bodyB=other_object_id) -# -# # Return True if any contact points exist -# return len(contact_points) > 0 -# -# -# def main(): -# """ -# Test the Gen3Lite Arm moving, gripper functionalities, and collision detection. -# """ -# -# # Initialize PyBullet simulation -# pb.connect(pb.GUI, options="--opengl2") -# pb.setGravity(0, 0, -9.8) -# -# # Create the controller -# controller = Gen3LiteArmController() -# pb.setTimeStep(controller.dt) -# -# # Test homing functionality -# print("\nTesting Gen3Lite Arm controller homing...") -# controller.move_to_cartesian([0.4, 0, 0.4], controller.default_ori) -# -# # --- COLLISION DETECTION TEST START --- -# print("\nTesting Collision Detection...") -# -# # 1. Create a dummy box object -# col_box_id = pb.createCollisionShape(pb.GEOM_BOX, halfExtents=[0.05, 0.05, 0.05]) -# -# # 2. Spawn the box far away (at x=1.0) where it shouldn't hit the arm -# box_id = pb.createMultiBody(baseMass=0, baseCollisionShapeIndex=col_box_id, basePosition=[1.0, 0, 0.5]) -# pb.stepSimulation() -# -# is_collision = controller.check_collision(box_id) -# print(f"Box at [1.0, 0, 0.5]. Collision detected? {is_collision} (Expected: False)") -# -# # 3. Move the box to where the arm currently is (approx [0.4, 0, 0.4]) -# print("Moving box to collide with arm...") -# pb.resetBasePositionAndOrientation(box_id, [0.4, 0, 0.4], [0, 0, 0, 1]) -# pb.stepSimulation() -# -# is_collision = controller.check_collision(box_id) -# print(f"Box at [0.4, 0, 0.4]. Collision detected? {is_collision} (Expected: True)") -# -# # Remove the box to continue other tests cleanly -# pb.removeBody(box_id) -# # --- COLLISION DETECTION TEST END --- -# -# controller.move_to_cartesian([-0.4, 0, 0.4], controller.default_ori) -# -# # Test gripper functionality -# print("\nTesting gripper open/close...") -# controller.open_gripper() -# controller.close_gripper() -# -# # Test the direct joint control -# print("\nTesting direct joint control...") -# home_ = [0, 0, 0, 0, 0, -math.pi * 0.5, 0] -# controller.move_to_joint_positions(home_) -# -# print("All tests completed.") -# pb.disconnect() -# -# -# if __name__ == "__main__": -# main() \ No newline at end of file diff --git a/pybullet/robots.py b/pybullet/robots.py new file mode 100644 index 0000000..da763cd --- /dev/null +++ b/pybullet/robots.py @@ -0,0 +1,217 @@ +import pybullet as pb +import time +import math +from typing import List +import numpy as np + +class Gen3LiteArmController(object): + """ + A controller for the Kinova Gen3 Lite robotic arm in PyBullet. + + This class provides methods to control the arm's joints and interact + with it surrounding environment through collision checking. + """ + def __init__(self, dt=1 / 50.0, env_name="Free"): + + self.dt = dt + pb.connect(pb.GUI,) + pb.setGravity(0, 0, -9.8) + pb.setTimeStep(self.dt) + pb.configureDebugVisualizer(pb.COV_ENABLE_GUI, 0) + pb.configureDebugVisualizer(pb.COV_ENABLE_RENDERING, 0) + + # Load the Kinova Gen3 Lite URDF model. + # Ensure the path "gen3lite_urdf/gen3_lite.urdf" exists in your directory + self.__kinova_id = pb.loadURDF("gen3lite_urdf/gen3_lite.urdf", [0, 0, 0], useFixedBase=True) + pb.resetBasePositionAndOrientation(self.__kinova_id, [0, 0, 0.0], [0, 0, 0, 1]) + self.END_EFFECTOR_INDEX = 7 + + self.__n_joints = 7 + self.__lower_limits: List = [-.967, -2, -2.96, 0.19, -2.96, -2.09, -3.05] + self.__upper_limits: List = [.967, 2, 2.96, 2.29, 2.96, 2.09, 3.05] + self.__home_poses: List = [math.pi, 0, 0.5 * math.pi, 0.5 * math.pi, 0.5 * math.pi, -math.pi * 0.5, 0] + + self.joint_ids = [pb.getJointInfo(self.__kinova_id, i) for i in range(self.__n_joints)] + self.joint_ids = [j[0] for j in self.joint_ids if j[2] == pb.JOINT_REVOLUTE] + + # This function creates obstacles and sets a reachable goal + self.createBalloonMaze(env_name) + + # Set the viewer's camera viewpoint + pb.resetDebugVisualizerCamera( + cameraDistance=1.5, + cameraYaw=-120, + cameraPitch=-10, + cameraTargetPosition=self.goal_position + ) + + def getRanges(self): + return (self.__lower_limits,self.__upper_limits) + + def getCurrentJointAngles(self): + angles = [] + for id in self.joint_ids: + joint_state = pb.getJointState(self.__kinova_id,id) + angles.append(joint_state[0]) + return angles + + def setJointAngles(self, joint_angles): + for joint_index, q in enumerate(joint_angles): + pb.resetJointState(self.__kinova_id, joint_index, q) + + def setToHome(self): + self.setJointAngles(self.__home_poses) + + def fkine(self,angles): + # Record where we were (usually home, but just to be safe...) + curr = self.getCurrentJointAngles() + # Move the arm to the goal, specified in joint angles + self.setJointAngles(angles) + # Record the end effectors x,y,z position + state = pb.getLinkState(self.__kinova_id, self.END_EFFECTOR_INDEX) + # Move the arm back to where it began + self.setJointAngles(curr) + + return state[0] + + def execPath(self,path): + self.setJointAngles(self.__home_poses) + + pb.configureDebugVisualizer(pb.COV_ENABLE_RENDERING, 1) + for p in path: + self.setJointAngles(p) + time.sleep(0.25) + + def createBalloonMaze(self,env_name): + if env_name == "Hardest": + self.createHardestMaze() + elif env_name == "Easiest": + self.createEasiestMaze() + elif env_name == "Free": + self.createFreeMaze() + + def createFreeMaze(self): + + # Initialize to home position. + self.setToHome() + + # This is a hard-coded sensible goal to try to reach + self.goal_angles = [0.1026325022237283, -0.2931188624740633, 1.2717083400432991, 0.048794139164578594, 0.07744723004754135, -0.8437927483158898, -0.024709326684397483] + + self.goal_position = self.fkine(self.goal_angles) + + # Make a visual marker for the goal + goal_visual_id = pb.createVisualShape(pb.GEOM_BOX, + halfExtents=[0.05,0.05,0.05], + rgbaColor=[0.0,0.0,1.0,0.5] + ) + + pb.createMultiBody(baseMass=0, basePosition=self.goal_position, baseVisualShapeIndex=goal_visual_id ) + + def createEasiestMaze(self): + self.createFreeMaze() + balloon_collision_id = pb.createCollisionShape(pb.GEOM_SPHERE, radius=0.07) + balloon_visual_id = pb.createVisualShape(pb.GEOM_SPHERE, radius=0.07,rgbaColor=[1.0,0.0,0.0,0.5]) + + x = 0.4 + for y in [-0.125, 0.125]: + for z in [0.25, 0.5]: + box_id = pb.createMultiBody(baseMass=0, basePosition=[x, y, z],baseCollisionShapeIndex=balloon_collision_id, + baseVisualShapeIndex=balloon_visual_id + ) + + def createHardestMaze(self): + self.createEasiestMaze() + + balloon_collision_id = pb.createCollisionShape(pb.GEOM_SPHERE, radius=0.15) + balloon_visual_id = pb.createVisualShape(pb.GEOM_SPHERE, radius=0.15,rgbaColor=[1.0,0.0,0.0,0.5]) + + x = 0 + for y in [-0.3, 0.3]: + for z in [0.2, 0.6]: + box_id = pb.createMultiBody(baseMass=0, basePosition=[x, y, z],baseCollisionShapeIndex=balloon_collision_id, + baseVisualShapeIndex=balloon_visual_id + ) + + def visTreesAndPaths(self,trees,paths,rgbas_in): + + pb.configureDebugVisualizer(pb.COV_ENABLE_RENDERING, 0) + + for tree_idx in range(len(trees)): + tree = trees[tree_idx] + rgba_in = rgbas_in[tree_idx] + for n in tree.node_list: + parent = n.parent + if not parent is None: + self.setJointAngles(n.point) + n_pos = pb.getLinkState(self.__kinova_id, self.END_EFFECTOR_INDEX) + + self.setJointAngles(parent.point) + parent_pos = pb.getLinkState(self.__kinova_id, self.END_EFFECTOR_INDEX) + + point_vis_id = pb.createVisualShape(pb.GEOM_SPHERE, radius=0.005,rgbaColor=rgba_in) + node_sphere_id = pb.createMultiBody(baseMass=0, basePosition=n_pos[0], baseVisualShapeIndex=point_vis_id ) + + pb.addUserDebugLine(lineFromXYZ=n_pos[0],lineToXYZ=parent_pos[0],lineColorRGB=rgba_in[0:3],lineWidth=0.01,lifeTime=0) + + point_vis_id = pb.createVisualShape(pb.GEOM_SPHERE, radius=0.01, + rgbaColor=[1.0, 1.0, 0.0, 0.5]) + + for path in paths: + for idx in range(len(path)): + + parent_pos = self.fkine(path[idx]) + + node_sphere_id = pb.createMultiBody(baseMass=0, basePosition=parent_pos, baseVisualShapeIndex=point_vis_id ) + + if idx+1 < len(path): + child_pos = self.fkine(path[idx+1]) + + pb.addUserDebugLine(lineFromXYZ=parent_pos,lineToXYZ=child_pos,lineColorRGB=[1.0, 1.0, 0.0],lineWidth=0.02,lifeTime=0) + + self.setToHome() + pb.configureDebugVisualizer(pb.COV_ENABLE_RENDERING, 1) + print("Finished plotting tree. Press q to continue.") + + while True: + pb.stepSimulation() + time.sleep(1./240.) + + keys = pb.getKeyboardEvents() + + if ord('q') in keys and keys[ord('q')] & pb.KEY_WAS_TRIGGERED: + print("Q pressed. Continuing.") + break + + def collision_free(self,p1,p2): + + self.setJointAngles(p1) + if self.check_collision(): + return False + self.setJointAngles(p2) + if self.check_collision(): + return False + return True + + def check_collision(self): + """ + Checks for collisions between the robot and ANY other body in the environment, as well as self-collisions + (robot links hitting each other). + + Returns: + bool: True if any collision is detected, False otherwise. + """ + # Ensure collision detection is up to date + pb.performCollisionDetection() + + # Iterate over all bodies in the PyBullet simulation + for i in range(pb.getNumBodies()): + other_body_id = pb.getBodyUniqueId(i) + contact_points = pb.getContactPoints(bodyA=self.__kinova_id, bodyB=other_body_id) + + # If contacts are found, return True immediately + if contact_points is None or len(contact_points) > 0: + return True + + # If loop completes without returning, no collisions were found + return False diff --git a/pybullet/rrt_path.npy b/pybullet/rrt_path.npy new file mode 100644 index 0000000..40d0a0e Binary files /dev/null and b/pybullet/rrt_path.npy differ diff --git a/pybullet/rrt_solution.py b/pybullet/rrt_solution.py new file mode 100644 index 0000000..3aac4d9 --- /dev/null +++ b/pybullet/rrt_solution.py @@ -0,0 +1,163 @@ +import numpy as np +from tqdm import tqdm +from scipy.spatial import cKDTree +import random +import argparse +import gen3lite_controller_collision_detection + +class Node: + def __init__(self, point): + self.point = np.array(point) + self.parent = None + +class Tree: + def __init__(self, node_list, kdtree): + self.node_list = node_list + self.kdtree = kdtree + + def add(self,new_point): + self.node_list.append(new_point) + + if len(self.node_list) % 50 == 0: + data = [n.point for n in self.node_list] + self.kdtree = cKDTree(data) + +class RRT: + def __init__(self, step_size=0.1,max_iter=50000,env_name="Free"): + self.controller = gen3lite_controller_collision_detection.Gen3LiteArmController(env_name=env_name) + + self.start = Node(self.controller.getCurrentJointAngles()) + self.goal = Node(self.controller.goal_angles) + self.rand_ranges = self.controller.getRanges() + self.step_size = step_size + self.max_iter = max_iter + self.start_tree = Tree([self.start],cKDTree([self.start.point])) + self.goal_tree = Tree([self.goal],cKDTree([self.goal.point])) + self.path_to_goal = [] + + def add_node(self,tree,target=None): + if target is None: + rnd_point = self.sample() + else: + rnd_point = target.point + nearest_node = self.nearest_node(rnd_point,tree) + new_node = self.steer(nearest_node, rnd_point) + + if self.collision_free(nearest_node.point, new_node.point): + tree.add(new_node) + return new_node + else: + return None + + def plan(self): + + for k in tqdm(range(self.max_iter)): + if k % 2: + new_node = self.add_node(self.start_tree) + + while(new_node is not None): + ret = self.add_node(self.goal_tree,new_node) + if ret == None: + break + if self.reached_goal(ret,goal=new_node): + self.path_to_goal = self.extract_path(new_node,ret) + return True + + else: + new_node = self.add_node(self.goal_tree) + + while(new_node is not None): + ret = self.add_node(self.start_tree,new_node) + if ret == None: + break + + if self.reached_goal(ret,goal=new_node): + self.path_to_goal = self.extract_path(ret,new_node) + return True + + return False + + def sample(self): + point = [] + for i in range(0,len(self.rand_ranges[0])): + point.append(random.uniform(self.rand_ranges[0][i], self.rand_ranges[1][i])) + return np.array(point) + + def nearest_node(self, point, tree): + _, idx = tree.kdtree.query(point) + return tree.node_list[idx] + + def steer(self, from_node, to_point): + direction = to_point - from_node.point + distance = np.linalg.norm(direction) + + if distance < self.step_size: + new_point = to_point + else: + direction = direction / distance + new_point = from_node.point + self.step_size * direction + + new_node = Node(new_point) + new_node.parent = from_node + return new_node + + def collision_free(self, p1, p2): + return self.controller.collision_free(p1,p2) + + def reached_goal(self, node,goal=None): + if goal is None: + goal = self.goal + + return np.linalg.norm(node.point - goal.point) < self.step_size and self.collision_free(node.point,goal.point) + + def extract_path(self, start_node,goal_node): + # Build the start-tree path, leaf to root (backwards) + start_tree_path = [] + while start_node is not None: + start_tree_path.append(start_node.point) + start_node = start_node.parent + + # Build the goal-tree path, leaf to root (forwards) + goal_tree_path = [] + while goal_node is not None: + goal_tree_path.append(goal_node.point) + goal_node = goal_node.parent + + # First add the start path, reversing + overall_path = start_tree_path[::-1] + # Add the goal path + overall_path.extend(goal_tree_path) + return overall_path + +if __name__ == "__main__": + + parser = argparse.ArgumentParser( + prog='arm_rrt', + description='Plans and executes paths for arms around obstacles.') + parser.add_argument('--filename',default='rrt_path.npy') + parser.add_argument('-e', '--environment',default='middle') + parser.add_argument('-p', '--plan',action='store_true') + parser.add_argument('-r', '--run',action='store_true') + args = parser.parse_args() + + rrt = RRT(env_name=args.environment) + + if args.plan: + success = rrt.plan() + + if success: + print("Tree planning reached the goal.") + np.save(args.filename,rrt.path_to_goal) + else: + print("Failed to find a path to the goal.") + + rrt.controller.visTreesAndPaths( + [rrt.start_tree,rrt.goal_tree], + [rrt.path_to_goal], + rgbas_in=[[0.5,0.0,0.5,1.0],[0.902,0.106,0.714,1.0]] + ) + + if args.run: + path_to_goal = np.load(args.filename) + rrt.controller.execPath(path_to_goal) + \ No newline at end of file