def randomQ(arm): (lower, upper) = arm.GetJointLimits() dofs = numpy.zeros(len(lower)) lower[0] = -math.pi / 2.0 upper[0] = math.pi / 2.0 for i in xrange(len(lower)): dofs[i] = random.uniform(lower[i], upper[i]) return dofs if __name__ == '__main__': rospy.init_node("path_testing") realarm = ArmInterface() q0 = realarm.convertToList(realarm.joint_angles()) startq = realarm.joint_angles() while True: # Rather than peturbing pose, perturb q as a hack delta_q = numpy.random.rand(7) * 0.2 q1_seed = numpy.add(q0, delta_q) raw_input("seed") realarm.move_to_joint_positions(realarm.convertToDict(q1_seed)) p1 = realarm.endpoint_pose() raw_input("q0") realarm.move_to_joint_positions(realarm.convertToDict(q0)) p0 = realarm.endpoint_pose() raw_input("p1")
if __name__ == '__main__': rospy.init_node("test_robot") r = ArmInterface( ) # create arm interface instance (see https://justagist.github.io/franka_ros_interface/DOC.html#arminterface for all available methods for ArmInterface() object) cm = r.get_controller_manager( ) # get controller manager instance associated with the robot (not required in most cases) mvt = r.get_movegroup_interface( ) # get the moveit interface for planning and executing trajectories using moveit planners (see https://justagist.github.io/franka_ros_interface/DOC.html#franka_moveit.PandaMoveGroupInterface for documentation) elapsed_time_ = rospy.Duration(0.0) period = rospy.Duration(0.005) r.move_to_neutral() # move robot to neutral pose initial_pose = r.joint_angles() # get current joint angles of the robot jac = r.zero_jacobian() # get end-effector jacobian count = 0 rate = rospy.Rate(1000) rospy.loginfo("Commanding...\n") joint_names = r.joint_names() vals = r.joint_angles() while not rospy.is_shutdown(): elapsed_time_ += period delta = 3.14 / 16.0 * ( 1 - np.cos(3.14 / 5.0 * elapsed_time_.to_sec())) * 0.2
# Stop recording rosbag terminate_ros_node("/record") rospy.sleep(1) #hand.Open() TODO def zeroFTSensor(): zeroSensorRos = rospy.ServiceProxy('/netft/zero', srv.Zero) rospy.wait_for_service('/netft/zero', timeout=0.5) zeroSensorRos() def terminate_ros_node(s): list_cmd = subprocess.Popen("rosnode list", shell=True, stdout=subprocess.PIPE) list_output = list_cmd.stdout.read() retcode = list_cmd.wait() assert retcode == 0, "List command returned %d" % retcode for string in list_output.split("\n"): if string.startswith(s): os.system("rosnode kill " + string) if __name__ == '__main__': rospy.init_node("grip_tests") realarm = ArmInterface() q0 = realarm.joint_angles() IPython.embed()
def skew_mat(p): return np.array([ [0, -p[2], p[1]], [p[2], 0, -p[0]], [-p[1], p[0], 0] ]) if __name__ == '__main__': rospy.init_node("reference_governor") # Create the robot robot = ArmInterface() vals_pos = robot.joint_angles() vals_vel = robot.joint_velocities() joint_names = robot.joint_names() rospy.loginfo('RefGov: Moving to neutral') robot.move_to_neutral() # Subscriber to the camera_info topic camera_info_msg = rospy.wait_for_message("/d435_color/camera_info", CameraInfo, timeout=None) intrinsic = get_intrinsic_param(camera_info_msg) # Transformations tf_listener = tf.TransformListener() tf_broadcaster = tf.TransformBroadcaster()
def main(args): NOISE = 0.00005 # get a bunch of random blocks blocks = load_blocks(fname=args.blocks_file, num_blocks=10, remove_ixs=[1]) agent = PandaAgent(blocks, NOISE, use_platform=False, teleport=False, use_action_server=False, use_vision=False, real=True) agent.step_simulation(T=1, vis_frames=True, lifeTime=0.) agent.plan() fixed = [f for f in agent.fixed if f is not None] grasps_fn = get_grasp_gen(agent.robot, add_slanted_grasps=False, add_orthogonal_grasps=True) path_planner = get_free_motion_gen(agent.robot, fixed) ik_fn = get_ik_fn(agent.robot, fixed, approach_frame='gripper', backoff_frame='global', use_wrist_camera=False) from franka_interface import ArmInterface arm = ArmInterface() #arm.move_to_neutral() start_q = arm.convertToList(arm.joint_angles()) start_q = pb_robot.vobj.BodyConf(agent.robot, start_q) body = agent.pddl_blocks[args.id] pose = pb_robot.vobj.BodyPose(body, body.get_base_link_pose()) for g in grasps_fn(body): grasp = g[0] # Check that the grasp points straight down. obj_worldF = pb_robot.geometry.tform_from_pose(pose.pose) grasp_worldF = np.dot(obj_worldF, grasp.grasp_objF) grasp_worldR = grasp_worldF[:3, :3] e_x, e_y, e_z = np.eye(3) is_top_grasp = grasp_worldR[:, 2].dot(-e_z) > 0.999 if not is_top_grasp: continue print('Getting IK...') approach_q = ik_fn(body, pose, grasp, return_grasp_q=True)[0] print('Planning move to path') command1 = path_planner(start_q, approach_q) print('Planning return home') command2 = path_planner(approach_q, start_q) agent.execute() input('Ready to execute?') command1[0][0].simulate(timestep=0.25) input('Move back in sim?') command2[0][0].simulate(timestep=0.25) input('Move to position on real robot?') command1[0][0].execute(realRobot=arm) input('Reset position on real robot?') command2[0][0].execute(realRobot=arm) break