def send_right_arm_cartesian_command(self, pose_stamped):
pose_stamped = convert_to_pose_msg(pose_stamped)
pose_stamped.pose.orientation = normalize_quaternion(
pose_stamped.pose.orientation)
right_arm_command = MotionCommand()
right_arm_command.header.frame_id = 'victor_right_arm_world_frame_kuka'
right_arm_command.cartesian_pose = pose_stamped.pose
right_arm_command.control_mode = self.get_right_arm_control_mode()
while self.right_arm_command_pub.get_num_connections() < 1:
rospy.sleep(0.01)
self.right_arm_command_pub.publish(right_arm_command)
def send_arm_command(self, command_pub: rospy.Publisher,
right_arm_control_mode: ControlMode, positions):
if positions is not None:
# FIXME: what if we allow the BaseRobot class to use moveit, but just don't have it require that
# any actions are running?
# NOTE: why are these values not checked by the lower-level code? the Java code knows what the joint limits
# are so why does it not enforce them?
# TODO: use enforce bounds? https://github.com/ros-planning/moveit/pull/2356
limit_enforced_positions = []
for i, joint_name in enumerate(right_arm_joints):
joint: moveit_commander.RobotCommander.Joint = self.robot_commander.get_joint(
joint_name)
limit_enforced_position = np.clip(positions[i],
joint.min_bound() + 1e-2,
joint.max_bound() - 1e-2)
limit_enforced_positions.append(limit_enforced_position)
cmd = MotionCommand()
cmd.header.stamp = rospy.Time.now()
cmd.joint_position = list_to_jvq(limit_enforced_positions)
cmd.control_mode = right_arm_control_mode
command_pub.publish(cmd)
def run_lowpass(self, msg):
if self.prev_time is None:
self.prev_time = time.time()
now = time.time()
dt = now - self.prev_time
self.prev_time = now
if dt > 1:
print("Incredibly long time between messages. Something is weird")
print("dt: {}".format(dt))
assert False
# Update our internal copy and calculate error for later thresholding
meas = np.array([
getattr(msg.measured_joint_position, joint)
for joint in joint_names
])
cmd = np.array([
getattr(msg.commanded_joint_position, joint)
for joint in joint_names
])
err = np.linalg.norm(meas - cmd)
self.last_pos = meas
if err > self.threshold:
# Robot is far enough away from setpoint that we assume someone is pushing it
self.follow = True
if err < self.arrive_threshold:
# Robot is close enough to setpoint that we go into "hold position" mode
self.follow = False
if not self.follow:
# Just hold position
return
# Stay away from hard limits
meas = meas.clip(joint_lower, joint_upper)
# Low pass filter to avoid jerky motions
alpha = dt / (self.low_pass_tau + dt)
new_cmd = (1.0 - alpha) * cmd + alpha * meas
# Send the actual command
cmd_msg = MotionCommand()
cmd_msg.control_mode.mode = ControlMode.JOINT_IMPEDANCE
for idx in range(len(joint_names)):
setattr(cmd_msg.joint_position, joint_names[idx], new_cmd[idx])
self.pub.publish(cmd_msg)
rospy.Subscriber("/victor/joint_states", JointState, get_current_pose_cb)
positions = [
[0, 0, 0, 0, 0, 0, 0],
# [0,1.0,0,1.0,0,1.0,0],
# [-0.5,1.0,0,1.0,0,1.0,0],
# [0.5,1.2,-0.5,-1.0,-0.3,0,0],
# [0,0,0,0,0,0,0],
]
velocities = [0, 0, 0, 0, 0, 0, 0]
# Wait to get current pose:
while current_pose is None:
rospy.sleep(0.1)
r = rospy.Rate(5)
i = 0
while not rospy.is_shutdown():
cmd = MotionCommand(joint_position=list_to_jvq(positions[i]),
joint_velocity=list_to_jvq(velocities),
control_mode=ControlMode(ControlMode.JOINT_POSITION))
cmd.header.stamp = rospy.Time.now()
arm_command_pub.publish(cmd)
if np.linalg.norm(np.array(positions[i]) - np.array(current_pose)) < 0.001:
i = i + 1
if i == len(positions):
break
r.sleep()
def __init__(self, parent_layout, arm_name, box_row):
self.name = arm_name
self.block_command = False
self.v_layout = QGridLayout()
self.col_num = 11
self.row_count = [0] * self.col_num
self.reset_arm_to_zero_button = self.init_button('Home the Arm', 0, self.reset_arm_command_to_zero)
self.reset_arm_slider_to_current_value_button = self.init_button(
'Reset the Sliders', 1, self.reset_arm_slider_to_current_value)
self.open_gripper_button = self.init_button('Open the Gripper', 2, self.open_gripper_command)
self.close_gripper_button = self.init_button('Close the Gripper', 4, self.close_gripper_command)
self.reset_speed_force_button = self.init_button('Reset Speed and Force', 6, self.reset_speed_force_command)
self.reset_gripper_position_slider_to_current_value_button = self.init_button(
'Reset the Sliders', 8, self.reset_gripper_position_slider_to_current_value)
# Create all joint sliders, textboxes, and labels
self.joint_labels = {}
self.joint_sliders = {}
self.joint_textboxes = {}
for joint_name in joint_names:
slider_callback = partial(self.joint_slider_moved, joint_name=joint_name)
text_callback = partial(self.joint_textbox_modified, joint_name=joint_name)
label = 'Joint ' + joint_name[-1] + ' Command'
self.joint_labels[joint_name] = self.init_label(label, 0)
self.joint_sliders[joint_name] = self.init_slider(joint_limits_with_margin[joint_name],
0, slider_callback)
self.joint_textboxes[joint_name] = self.init_textbox('0', 1, text_callback, 1)
# Create all finger joint sliders, textboxes, and labels
self.finger_labels = {}
self.finger_sliders = {}
self.finger_textboxes = {}
# increment blank cell in the header row
for col in [3, 5, 7, 9, 10]:
self.row_count[col] += 1
for finger_joint_name in finger_joint_names:
col = finger_column[finger_joint_name]
for j, finger_command_name in enumerate(finger_command_names):
slider_callback = partial(self.finger_joint_slider_moved,
finger_joint_name=finger_joint_name,
finger_command_name=finger_command_name)
text_callback = partial(self.finger_joint_textbox_modified,
finger_joint_name=finger_joint_name,
finger_command_name=finger_command_name)
label = finger_joint_labels[finger_joint_name] + ' Command ' + \
finger_command_name[0].upper() + finger_command_name[1:]
self.finger_labels[(finger_joint_name, finger_command_name)] = self.init_label(label, col)
self.finger_sliders[(finger_joint_name, finger_command_name)] = \
self.init_slider((0, finger_range_discretization), col, slider_callback,
finger_command_default[finger_command_name] * finger_range_discretization)
self.finger_textboxes[(finger_joint_name, finger_command_name)] = \
self.init_textbox('%5.3f' % finger_command_default[finger_command_name], col + 1, text_callback, 1)
# Create finger synchronization checkboxes
self.finger_synchronization_checkboxes = {}
self.synchonize_label = self.init_label('Synchronize', 8)
for j, finger_command_name in enumerate(finger_command_names):
if j == 0:
checkbox_row_skip = 0
else:
checkbox_row_skip = 1
cb_callback = partial(self.finger_synchronization_checkbox_changed, finger_command_name=finger_command_name)
self.finger_synchronization_checkboxes[finger_command_name] = \
self.init_checkbox('', 8, cb_callback, checkbox_row_skip)
self.control_mode_label = self.init_label('Control Mode', 2, 1)
control_mode_list = ['JOINT_POSITION', 'CARTESIAN_POSE', 'JOINT_IMPEDANCE', 'CARTESIAN_IMPEDANCE']
stiffness_list = ['STIFF', 'MEDIUM', 'SOFT']
s = victor_utils.Stiffness
self.stiffness_map = [s.STIFF, s.MEDIUM, s.SOFT]
self.stiffness = self.stiffness_map[0]
self.control_mode_combobox = self.init_combobox(control_mode_list, 2, self.change_control_mode, 0)
self.stiffness_combobox = self.init_combobox(stiffness_list, 2, self.set_stiffness, 0)
self.finger_command = Robotiq3FingerCommand()
self.arm_command = MotionCommand()
self.arm_status = MotionStatus()
self.gripper_status = Robotiq3FingerStatus()
self.arm_status_updated = False
self.gripper_status_updated = False
self.finger_command.finger_a_command.speed = 1.0
self.finger_command.finger_b_command.speed = 1.0
self.finger_command.finger_c_command.speed = 1.0
self.finger_command.scissor_command.speed = 1.0
self.finger_command_publisher = \
rospy.Publisher('victor/' + self.name + '/gripper_command', Robotiq3FingerCommand, queue_size=10)
self.arm_command_publisher = rospy.Publisher('victor/' + self.name + '/motion_command', MotionCommand, queue_size=10)
self.gripper_status_subscriber = \
rospy.Subscriber('victor/' + self.name + '/gripper_status', Robotiq3FingerStatus, self.gripper_status_callback)
self.arm_status_subscriber = \
rospy.Subscriber('victor/' + self.name + '/motion_status', MotionStatus, self.arm_status_callback)
self.fingers_same_command = {finger_command_name: False for finger_command_name in finger_command_names}
self.groupbox = QGroupBox(self.name)
self.groupbox.setLayout(self.v_layout)
self.groupbox.setFlat(False)
parent_layout.addWidget(self.groupbox, box_row, 0)
print('Getting', self.name, 'current joint values ...')
sys.stdout.flush()
while (not self.arm_status_updated) or (not self.gripper_status_updated):
time.sleep(0.01)
print('Done')
self.reset_arm_slider_to_current_value()
self.reset_gripper_position_slider_to_current_value()
print('Getting {} current control mode ...'.format(self.name))
sys.stdout.flush()
get_current_control_mode = rospy.ServiceProxy('victor/' + self.name + '/get_control_mode_service', GetControlMode)
control_mode = get_current_control_mode()
self.active_control_mode_int = control_mode.active_control_mode.control_mode.mode
self.control_mode_combobox.setCurrentIndex(self.active_control_mode_int)
print('Done')