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()