def test_one_jog_for_all_joint(self):
'''Test to jog a jog command'''
joint_state = self.joint_state_
jog = JogJoint()
jog.header.stamp = rospy.Time.now()
jog.joint_names = self.joint_names
jog.deltas = [self.delta]*len(jog.joint_names)
self.pub.publish(jog)
rospy.sleep(1.0)
# Check if the robot is jogged by delta
for joint in self.joint_names:
index = joint_state.name.index(joint)
self.assertAlmostEqual(
joint_state.position[index] + self.delta, self.joint_state_.position[index], delta=0.0001)
def callback(self, joy):
msg = JogJoint()
msg.header.stamp = rospy.Time.now()
msg.header.frame_id = 'base_link'
msg.name = ['joint1', 'joint2', 'joint3', 'joint4', 'joint5', 'joint6']
# These buttons are binary
msg.displacement = [0] * 6
msg.displacement[0] = 0.05 * (-joy.buttons[4] + joy.buttons[5])
msg.displacement[1] = 0.05 * (joy.axes[0])
msg.displacement[2] = 0.05 * (joy.axes[1])
msg.displacement[3] = 0.05 * (joy.buttons[0] - joy.buttons[1])
msg.displacement[4] = 0.05 * (joy.buttons[2] - joy.buttons[3])
msg.displacement[5] = 0.05 * (joy.buttons[7] - joy.buttons[6])
self.pub.publish(msg)
def test_empty_jog_for_all_joint(self):
'''Test to publish empty jonit_names/positions jog command'''
joint_state = self.joint_state_
jog = JogJoint()
jog.header.stamp = rospy.Time.now()
self.pub.publish(jog)
rospy.sleep(1.0)
# Check if the robot is not moved
for joint in self.joint_names:
index = joint_state.name.index(joint)
self.assertAlmostEqual(
joint_state.position[index],
self.joint_state_.position[index],
delta=0.0001)
def test_loop_jogs_for_all_joint(self):
'''Test to jog same amount for forward and backward'''
joint_state = self.joint_state_
for i in range(10):
jog = JogJoint()
jog.header.stamp = rospy.Time.now()
jog.joint_names = self.joint_names
jog.deltas = [self.delta]*len(jog.joint_names)
self.pub.publish(jog)
for i in range(10):
jog = JogJoint()
jog.header.stamp = rospy.Time.now()
jog.joint_names = self.joint_names
jog.deltas = [-self.delta]*len(jog.joint_names)
self.pub.publish(jog)
rospy.sleep(1.0)
# Check if the robot is not moved
for joint in self.joint_names:
index = joint_state.name.index(joint)
self.assertAlmostEqual(
joint_state.position[index],
self.joint_state_.position[index],
delta=0.0001)
def callback(data):
global L
global data_backup
global gripper
global time_gripper
if (data.data[-2] != gripper) and (time.time() - time_gripper > 2):
time.sleep(0.5)
msg = Float64MultiArray()
dim = [MultiArrayDimension()]
dim[0].label = "height"
dim[0].size = 1
dim[0].stride = 1
msg.layout.dim = dim
msg.layout.data_offset = 0
msg.data = [data.data[-2]]
pub_gripper.publish(msg)
gripper = data.data[-2]
time_gripper = time.time()
time.sleep(0.5)
else:
#print(data.data)
ee_pos = robot.move_group.get_current_pose().pose
current_joints = robot.move_group.get_current_joint_values()
L += [current_joints]
np.save('./out/angles_robot', L)
if security_halt(current_joints):
print("security fail")
msg = JogJoint()
msg.header.stamp = rospy.get_rostime()
msg.joint_names = ['shoulder_lift_joint']
msg.deltas = [0.05]
pub.publish(msg)
else:
msg = JogJoint()
msg.header.stamp = rospy.get_rostime()
names = []
deltas = []
if data.data[-5] == 1: # if the base angle is correct
data_backup[2] = -current_joints[0] + (data.data[0] - np.pi)
names = names + [JOINT_NAMES[0]]
deltas = deltas + [data_backup[2]]
if data.data[-4] == 1: # if the arm was found
data_backup[:2] = [
-current_joints[1] + (-data.data[1] - np.pi),
-current_joints[2] + (-data.data[2])
]
names = names + [JOINT_NAMES[1], JOINT_NAMES[2]]
deltas = deltas + data_backup[:2]
if data.data[-3] == 1: # if the hand was found
names = names + [JOINT_NAMES[3], JOINT_NAMES[5]]
deltas = deltas + [
-current_joints[3] + (-data.data[3]), -current_joints[5] +
(data.data[4] + np.pi)
]
norm_deltas = [max(min(x, 1), -1) for x in deltas]
msg.joint_names = names
msg.deltas = norm_deltas
#rospy.loginfo(msg)
pub.publish(msg)