def main(): sim = True # instantiate an ada ada = adapy.Ada(True) armHome = [-1.5, 3.22, 1.23, -2.19, 1.8, 1.2] goalConfig = [-1.72, 4.44, 2.02, -2.04, 2.66, 1.39] delta = 0.25 eps = 0.2 if sim: ada.set_positions(goalConfig) # launch viewer viewer = ada.start_viewer("dart_markers/simple_trajectories", "map") # add objects to world canURDFUri = "package://pr_assets/data/objects/can.urdf" sodaCanPose = [0.25, -0.35, 0.0, 0, 0, 0, 1] tableURDFUri = "package://pr_assets/data/furniture/uw_demo_table.urdf" tablePose = [0.3, 0.0, -0.7, 0.707107, 0, 0, 0.707107] world = ada.get_world() can = world.add_body_from_urdf(canURDFUri, sodaCanPose) table = world.add_body_from_urdf(tableURDFUri, tablePose) # add collision constraints collision_free_constraint = ada.set_up_collision_detection( ada.get_arm_state_space(), ada.get_arm_skeleton(), [can, table]) full_collision_constraint = ada.get_full_collision_constraint( ada.get_arm_state_space(), ada.get_arm_skeleton(), collision_free_constraint) # easy goal adaRRT = AdaRRT(start_state=np.array(armHome), goal_state=np.array(goalConfig), ada=ada, ada_collision_constraint=full_collision_constraint, step_size=delta, goal_precision=eps) rospy.sleep(1.0) path = adaRRT.build() if path is not None: print("Path waypoints:") print(np.asarray(path)) waypoints = [] for i, waypoint in enumerate(path): waypoints.append((0.0 + i, waypoint)) t0 = time.clock() traj = ada.compute_smooth_joint_space_path(ada.get_arm_state_space(), waypoints) t = time.clock() - t0 print(str(t) + "seconds elapsed") raw_input('Press ENTER to execute trajectory and exit') ada.execute_trajectory(traj) rospy.sleep(1.0)
#!/usr/bin/env python import adapy import rospy import sys import pdb from moveit_ros_planning_interface._moveit_roscpp_initializer import roscpp_init rospy.init_node("adapy_simple_traj") roscpp_init('adapy_simple_traj', []) rate = rospy.Rate(10) IS_SIM = True if not rospy.is_shutdown(): ada = adapy.Ada(IS_SIM) if not IS_SIM: if not ada.start_trajectory_controllers(): print("Could not start trajectory controller.") sys.exit(1) rospy.sleep(1) # wait for ada to initialize viewer = ada.start_viewer("dart_markers/simple_trajectories", "map") canURDFUri = "package://pr_assets/data/objects/can.urdf" sodaCanPose = [1.0, 0.0, 0.73, 0, 0, 0, 1] tableURDFUri = "package://pr_assets/data/furniture/uw_demo_table.urdf" tablePose = [1., 0.0, 0.0, 0.707107, 0, 0, 0.707107] world = ada.get_world() can = world.add_body_from_urdf(canURDFUri, sodaCanPose) table = world.add_body_from_urdf(tableURDFUri, tablePose)
#!/usr/bin/env python import adapy import rospy import pdb pdb.set_trace() rospy.init_node("adapy_simple_traj") rate = rospy.Rate(10) if not rospy.is_shutdown(): ada = adapy.Ada(True) viewer = ada.start_viewer("dart_markers/simple_trajectories", "map") canURDFUri = "package://pr_assets/data/objects/can.urdf" sodaCanPose = [1.0, 0.0, 0.73, 0, 0, 0, 1] tableURDFUri = "package://pr_assets/data/furniture/uw_demo_table.urdf" tablePose = [1., 0.0, 0.0, 0.707107, 0, 0, 0.707107] world = ada.get_world() can = world.add_body_from_urdf(canURDFUri, sodaCanPose) table = world.add_body_from_urdf(tableURDFUri, tablePose) collision = ada.get_self_collision_constraint() arm_skeleton = ada.get_arm_skeleton() positions = arm_skeleton.get_positions() arm_state_space = ada.get_arm_state_space() positions2 = positions.copy() positions3 = positions.copy() positions4 = positions.copy()
def main(if_sim): # initialize roscpp, if it's for real robot # http://wiki.ros.org/ROS/Tutorials/Using%20a%20C%2B%2B%20class%20in%20Python if not if_sim: from moveit_ros_planning_interface._moveit_roscpp_initializer import roscpp_init roscpp_init('soda_grasp_ik', []) # instantiate an ada ada = adapy.Ada(if_sim) # launch viewer viewer = ada.start_viewer("dart_markers/sode_grasp", "map") world = ada.get_world() hand = ada.get_hand() hand_node = hand.get_endeffector_body_node() arm_skeleton = ada.get_arm_skeleton() arm_state_space = ada.get_arm_state_space() # joint positions of the starting pose arm_home = [-1.5, 3.22, 1.23, -2.19, 1.8, 1.2] if if_sim: ada.set_positions(arm_home) else: raw_input("Please move arm to home position with the joystick. " + "Press ENTER to continue...") viewer.add_frame(hand_node) # add objects to world soda_pose = np.eye(4) soda_pose[0, 3] = 0.25 soda_pose[1, 3] = -0.35 sodaURDFUri = "package://pr_assets/data/objects/can.urdf" soda = world.add_body_from_urdf_matrix(sodaURDFUri, soda_pose) tableURDFUri = "package://pr_assets/data/furniture/uw_demo_table.urdf" # x, y, z, rw, rx, ry, rz table_pose = [0.3, 0.0, -0.75, 0.707107, 0., 0., 0.707107] table = world.add_body_from_urdf(tableURDFUri, table_pose) # add collision constraints collision_free_constraint = ada.set_up_collision_detection( ada.get_arm_state_space(), ada.get_arm_skeleton(), [soda, table]) full_collision_constraint = ada.get_full_collision_constraint( ada.get_arm_state_space(), ada.get_arm_skeleton(), collision_free_constraint) rospy.sleep(1.) raw_input("Press ENTER to generate the TSR...") # create TSR sodaTSR = createSodaTSR(soda_pose, hand) marker = viewer.add_tsr_marker(sodaTSR) raw_input("Press ENTER to start planning goals...") # set up IK generator ik_sampleable = adapy.create_ik(arm_skeleton, arm_state_space, sodaTSR, hand_node) ik_generator = ik_sampleable.create_sample_generator() configurations = [] samples = 0 maxSamples = 10 while samples < maxSamples and ik_generator.can_sample(): goal_state = ik_generator.sample(arm_state_space) samples += 1 if len(goal_state) == 0: continue configurations.append(goal_state) if len(configurations) == 0: print("No valid configurations found!") if if_sim: ada.set_positions(arm_home) raw_input("Press ENTER to start RRT planning...") trajectory = None for configuration in configurations: # Your AdaRRT planner ### FILL in your code here if trajectory: break if not trajectory: print("Failed to find a solution!") sys.exit(1) else: print("Found a trajectory!") # smooth the RRTs trajectory shortcut(trajectory, ada, full_collision_constraint) waypoints = [] for i, waypoint in enumerate(trajectory): waypoints.append((0.0 + i, waypoint)) # compute trajectory in joint space t0 = time.clock() traj = ada.compute_joint_space_path(ada.get_arm_state_space(), waypoints) retimed_traj = ada.compute_retime_path(ada.get_arm_skeleton(), traj) t = time.clock() - t0 print(str(t) + "seconds elapsed") raw_input('Press ENTER to execute the trajectory...') # execute the trajectory if not if_sim: ada.start_trajectory_executor() ada.execute_trajectory(retimed_traj) raw_input('Press ENTER after robot has approached the can...') if not if_sim: ada.set_positions(waypoints[-1][1]) # execute the grasp print("Closing hand") ### FILL in your code here raw_input('Press ENTER after robot has succeeded closing the hand...') if if_sim: hand.grab(soda) # compute the Jacobian pseudo-inverse for moving the hand upwards if if_sim: N = 50 Dz = 0.01 # waypoints = [] for i in range(N): q = arm_skeleton.get_positions() ### FILL in your code here ada.set_positions(q) viewer.update() time.sleep(0.05) else: # in real world q = arm_skeleton.get_positions() jac = arm_skeleton.get_jacobian(hand.get_endeffector_body_node()) delta_x = np.array([0, 0, 0, 0.5, 0, 0]) delta_q = np.matmul(np.linalg.pinv(jac), delta_x) q = q + delta_q traj = ada.plan_to_configuration(ada.get_arm_state_space(), ada.get_arm_skeleton(), q) retimed_traj = ada.compute_retime_path(ada.get_arm_skeleton(), traj) raw_input('Press ENTER to execute the lift') ada.execute_trajectory(retimed_traj) raw_input('Press ENTER after robot has succeeded lifting up the can...') # clean the scene world.remove_skeleton(soda) world.remove_skeleton(table)