def __init__(self): """ Module initialization """ # Get CWD home_cwd = os.getcwd() # Get command line arguments param_files = sys.argv[1] param_dir = sys.argv[2] # Initialize rospy node self.node_name = "reflexive_module" rospy.init_node(self.node_name) rospy.loginfo("'{}' node initialized".format(self.node_name)) # Get main parameters from file self.global_frame = "world" param_dict = params.retrieve_params_yaml_files(param_files, param_dir) self.rate = param_dict["write_rate"] self.robot = param_dict["robot"] self.connections = param_dict["connections"] self.local_ip = self.connections["left_arm"] ref_frame = param_dict["reference_frame"] joints = param_dict["joints"] # Setup master arm self.master = "left" #<-- TODO: improve this # Get full joint names self.arm_joints = joints["arm"] self.left_arm_joints = ["left_{}_joint".format(joint) for joint in joints["arm"]] self.left_eef_joints = ["left_{}_joint".format(joint) for joint in joints["eef"]] self.right_arm_joints = ["right_{}_joint".format(joint) for joint in joints["arm"]] self.right_eef_joints = ["right_{}_joint".format(joint) for joint in joints["eef"]] # State variables self.arm_state = [None]*7 # Control parameters self.poses = TimedPoseArray() self.new_command = False self.state = "start" self.status = 0 # Initialize cleanup for the node rospy.on_shutdown(self.cleanup) # Get a lock self.lock = thread.allocate_lock() # Initialize communications self.init_comms() rospy.loginfo("Communcation interfaces setup") # Run the module self.run_state_machine()
def __init__(self): """ Module initialization """ # Get CWD home_cwd = os.getcwd() # Get command line arguments param_files = sys.argv[1] param_dir = sys.argv[2] self.side = sys.argv[3] # Initialize rospy node self.node_name = "{}_reflexive_module".format(self.side) rospy.init_node(self.node_name) rospy.loginfo("'{}' node initialized".format(self.node_name)) # Get main parameters from file self.global_frame = "world" param_dict = params.retrieve_params_yaml_files(param_files, param_dir) self.rate = 30 #param_dict["rate"] self.robot = param_dict["robot"] self.connections = param_dict["connections"] self.local_ip = self.connections["{}_arm".format(self.side)] ref_frame = param_dict["reference_frame"] joints = param_dict["joints"] # Get full joint names self.arm_joints = [ "{}_{}_joint".format(self.side, joint) for joint in joints["arm"] ] self.eef_joints = [ "{}_{}_joint".format(self.side, joint) for joint in joints["eef"] ] # State variables self.arm_state = JointState() # Control parameters self.control_type = None self.trajectory = JointTrajectory() self.new_command = False self.state = "start" self.status = 0 # Initialize cleanup for the node rospy.on_shutdown(self.cleanup) # Get a lock self.lock = thread.allocate_lock() # Initialize communications self.init_comms() rospy.loginfo("Communcation interfaces setup") # Run the module self.run_state_machine()
def __init__(self): """ Module initialization """ # Get CWD home_cwd = os.getcwd() # Get command line arguments param_files = sys.argv[1] param_dir = sys.argv[2] # Initialize rospy node rospy.init_node("state_estimation_module") rospy.loginfo("'state_estimation_module' node initialized") # Get main parameters from file param_dict = params.retrieve_params_yaml_files(param_files, param_dir) self.rate = param_dict["rate"] self.robot = param_dict["robot"] joints = param_dict["joints"] self.connections = param_dict["connections"] self.local_ip = self.connections["main"] self.global_frame = "world" ref_frame = param_dict["reference_frame"] self.num_joints = len(joints["arm"]) left_base = "left_" + param_dict["base_link"] right_base = "right_" + param_dict["base_link"] left_eef = "left_" + param_dict["eef_link"] right_eef = "right_" + param_dict["eef_link"] # Get arm joint names left_name = [] right_name = [] for i in range(0,self.num_joints): left_name.append("left_" + joints["arm"][i] + "_joint") right_name.append("right_" + joints["arm"][i] + "_joint") # Store arm information TODO: handle end-effector joints index = [0]*self.num_joints self.arms = {"left_arm": {"name": left_name, "index": index, "base": left_base, "eef": left_eef}, "right_arm": {"name": right_name, "index": index, "base": right_base, "eef": right_eef},} # Set master arm (TODO: make this a variable) self.master = "left" # Initialize the state storage variable self.state = DualArmState() self.state.left_jacobian = np.zeros((6*self.num_joints)) self.state.right_jacobian = np.zeros((6*self.num_joints)) self.state.rel_jacobian = np.zeros((6*self.num_joints)) # Initialize state solvers self.jacobian_solver = DualArmJacobianSolver(self.robot, self.arms, self.master) self.eef_solver = EndEffectorStateSolver(self.robot, self.arms) # Initialize cleanup for the node rospy.on_shutdown(self.cleanup) # Get a lock self.lock = thread.allocate_lock() # Initialize communications self.init_comms() rospy.loginfo("Communcation interfaces setup") # Wait for source topics to start publishing rospy.sleep(2.5) # Start state machine self.run_state_machine()
def __init__(self): """ Module initialization """ # Get CWD home_cwd = os.getcwd() # Get command line arguments param_files = sys.argv[1] param_dir = sys.argv[2] # Initialize rospy node self.node_name = "habitual_module" rospy.init_node(self.node_name) rospy.loginfo("'{}' node initialized".format(self.node_name)) # 'Set-in-stone' parameters TODO: change these to be changeable self.global_frame = "world" self.rate = 20.0 self.master = "left" # Get main parameters from file param_dict = params.retrieve_params_yaml_files(param_files, param_dir) self.robot = param_dict["robot"] self.connections = param_dict["connections"] self.local_ip = self.connections["left_arm"] ref_frame = param_dict["reference_frame"] joints = param_dict["joints"] # Get full joint names for both arms self.arm_joints = joints["arm"] self.left_arm_joints = [ "left_{}".format(joint) for joint in joints["arm"] ] self.left_eef_joints = [ "left_{}".format(joint) for joint in joints["eef"] ] self.right_arm_joints = [ "right_{}".format(joint) for joint in joints["arm"] ] self.right_eef_joints = [ "right_{}".format(joint) for joint in joints["eef"] ] self.eef_name = param_dict["eef_link"] self.ref_frame = param_dict["reference_frame"] # Robot state variables self.arm_states = [None] * 7 # Module parameters self.state = "start" self.status = 0 self.fail_info = "" self.increment = 0 # Task setup variables self.new_command = False self.move_type = 0 self.command = 0 self.executed = False self.start_left_pose = Pose() self.goal_left_pose = Pose() self.start_right_pose = Pose() self.goal_right_pose = Pose() # Trajectory information variables self.current_left_pose = PoseStamped() self.current_right_pose = PoseStamped() self.pose_trajectory = TimedPoseArray() # Initialize cleanup for the node rospy.on_shutdown(self.cleanup) # Get a lock self.lock = thread.allocate_lock() # Initialize communications self.init_comms() rospy.loginfo("Communcation interfaces setup") # Run the module self.run_state_machine()
def __init__(self): """ Initialize the the HL module with a move group interface for the selected planning group. """ # Initialize moveit_commander moveit_commander.roscpp_initialize(sys.argv) # Get command line arguments param_files = sys.argv[1] param_dir = sys.argv[2] self.side = sys.argv[3] # Get kinematic parameters from retrieved files param_dict = params.retrieve_params_yaml_files(param_files, param_dir) self.robot = param_dict["robot"] self.planning_group = self.side + param_dict["planning_group"] self.gripper_group = self.side + param_dict["gripper_group"] self.ref_frame = "world" #TODO: tie to input arguments? arm_joints = param_dict["joints"]["arm"] self.joint_names = [ "{}_{}_joint".format(self.side, j) for j in arm_joints ] # Get ip connection for other computer (TODO: change for more than one connection) connections = param_dict["connections"] if self.side == "left": self.local_ip = connections["main"] self.connection = connections["main"] else: self.local_ip = connections["right_arm"] self.connection = connections["main"] # Print parameters to screen rospy.loginfo("Side: {}".format(self.side)) rospy.loginfo("Planning group: {}".format(self.planning_group)) rospy.loginfo("Gripper planning group: {}".format(self.gripper_group)) rospy.loginfo("Reference frame: {}".format(self.ref_frame)) rospy.loginfo("Joints: {}".format(self.joint_names)) rospy.loginfo("Secondary computer IP address: {}".format( self.connection)) # Initialize rospy node rospy.init_node("{}_habitual_module".format(self.side)) # Initialize cleanup for this node rospy.on_shutdown(self.cleanup) # Get a lock self.lock = thread.allocate_lock() # Initialize a move group for the arm and end effector planning groups self.arm = moveit_commander.MoveGroupCommander("{}".format( self.planning_group)) self.gripper = moveit_commander.MoveGroupCommander("{}".format( self.gripper_group)) # Get/set robot parameters self.eef_link = self.side + "_" + param_dict["eef_link"] self.target_ref_frame = self.ref_frame self.arm.set_pose_reference_frame(self.ref_frame) self.correction_needed = False # Module information setup self.state = "start" self.status = 0 self.fail_info = "" self.increment = 0 # Task information setup self.new_command = 0 self.move_type = 0 self.start_pose = Pose() self.goal_pose = Pose() self.command = 0 self.executed = False # Trajectory information setup self.current_pose = PoseStamped() self.stamps = "" self.pose_traj = PoseArray() self.trajectory = RobotTrajectory() # Status setup self.dl_return = DLtoHL() self.rl_return = int() # Planning information setup self.arm.allow_replanning(True) self.arm.set_goal_position_tolerance(0.001) self.arm.set_goal_orientation_tolerance(0.01) self.arm.set_goal_joint_tolerance(0.001) rospy.loginfo("MoveIt! interface initialized") # Setup a connection to the 'compute_ik' service self.ik_srv = rospy.ServiceProxy("/compute_ik", GetPositionIK) self.ik_srv.wait_for_service() rospy.loginfo("IK service initialized") # Setup a connection to the 'compute_fk' service self.fk_srv = rospy.ServiceProxy("/compute_fk", GetPositionFK) self.fk_srv.wait_for_service() rospy.loginfo("FK service initialized") # Initialize communications self.comms_initialization() rospy.loginfo("Communcation interfaces setup") # Run main loop self.main()