def parse_robot(robot_json): pose = parse_pose(robot_json) if robot_json['name'] == 'pr2': with HideOutput(True): robot_id = load_model(DRAKE_PR2_URDF, fixed_base=True) set_group_conf(robot_id, 'base', base_values_from_pose(pose)) else: # TODO: set the z? #set_pose(robot_id, pose) raise NotImplementedError(robot_json['name']) for joint, values in robot_json['conf'].items(): [value] = values if has_joint(robot_id, joint): set_joint_position(robot_id, joint_from_name(robot_id, joint), value) else: print('Robot {} lacks joint {}'.format(robot_json['name'], joint)) if robot_json['name'] == 'pr2': set_group_conf(robot_id, 'torso', [0.2]) set_group_conf(robot_id, 'left_arm', REST_LEFT_ARM) set_group_conf(robot_id, 'right_arm', rightarm_from_leftarm(REST_LEFT_ARM)) return robot_id
def command_torso(self, pose, timeout=2.0, blocking=True): # Commands Torso to a certain position with ClientSaver(self.client): torso_joint = joint_from_name(self.robot, self.TORSO) set_joint_position(self.robot, torso_joint, pose) if self.execute_motor_control: control_joint(self.robot, torso_joint, pose) else: set_joint_position(self.robot, torso_joint, pose)
def test_kinematic(robot, door, target_x): wait_if_gui('Begin?') robot_joints = get_movable_joints(robot)[:3] joint = robot_joints[0] start_x = get_joint_position(robot, joint) num_steps = int(math.ceil(abs(target_x - start_x) / 1e-2)) for x in interpolate(start_x, target_x, num_steps=num_steps): set_joint_position(robot, joint=joint, value=x) #with LockRenderer(): solve_collision_free(door, robot, draw=False) wait_for_duration(duration=1e-2) #wait_if_gui() wait_if_gui('Finish?')
def open_door(self, joint): set_joint_position(self.kitchen, joint, self.open_conf(joint))
def close_door(self, joint): set_joint_position(self.kitchen, joint, self.closed_conf(joint))
def main(floor_width=2.0): # Creates a pybullet world and a visualizer for it connect(use_gui=True) identity_pose = (unit_point(), unit_quat()) origin_handles = draw_pose( identity_pose, length=1.0 ) # Draws the origin coordinate system (x:RED, y:GREEN, z:BLUE) # Bodies are described by an integer index floor = create_box(w=floor_width, l=floor_width, h=0.001, color=TAN) # Creates a tan box object for the floor set_point(floor, [0, 0, -0.001 / 2.]) # Sets the [x,y,z] translation of the floor obstacle = create_box(w=0.5, l=0.5, h=0.1, color=RED) # Creates a red box obstacle set_point( obstacle, [0.5, 0.5, 0.1 / 2.]) # Sets the [x,y,z] position of the obstacle print('Position:', get_point(obstacle)) set_euler(obstacle, [0, 0, np.pi / 4 ]) # Sets the [roll,pitch,yaw] orientation of the obstacle print('Orientation:', get_euler(obstacle)) with LockRenderer( ): # Temporarily prevents the renderer from updating for improved loading efficiency with HideOutput(): # Temporarily suppresses pybullet output robot = load_model(ROOMBA_URDF) # Loads a robot from a *.urdf file robot_z = stable_z( robot, floor ) # Returns the z offset required for robot to be placed on floor set_point(robot, [0, 0, robot_z]) # Sets the z position of the robot dump_body(robot) # Prints joint and link information about robot set_all_static() # Joints are also described by an integer index # The turtlebot has explicit joints representing x, y, theta x_joint = joint_from_name(robot, 'x') # Looks up the robot joint named 'x' y_joint = joint_from_name(robot, 'y') # Looks up the robot joint named 'y' theta_joint = joint_from_name( robot, 'theta') # Looks up the robot joint named 'theta' joints = [x_joint, y_joint, theta_joint] base_link = link_from_name( robot, 'base_link') # Looks up the robot link named 'base_link' world_from_obstacle = get_pose( obstacle ) # Returns the pose of the origin of obstacle wrt the world frame obstacle_aabb = get_subtree_aabb(obstacle) draw_aabb(obstacle_aabb) random.seed(0) # Sets the random number generator state handles = [] for i in range(10): for handle in handles: remove_debug(handle) print('\nIteration: {}'.format(i)) x = random.uniform(-floor_width / 2., floor_width / 2.) set_joint_position(robot, x_joint, x) # Sets the current value of the x joint y = random.uniform(-floor_width / 2., floor_width / 2.) set_joint_position(robot, y_joint, y) # Sets the current value of the y joint yaw = random.uniform(-np.pi, np.pi) set_joint_position(robot, theta_joint, yaw) # Sets the current value of the theta joint values = get_joint_positions( robot, joints) # Obtains the current values for the specified joints print('Joint values: [x={:.3f}, y={:.3f}, yaw={:.3f}]'.format(*values)) world_from_robot = get_link_pose( robot, base_link) # Returns the pose of base_link wrt the world frame position, quaternion = world_from_robot # Decomposing pose into position and orientation (quaternion) x, y, z = position # Decomposing position into x, y, z print('Base link position: [x={:.3f}, y={:.3f}, z={:.3f}]'.format( x, y, z)) euler = euler_from_quat( quaternion) # Converting from quaternion to euler angles roll, pitch, yaw = euler # Decomposing orientation into roll, pitch, yaw print('Base link orientation: [roll={:.3f}, pitch={:.3f}, yaw={:.3f}]'. format(roll, pitch, yaw)) handles.extend( draw_pose(world_from_robot, length=0.5) ) # # Draws the base coordinate system (x:RED, y:GREEN, z:BLUE) obstacle_from_robot = multiply( invert(world_from_obstacle), world_from_robot) # Relative transformation from robot to obstacle robot_aabb = get_subtree_aabb( robot, base_link) # Computes the robot's axis-aligned bounding box (AABB) lower, upper = robot_aabb # Decomposing the AABB into the lower and upper extrema center = (lower + upper) / 2. # Computing the center of the AABB extent = upper - lower # Computing the dimensions of the AABB handles.extend(draw_aabb(robot_aabb)) collision = pairwise_collision( robot, obstacle ) # Checks whether robot is currently colliding with obstacle print('Collision: {}'.format(collision)) wait_for_duration(1.0) # Like sleep() but also updates the viewer wait_for_user() # Like raw_input() but also updates the viewer # Destroys the pybullet world disconnect()
def fn(body, pose, grasp): obstacles = [body] + fixed gripper_pose = end_effector_from_body(pose.pose, grasp.grasp_pose) approach_pose = approach_from_grasp(grasp.approach_pose, gripper_pose) draw_pose(get_tool_pose(robot, ARM), length=0.04) draw_pose(approach_pose, length=0.04) draw_pose(gripper_pose, length=0.04) # print(movable_joints) # print(torso_arm) # wait_for_interrupt() # TODO: gantry_x_joint # TODO: proper inverse reachability base_link = link_from_name(robot, IK_BASE_FRAMES[ARM]) base_pose = get_link_pose(robot, base_link) body_point_in_base = point_from_pose(multiply(invert(base_pose), pose.pose)) y_joint = joint_from_name(robot, '{}_gantry_y_joint'.format(prefix_from_arm(ARM))) initial_y = get_joint_position(robot, y_joint) ik_y = initial_y + SIGN_FROM_ARM[ARM]*body_point_in_base[1] set_joint_position(robot, y_joint, ik_y) for _ in range(num_attempts): if USE_IKFAST: q_approach = sample_tool_ik(robot, ARM, approach_pose) if q_approach is not None: set_joint_positions(robot, torso_arm, q_approach) else: set_joint_positions(robot, torso_arm, sample_fn()) # Random seed q_approach = inverse_kinematics(robot, grasp.link, approach_pose) if (q_approach is None) or any(pairwise_collision(robot, b) for b in obstacles): print('- ik for approaching fails!') continue conf = BodyConf(robot, joints=arm_joints) if USE_IKFAST: q_grasp = sample_tool_ik(robot, ARM, gripper_pose, closest_only=True) if q_grasp is not None: set_joint_positions(robot, torso_arm, q_grasp) else: conf.assign() q_grasp = inverse_kinematics(robot, grasp.link, gripper_pose) if (q_grasp is None) or any(pairwise_collision(robot, b) for b in obstacles): print('- ik for grasp fails!') continue if teleport: path = [q_approach, q_grasp] else: conf.assign() #direction, _ = grasp.approach_pose #path = workspace_trajectory(robot, grasp.link, point_from_pose(approach_pose), -direction, # quat_from_pose(approach_pose)) path = plan_direct_joint_motion(robot, torso_arm, q_grasp, obstacles=obstacles, self_collisions=self_collisions) if path is None: if DEBUG_FAILURE: print('Approach motion failed') continue command = Command([BodyPath(robot, path, joints=torso_arm), Attach(body, robot, grasp.link), BodyPath(robot, path[::-1], joints=torso_arm, attachments=[grasp])]) return (conf, command) # TODO: holding collisions return None
def open_gripper(robot): for joint in get_movable_joints(robot): set_joint_position(robot, joint, get_max_limit(robot, joint))
def main(): # TODO: teleporting kuka arm parser = argparse.ArgumentParser() # Automatically includes help parser.add_argument('-viewer', action='store_true', help='enable viewer.') args = parser.parse_args() client = connect(use_gui=args.viewer) add_data_path() #planeId = p.loadURDF("plane.urdf") table = p.loadURDF("table/table.urdf", 0, 0, 0, 0, 0, 0.707107, 0.707107) box = create_box(.07, .05, .15) # boxId = p.loadURDF("r2d2.urdf",cubeStartPos, cubeStartOrientation) #pr2 = p.loadURDF("pr2_description/pr2.urdf") pr2 = p.loadURDF("pr2_description/pr2_fixed_torso.urdf") #pr2 = p.loadURDF("/Users/caelan/Programs/Installation/pr2_drake/pr2_local2.urdf",) #useFixedBase=0,) #flags=p.URDF_USE_SELF_COLLISION) #flags=p.URDF_USE_SELF_COLLISION_EXCLUDE_PARENT) #flags=p.URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS) #pr2 = p.loadURDF("pr2_drake/urdf/pr2_simplified.urdf", useFixedBase=False) initially_colliding = get_colliding_links(pr2) print len(initially_colliding) origin = (0, 0, 0) print p.getNumConstraints() # TODO: no way of controlling the base position by itself # TODO: PR2 seems to collide with itself frequently # real_time = False # p.setRealTimeSimulation(real_time) # left_joints = [joint_from_name(pr2, name) for name in LEFT_JOINT_NAMES] # control_joints(pr2, left_joints, TOP_HOLDING_LEFT_ARM) # while True: # control_joints(pr2, left_joints, TOP_HOLDING_LEFT_ARM) # if not real_time: # p.stepSimulation() # A CollisionMap robot allows the user to specify self-collision regions indexed by the values of two joints. # GetRigidlyAttachedLinks print pr2 # for i in range (10000): # p.stepSimulation() # time.sleep(1./240.) #print get_joint_names(pr2) print[get_joint_name(pr2, joint) for joint in get_movable_joints(pr2)] print get_joint_position(pr2, joint_from_name(pr2, TORSO_JOINT_NAME)) #open_gripper(pr2, joint_from_name(pr2, LEFT_GRIPPER)) #print get_joint_limits(pr2, joint_from_name(pr2, LEFT_GRIPPER)) #print get_joint_position(pr2, joint_from_name(pr2, LEFT_GRIPPER)) print self_collision(pr2) """ print p.getNumConstraints() constraint = fixed_constraint(pr2, -1, box, -1) # table p.changeConstraint(constraint) print p.getNumConstraints() print p.getConstraintInfo(constraint) print p.getConstraintState(constraint) p.stepSimulation() raw_input('Continue?') set_point(pr2, (-2, 0, 0)) p.stepSimulation() p.changeConstraint(constraint) print p.getConstraintInfo(constraint) print p.getConstraintState(constraint) raw_input('Continue?') print get_point(pr2) raw_input('Continue?') """ # TODO: would be good if we could set the joint directly print set_joint_position(pr2, joint_from_name(pr2, TORSO_JOINT_NAME), 0.2) # Updates automatically print get_joint_position(pr2, joint_from_name(pr2, TORSO_JOINT_NAME)) #return left_joints = [joint_from_name(pr2, name) for name in LEFT_JOINT_NAMES] right_joints = [joint_from_name(pr2, name) for name in RIGHT_JOINT_NAMES] print set_joint_positions( pr2, left_joints, TOP_HOLDING_LEFT_ARM) # TOP_HOLDING_LEFT_ARM | SIDE_HOLDING_LEFT_ARM print set_joint_positions( pr2, right_joints, REST_RIGHT_ARM) # TOP_HOLDING_RIGHT_ARM | REST_RIGHT_ARM print get_body_name(pr2) print get_body_names() # print p.getBodyUniqueId(pr2) print get_joint_names(pr2) #for joint, value in zip(LEFT_ARM_JOINTS, REST_LEFT_ARM): # set_joint_position(pr2, joint, value) # for name, value in zip(LEFT_JOINT_NAMES, REST_LEFT_ARM): # joint = joint_from_name(pr2, name) # #print name, joint, get_joint_position(pr2, joint), value # print name, get_joint_limits(pr2, joint), get_joint_type(pr2, joint), get_link_name(pr2, joint) # set_joint_position(pr2, joint, value) # #print name, joint, get_joint_position(pr2, joint), value # for name, value in zip(RIGHT_JOINT_NAMES, REST_RIGHT_ARM): # set_joint_position(pr2, joint_from_name(pr2, name), value) print p.getNumJoints(pr2) jointId = 0 print p.getJointInfo(pr2, jointId) print p.getJointState(pr2, jointId) # for i in xrange(10): # #lower, upper = BASE_LIMITS # #q = np.random.rand(len(lower))*(np.array(upper) - np.array(lower)) + lower # q = np.random.uniform(*BASE_LIMITS) # theta = np.random.uniform(*REVOLUTE_LIMITS) # quat = z_rotation(theta) # print q, theta, quat, env_collision(pr2) # #set_point(pr2, q) # set_pose(pr2, q, quat) # #p.getMouseEvents() # #p.getKeyboardEvents() # raw_input('Continue?') # Stalls because waiting for input # # # TODO: self collisions # for i in xrange(10): # for name in LEFT_JOINT_NAMES: # joint = joint_from_name(pr2, name) # value = np.random.uniform(*get_joint_limits(pr2, joint)) # set_joint_position(pr2, joint, value) # raw_input('Continue?') #start = (-2, -2, 0) #set_base_values(pr2, start) # #start = get_base_values(pr2) # goal = (2, 2, 0) # p.addUserDebugLine(start, goal, lineColorRGB=(1, 1, 0)) # addUserDebugText # print start, goal # raw_input('Plan?') # path = plan_base_motion(pr2, goal) # print path # if path is None: # return # print len(path) # for bq in path: # set_base_values(pr2, bq) # raw_input('Continue?') # left_joints = [joint_from_name(pr2, name) for name in LEFT_JOINT_NAMES] # for joint in left_joints: # print joint, get_joint_name(pr2, joint), get_joint_limits(pr2, joint), \ # is_circular(pr2, joint), get_joint_position(pr2, joint) # # #goal = np.zeros(len(left_joints)) # goal = [] # for name, value in zip(LEFT_JOINT_NAMES, REST_LEFT_ARM): # joint = joint_from_name(pr2, name) # goal.append(wrap_joint(pr2, joint, value)) # # path = plan_joint_motion(pr2, left_joints, goal) # print path # for q in path:s # set_joint_positions(pr2, left_joints, q) # raw_input('Continue?') print p.JOINT_REVOLUTE, p.JOINT_PRISMATIC, p.JOINT_FIXED, p.JOINT_POINT2POINT, p.JOINT_GEAR # 0 1 4 5 6 movable_joints = get_movable_joints(pr2) print len(movable_joints) for joint in xrange(get_num_joints(pr2)): if is_movable(pr2, joint): print joint, get_joint_name(pr2, joint), get_joint_type( pr2, joint), get_joint_limits(pr2, joint) #joints = [joint_from_name(pr2, name) for name in LEFT_JOINT_NAMES] #set_joint_positions(pr2, joints, sample_joints(pr2, joints)) #print get_joint_positions(pr2, joints) # Need to print before the display updates? # set_base_values(pr2, (1, -1, -np.pi/4)) # movable_joints = get_movable_joints(pr2) # gripper_pose = get_link_pose(pr2, link_from_name(pr2, LEFT_ARM_LINK)) # print gripper_pose # print get_joint_positions(pr2, movable_joints) # p.addUserDebugLine(origin, gripper_pose[0], lineColorRGB=(1, 0, 0)) # p.stepSimulation() # raw_input('Pre2 IK') # set_joint_positions(pr2, left_joints, SIDE_HOLDING_LEFT_ARM) # TOP_HOLDING_LEFT_ARM | SIDE_HOLDING_LEFT_ARM # print get_joint_positions(pr2, movable_joints) # p.stepSimulation() # raw_input('Pre IK') # conf = inverse_kinematics(pr2, gripper_pose) # Doesn't automatically set configuraitons # print conf # print get_joint_positions(pr2, movable_joints) # set_joint_positions(pr2, movable_joints, conf) # print get_link_pose(pr2, link_from_name(pr2, LEFT_ARM_LINK)) # #print get_joint_positions(pr2, movable_joints) # p.stepSimulation() # raw_input('Post IK') # return # print pose_from_tform(TOOL_TFORM) # gripper_pose = get_link_pose(pr2, link_from_name(pr2, LEFT_ARM_LINK)) # #gripper_pose = multiply(gripper_pose, TOOL_POSE) # #gripper_pose = get_gripper_pose(pr2) # for i, grasp_pose in enumerate(get_top_grasps(box)): # grasp_pose = multiply(TOOL_POSE, grasp_pose) # box_pose = multiply(gripper_pose, grasp_pose) # set_pose(box, *box_pose) # print get_pose(box) # raw_input('Grasp {}'.format(i)) # return torso = joint_from_name(pr2, TORSO_JOINT_NAME) torso_point, torso_quat = get_link_pose(pr2, torso) #torso_constraint = p.createConstraint(pr2, torso, -1, -1, # p.JOINT_FIXED, jointAxis=[0] * 3, # JOINT_FIXED # parentFramePosition=torso_point, # childFramePosition=torso_quat) create_inverse_reachability(pr2, box, table) ir_database = load_inverse_reachability() print len(ir_database) return link = link_from_name(pr2, LEFT_ARM_LINK) point, quat = get_link_pose(pr2, link) print point, quat p.addUserDebugLine(origin, point, lineColorRGB=(1, 1, 0)) # addUserDebugText raw_input('Continue?') current_conf = get_joint_positions(pr2, movable_joints) #ik_conf = p.calculateInverseKinematics(pr2, link, point) #ik_conf = p.calculateInverseKinematics(pr2, link, point, quat) min_limits = [get_joint_limits(pr2, joint)[0] for joint in movable_joints] max_limits = [get_joint_limits(pr2, joint)[1] for joint in movable_joints] max_velocities = [ get_max_velocity(pr2, joint) for joint in movable_joints ] # Range of Jacobian print min_limits print max_limits print max_velocities ik_conf = p.calculateInverseKinematics(pr2, link, point, quat, lowerLimits=min_limits, upperLimits=max_limits, jointRanges=max_velocities, restPoses=current_conf) value_from_joint = dict(zip(movable_joints, ik_conf)) print[value_from_joint[joint] for joint in joints] #print len(ik_conf), ik_conf set_joint_positions(pr2, movable_joints, ik_conf) #print len(movable_joints), get_joint_positions(pr2, movable_joints) print get_joint_positions(pr2, joints) raw_input('Finish?') p.disconnect()