def publish_positions(self, positions, moving_time=None, accel_time=None, blocking=True):
self.set_trajectory_time(moving_time, accel_time)
self.joint_positions = list(positions)
joint_commands = JointCommands(positions)
self.pub_joint_commands.publish(joint_commands)
if blocking:
rospy.sleep(self.moving_time)
self.T_sb = mr.FKinSpace(self.robot_des.M, self.robot_des.Slist, positions)
def set_joint_commands(self,
commands,
moving_time=None,
accel_time=None,
delay=0):
self.set_trajectory_time(moving_time, accel_time)
joint_commands = JointCommands(commands)
self.pub_joint_commands.publish(joint_commands)
if (delay > 0):
rospy.sleep(delay)
def __init__(self):
self.robot_name = rospy.get_param("~robot_name") # Name of the robot
self.robot_des = getattr(mrd, self.robot_name) # Get the Modern Robotics description of the specified robot
self.joint_states = None # Current joint states
self.ee_reference = None # Snapshot of the end-effector pose w.r.t the 'Space' frame - used when tracking a horizontal line in space
self.velocity_x_ik_only = False # True if the end-effector is tracking a horizontal line in space only (as opposed to a vertical or diagonal line)
self.gripper_state = GripperState.IDLE # Current gripper controller state
self.joint_limit_padding = 0.2 # All joint limits are buffered by this amount [rad] as it takes time for joints to stop moving
self.stop_single_joint = False # True if a joint limit was breached while in 'SINGLE_JOINT' control mode
self.ee_vel = 0.15 # Desired end-effector velocity [m/s] w.r.t the 'shoulder_link' frame
self.controllers = [] # List of PID controller objects used when in the 'ROBOT_POSE_CONTROL' mode
self.mutex = threading.Lock() # Mutex to essentially ensure that the 'joy_control_cb' and 'controller' timer occur sequentially
self.js_mutex = threading.Lock() # Mutex to get the most up-to-date joint states
self.angles = None # Desired angular positions of the joints (used in the 'ROBOT_POSE_CONTROL' mode)
self.gripper_pwm = 300 # Desired gripper pwm when opening or closing
self.arm_vel_max = 2.5 # Max allowed single joint velocity [rad/s] when in 'SINGLE_JOINT' mode
self.T_ssh = None # Transform from the 'shoulder_link' frame w.r.t the 'Space' frame - used in the 'VELOCITY_IK' mode
self.arm_state = ArmState.IDLE # Current joint controller state
self.last_active_arm_state = ArmState.IDLE # Last joint controller state (excluding 'STOPPED' and 'IDLE')
self.joy_speeds = {"course" : 1, "fine" : 1, "current" : 1} # Holds the 'course' and 'fine' speed values
self.pub_joint_commands = rospy.Publisher("joint/commands", JointCommands, queue_size=100) # ROS Publisher to command joint velocities [rad/s]
self.pub_gripper_command = rospy.Publisher("gripper/command", Float64, queue_size=100) # ROS Publisher to command gripper position [rad]
self.sub_joint_states = rospy.Subscriber("joint_states", JointState, self.joint_state_cb) # ROS Subscriber to get the current joint states
self.sub_joy_commands = rospy.Subscriber("joy/commands", JoyControl, self.joy_control_cb) # ROS Subscriber to get the joystick commands
rospy.wait_for_service("get_robot_info")
srv_robot_info = rospy.ServiceProxy("get_robot_info", RobotInfo)
self.resp = srv_robot_info() # ROS Service to get joint limits
self.upper_joint_limits = [x - self.joint_limit_padding for x in self.resp.upper_joint_limits] # Upper joint limits with padding included
self.upper_joint_limits[2] = 1.0 # Prevent the 'elbow' joint from going past this position [rad] to avoid singularities
self.lower_joint_limits = [x + self.joint_limit_padding for x in self.resp.lower_joint_limits] # Lower joint limits with padding included
self.num_joints = self.resp.num_joints # Number of joints in the arm
self.joint_commands = JointCommands() # Class-wide Joint Commands message
self.joint_commands.cmd = [0] * self.num_joints
self.twist = [0, 0, 0, 0, 0, 0] # Modern_Robotics Twist [Wx, Wy, Wz, Vx, Vy, Vz] of end-effector w.r.t the 'shoulder_link' frame
for x in range(self.num_joints):
pid_controller = pid.PID()
self.controllers.append(pid_controller)
while (self.joint_states == None and not rospy.is_shutdown()):
pass
self.get_initial_T_ssh()
self.tmr_controller = rospy.Timer(rospy.Duration(0.01), self.controller) # ROS Timer to poll the arm and gripper control states
def __init__(self):
# Initialization parameters to control the Interbotix Arm
rospy.wait_for_service("get_robot_info")
srv_robot_info = rospy.ServiceProxy(
"get_robot_info", RobotInfo
) # ROS Service to get joint limit information among other things
self.resp = srv_robot_info(
) # Store the robot information in this variable
self.joint_indx_dict = dict(
zip(self.resp.joint_names, range(self.resp.num_single_joints))
) # Map each joint-name to their respective index in the joint_names list (which conveniently matches their index in the joint-limit lists)
self.joy_msg = ArmJoyControl(
) # Incoming message coming from the 'joy_control' node
self.arm_model = rospy.get_param("~robot_name") # Arm-model type
self.num_joints = self.resp.num_joints # Number of joints in the arm
self.speed_max = 3.0 # Max scaling factor when bumping up joint speed
self.gripper_pwm = 200 # Initial gripper PWM value
self.gripper_moving = False # Boolean that is set to 'True' if the gripper is moving - 'False' otherwise
self.gripper_command = Float64(
) # Float64 message to be sent to the 'gripper' joint
self.gripper_index = self.num_joints + 1 # Index of the 'left_finger' joint in the JointState message
self.follow_pose = True # True if going to 'Home' or 'Sleep' pose
self.use = False
self.joint_states = None # Holds the most up-to-date JointState message
self.js_mutex = threading.Lock(
) # Mutex to make sure that 'self.joint_states' variable isn't being modified and read at the same time
self.joint_commands = JointCommands(
) # JointCommands message to command the arm joints as a group
self.target_positions = list(
self.resp.sleep_pos) # Holds the 'Sleep' or 'Home' joint values
self.robot_des = getattr(
mrd, self.arm_model) # Modern Robotics robot description
self.joy_speeds = {
"course": 2.0,
"fine": 2.0,
"current": 2.0
} # Dictionary containing the desired joint speed scaling factors
self.pub_joint_commands = rospy.Publisher(
"joint/commands", JointCommands, queue_size=100,
latch=True) # ROS Publisher to command joint positions [rad]
self.pub_gripper_command = rospy.Publisher(
"gripper/command", Float64,
queue_size=100) # ROS Publisher to command gripper PWM values
self.is_use = rospy.Subscriber("/vx300s/joy", Joy, self.check_cb)
self.sub_joy_commands = rospy.Subscriber(
"joy/commands", ArmJoyControl,
self.joy_control_cb) # ROS Subscriber to get the joystick commands
self.sub_joint_states = rospy.Subscriber(
"joint_states", JointState, self.joint_state_cb
) # ROS Subscriber to get the current joint states
while (self.joint_states == None and not rospy.is_shutdown()):
pass # Wait until we know the current joint values of the robot
self.joint_positions = list(
self.joint_states.position[:self.num_joints]
) # Holds the desired joint positions for the robot arm at any point in time
self.yaw = 0.0 # Holds the desired 'yaw' of the end-effector w.r.t. the 'base_link' frame
self.T_sy = np.identity(
4
) # Transformation matrix of a virtual frame with the same x, y, z, roll, and pitch values as 'base_link' but containing the 'yaw' of the end-effector
self.T_sb = mr.FKinSpace(
self.robot_des.M, self.robot_des.Slist, self.resp.sleep_pos
) # Transformation matrix of the end-effector w.r.t. the 'base_link' frame
self.T_yb = np.dot(
mr.TransInv(self.T_sy),
self.T_sb) # Transformation matrix of the end-effector w.r.t. T_sy
tmr_controller = rospy.Timer(
rospy.Duration(0.02),
self.controller) # ROS Timer to control the Interbotix Arm