def __init__(self, local=True): """ Instantiates an interface for darias. """ # if not local self._enabled = True self._root = "/franka_ros_interface" # if local # self._root = "/panda_simulator" # rospy.init_node("franka_robot_gym") rospy.init_node("robopy_interface") self.arms = Arms(self) self._arm_interface = ArmInterface(True) self._gripper_interface = GripperInterface() self._robot_status = RobotEnable() self._ctrl_manager = FrankaControllerManagerInterface( ns=self._root, sim=False) # TODO: (sim=false for real robot)) print(self._ctrl_manager) self._movegroup_interface = None # TODO: consider removing it self._joint_command_publisher = rospy.Publisher( self._root + '/motion_controller/arm/joint_commands', # 'joint_commands', JointCommand, tcp_nodelay=True, queue_size=1) while not (self.arms.ready): pass self.groups = {} self._building_groups() # TODO: temporary self._joint_names = self.groups["arm_gripper"].refs
plt.plot(sensor_readings[5, 10:], label="torque z [Nm]") plt.xlabel("number of iterations (adding up to 10 seconds)") plt.legend() plt.subplot(122) plt.title("Deviations from desired pose") plt.plot(pose_error[0, 10:], label="deviation x [m]") plt.plot(pose_error[1, 10:], label="deviation y [m]") plt.plot(pose_error[2, 10:], label="deviation z [m]") plt.plot(pose_error[3, 10:], label="quaternion x") plt.plot(pose_error[4, 10:], label="quaternion y") plt.plot(pose_error[5, 10:], label="quaternion z") plt.xlabel("number of iterations (adding up to 10 seconds)") plt.legend() plt.show() if __name__ == "__main__": rospy.init_node("ts_control_sim_only") #rospy.on_shutdown(_on_shutdown) publish_rate = 500 rate = rospy.Rate(publish_rate) robot = ArmInterface() print('Moving to starting position') robot.move_to_joint_positions(starting_position) #robot.move_to_neutral() impedance_control(rate, K_Pt, K_Po, K_m, K_d)
import rospy from franka_interface import ArmInterface if __name__ == '__main__': rospy.init_node("test_robot") r = ArmInterface() cm = r.get_controller_manager()
This code is for testing the torque controller. The robot should be in zeroG mode, i.e. It should stay still when no external force is acting, but should be very compliant (almost no resistance) when pushed. This code sends zero torque command continuously, which means the internal gravity compensation is the only torque being sent to the robot. Test by pushing robot. DO NOT RUN THIS CODE WITH CUSTOM END-EFFECTOR UNLESS YOU KNOW WHAT YOU'RE DOING! WARNING: This code only works if the robot model is good! If you have installed custom end-effector, the gravity compensation may not be good unless you have incorporated the model to the FCI via Desk!! """ if __name__ == '__main__': rospy.init_node("test_zeroG") r = ArmInterface() # create arm interface instance (see https://justagist.github.io/franka_ros_interface/DOC.html#arminterface for all available methods for ArmInterface() object) rospy.loginfo("Commanding...\n") r.move_to_neutral() # move robot to neutral pose rate = rospy.Rate(1000) joint_names = r.joint_names() while not rospy.is_shutdown(): r.set_joint_torques(dict(zip(joint_names, [0.0]*7))) # send 0 torques rate.sleep()
assert self._current_K_frame_transformation is not None, "FrankaFramesInterface: Current K Frame is not known." return list(self._current_K_frame_transformation) == list( DEFAULT_TRANSFORMATIONS.K_FRAME) def _request_setK_service(self, trans_mat): print trans_mat rospy.wait_for_service( '/franka_ros_interface/franka_control/set_K_frame') try: service_handle = rospy.ServiceProxy( '/franka_ros_interface/franka_control/set_K_frame', SetKFrame) response = service_handle(EE_T_K=trans_mat) rospy.loginfo( "Set K Frame Request Status: %s. \n\tDetails: %s" % ("Success" if response.success else "Failed!", response.error)) return response.success except rospy.ServiceException, e: rospy.logwarn("Set K Frame Request: Service call failed: %s" % e) return False if __name__ == '__main__': # main() from franka_interface import ArmInterface rospy.init_node("test") ee_setter = FrankaFramesInterface(ArmInterface()) # ee_setter.set_EE_frame([1,0,0,0,0,1,0,0,0,0,1,0,0,0,0,1])
def __init__(self, real_robot=False): # Event is clear while initialization or set_state is going on self.reset = Event() self.reset.clear() self.get_state_event = Event() self.get_state_event.set() self.real_robot = real_robot # TODO publisher, subscriber, target and state self._add_publishers() self.panda_arm = ArmInterface() self.target = [0.0] * 6 # TODO define number of target floats # TODO define number of panda states (At least the number of joints) self.panda_joint_names = [ 'panda_joint1', 'panda_joint2', 'panda_joint3', 'panda_joint4', 'panda_joint5', 'panda_joint6', 'panda_joint7' ] self.panda_finger_names = [ 'panda_finger_joint1', 'panda_finger_joint2' ] self.panda_joint_num = len(self.panda_joint_names) self.panda_state = [0.0] * self.panda_joint_num # TF Listener self.tf_listener = tf.TransformListener() # TF2 Listener # self.tf2_buffer = tf2_ros.Buffer() # self.tf2_listener = tf2_ros.TransformListener(self.tf2_buffer) # Static TF2 Broadcaster # self.static_tf2_broadcaster = tf2_ros.StaticTransformBroadcaster() # Robot control rate self.sleep_time = (1.0 / rospy.get_param('~action_cycle_rate')) - 0.01 self.control_period = rospy.Duration.from_sec(self.sleep_time) self.reference_frame = rospy.get_param('~reference_frame', 'base') self.ee_frame = 'tool0' # TODO is the value for self.ee_frame correct? self.target_frame = 'target' # Minimum Trajectory Point time from start # TODO check if this value is correct for the panda robot self.min_traj_duration = 0.5 if not self.real_robot: # Subscribers to link collision sensors topics # TODO add rospy.Subscribers rospy.Subscriber('/joint_states', JointState, self.callback_panda) # TODO add keys to collision sensors self.collision_sensors = dict.fromkeys([], False) # TODO currently not used self.safe_to_move = True # Target mode self.target_mode = rospy.get_param('~target_mode', FIXED_TARGET_MODE) self.target_mode_name = rospy.get_param('~target_model_name', 'box100')
return arm.convertToDict(dofs) 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()
def map_keyboard(): """ Map keyboard keys to robot joint motion. Keybindings can be found when running the script. """ limb = ArmInterface() gripper = GripperInterface(ns=limb.get_robot_params().get_base_namespace()) has_gripper = gripper.exists joints = limb.joint_names() def set_j(limb, joint_name, delta): joint_command = limb.joint_angles() joint_command[joint_name] += delta limb.set_joint_positions(joint_command) # limb.set_joint_positions_velocities([joint_command[j] for j in joints], [0.00001]*7) # impedance control when using this example might fail because commands are sent too quickly for the robot to respond def set_g(action): if has_gripper: if action == "close": gripper.close() elif action == "open": gripper.open() elif action == "calibrate": gripper.calibrate() bindings = { '1': (set_j, [limb, joints[0], 0.01], joints[0] + " increase"), 'q': (set_j, [limb, joints[0], -0.01], joints[0] + " decrease"), '2': (set_j, [limb, joints[1], 0.01], joints[1] + " increase"), 'w': (set_j, [limb, joints[1], -0.01], joints[1] + " decrease"), '3': (set_j, [limb, joints[2], 0.01], joints[2] + " increase"), 'e': (set_j, [limb, joints[2], -0.01], joints[2] + " decrease"), '4': (set_j, [limb, joints[3], 0.01], joints[3] + " increase"), 'r': (set_j, [limb, joints[3], -0.01], joints[3] + " decrease"), '5': (set_j, [limb, joints[4], 0.01], joints[4] + " increase"), 't': (set_j, [limb, joints[4], -0.01], joints[4] + " decrease"), '6': (set_j, [limb, joints[5], 0.01], joints[5] + " increase"), 'y': (set_j, [limb, joints[5], -0.01], joints[5] + " decrease"), '7': (set_j, [limb, joints[6], 0.01], joints[6] + " increase"), 'u': (set_j, [limb, joints[6], -0.01], joints[6] + " decrease") } if has_gripper: bindings.update({ '8': (set_g, "close", "close gripper"), 'i': (set_g, "open", "open gripper"), '9': (set_g, "calibrate", "calibrate gripper") }) done = False rospy.logwarn( "Controlling joints. Press ? for help, Esc to quit.\n\nWARNING: The motion will be slightly jerky!!\n" ) while not done and not rospy.is_shutdown(): c = getch() if c: #catch Esc or ctrl-c if c in ['\x1b', '\x03']: done = True rospy.signal_shutdown("Example finished.") elif c in bindings: cmd = bindings[c] if c == '8' or c == 'i' or c == '9': cmd[0](cmd[1]) print("command: %s" % (cmd[2], )) else: #expand binding to something like "set_j(right, 'j0', 0.1)" cmd[0](*cmd[1]) print("command: %s" % (cmd[2], )) else: print("key bindings: ") print(" Esc: Quit") print(" ?: Help") for key, val in sorted(viewitems(bindings), key=lambda x: x[1][2]): print(" %s: %s" % (key, val[2]))
def ExecuteActions(plan, real=False, pause=True, wait=True, prompt=True, obstacles=[], sim_fatal_failure_prob=0, sim_recoverable_failure_prob=0): # if prompt: # input("Execute in Simulation?") # obj_held = None for name, args in plan: # pb_robot.viz.remove_all_debug() # bodyNames = [args[i].get_name() for i in range(len(args)) if isinstance(args[i], pb_robot.body.Body)] #txt = '{} - {}'.format(name, bodyNames) # pb_robot.viz.add_text(txt, position=(0, 0.25, 0.5), size=2) executionItems = args[-1] for e in executionItems: if real: e.simulate(timestep=0.1, obstacles=obstacles) else: e.simulate(timestep=0.05, obstacles=obstacles) # Assign the object being held # if isinstance(e, pb_robot.vobj.BodyGrasp): # if name == "pick": # obj_held = e # else: # obj_held = None # Simulate failures if specified if (name in ["pick", "move_free"] and not isinstance(e, pb_robot.vobj.BodyGrasp) and not isinstance(e, pb_robot.vobj.MoveFromTouch)): if numpy.random.rand() < sim_fatal_failure_prob: raise ExecutionFailure( fatal=True, reason=f"Simulated fatal failure in {e}") elif numpy.random.rand() < sim_recoverable_failure_prob: # if (name in ["place", "place_home", "move_holding"]) or \ # (name=="pick" and isinstance(e, pb_robot.vobj.MoveFromTouch)): # obj_held_arg = obj_held # else: # obj_held_arg = None raise ExecutionFailure( fatal=False, reason=f"Simulated recoverable failure in {e}") if wait: input("Next?") elif pause: time.sleep(0.1) if real: input("Execute on Robot?") try: from franka_interface import ArmInterface except: print("Do not have rospy and franka_interface installed.") return #try: arm = ArmInterface() arm.set_joint_position_speed(0.3) #except: # print("Unable to connect to real robot. Exiting") # return print("Executing on real robot") input("start?") for name, args in plan: executionItems = args[-1] for e in executionItems: e.execute(realRobot=arm, obstacles=obstacles)
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