def closed_conf(self, joint): lower, upper = get_joint_limits(self.kitchen, joint) if 'drawer' in get_joint_name(self.kitchen, joint): fraction = 0.9 return fraction * lower + (1 - fraction) * upper if 'left' in get_joint_name(self.kitchen, joint): return upper return lower
def main(): connect(use_gui=True) add_data_path() plane = p.loadURDF("plane.urdf") with HideOutput(): with LockRenderer(): robot = load_model(FRANKA_URDF, fixed_base=True) dump_body(robot) print('Start?') wait_for_user() tool_link = link_from_name(robot, 'panda_hand') joints = get_movable_joints(robot) print('Joints', [get_joint_name(robot, joint) for joint in joints]) sample_fn = get_sample_fn(robot, joints) for i in range(10): print('Iteration:', i) conf = sample_fn() set_joint_positions(robot, joints, conf) wait_for_user() test_retraction(robot, PANDA_INFO, tool_link, max_distance=0.01, max_time=0.05) disconnect()
def update_state(self): # TODO: apply this directly from observations # No use applying this to base confs self.base_conf = FConf(self.world.robot, self.world.base_joints, init=True) arm_conf = FConf(self.world.robot, self.world.arm_joints, init=True) if (self.arm_conf is None) or not are_confs_close( arm_conf, self.arm_conf, tol=ARM_TOLERANCE): self.arm_conf = arm_conf else: print('At anticipated arm conf') gripper_conf = FConf(self.world.robot, self.world.gripper_joints, init=True) if (self.gripper_conf is None) or not are_confs_close( gripper_conf, self.gripper_conf, tol=GRIPPER_TOLERANCE): self.gripper_conf = gripper_conf else: print('At anticipated gripper conf') # TODO: do I still need to test if the current values are equal to the last ones? for joint in self.world.kitchen_joints: name = get_joint_name(self.world.kitchen, joint) position = get_joint_position(self.world.kitchen, joint) self.update_door_conf(name, position) self.update_door_conf(name, position) #wait_for_user() return self.check_consistent()
def main(): # The URDF loader seems robust to package:// and slightly wrong relative paths? connect(use_gui=True) add_data_path() plane = p.loadURDF("plane.urdf") #with HideOutput(): with LockRenderer(): robot = load_model(MOVO_URDF, fixed_base=True) dump_body(robot) print('Start?') wait_for_user() #joint_names = HEAD_JOINTS #joints = joints_from_names(robot, joint_names) joints = get_movable_joints(robot) print('Joints', [get_joint_name(robot, joint) for joint in joints]) sample_fn = get_sample_fn(robot, joints) for i in range(10): print('Iteration:', i) conf = sample_fn() set_joint_positions(robot, joints, conf) wait_for_user() disconnect()
def test_confs(robot, num_samples=10): joints = get_movable_joints(robot) print('Joints', [get_joint_name(robot, joint) for joint in joints]) sample_fn = get_sample_fn(robot, joints) for i in range(num_samples): print('Iteration:', i) conf = sample_fn() set_joint_positions(robot, joints, conf) wait_for_user()
def move_occluding(world): # Prevent obstruction by other objects # TODO: this is a bit of a hack due to pybullet world.set_base_conf([-5.0, 0, 0]) for joint in world.kitchen_joints: joint_name = get_joint_name(world.kitchen, joint) if joint_name in DRAWERS: world.open_door(joint) else: world.close_door(joint) for name in world.movable: set_pose(world.get_body(name), Pose(Point(z=-5.0)))
def open_conf(self, joint): joint_name = get_joint_name(self.kitchen, joint) if 'left' in joint_name: open_position = get_min_limit(self.kitchen, joint) else: open_position = get_max_limit(self.kitchen, joint) #print(get_joint_name(self.kitchen, joint), get_min_limit(self.kitchen, joint), get_max_limit(self.kitchen, joint)) # drawers: [0.0, 0.4] # left doors: [-1.57, 0.0] # right doors: [0.0, 1.57] if joint_name in CABINET_JOINTS: # TODO: could make fraction of max return CABINET_OPEN_ANGLE * open_position / abs(open_position) if joint_name in DRAWER_JOINTS: return DRAWER_OPEN_FRACTION * open_position return open_position
def main(): connect(use_gui=True) add_data_path() draw_pose(Pose(), length=1.) set_camera_pose(camera_point=[1, -1, 1]) plane = p.loadURDF("plane.urdf") with LockRenderer(): with HideOutput(True): robot = load_pybullet(FRANKA_URDF, fixed_base=True) assign_link_colors(robot, max_colors=3, s=0.5, v=1.) #set_all_color(robot, GREEN) obstacles = [plane] # TODO: collisions with the ground dump_body(robot) print('Start?') wait_for_user() info = PANDA_INFO tool_link = link_from_name(robot, 'panda_hand') draw_pose(Pose(), parent=robot, parent_link=tool_link) joints = get_movable_joints(robot) print('Joints', [get_joint_name(robot, joint) for joint in joints]) check_ik_solver(info) sample_fn = get_sample_fn(robot, joints) for i in range(10): print('Iteration:', i) conf = sample_fn() set_joint_positions(robot, joints, conf) wait_for_user() #test_ik(robot, info, tool_link, get_link_pose(robot, tool_link)) test_retraction(robot, info, tool_link, use_pybullet=False, max_distance=0.1, max_time=0.05, max_candidates=100) disconnect()
def get_door_sign(self, joint): return -1 if 'left' in get_joint_name(self.kitchen, joint) else +1
def get_joint_names(self): with ClientSaver(self.client): return [ get_joint_name(self.robot, joint) for joint in get_movable_joints(self.robot) ]
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()
def get_joint_names(body, joints): return [ get_joint_name(body, joint).encode('ascii') # ,'ignore') for joint in joints ]