period = rospy.Duration(0.005)
r.move_to_neutral() # move robot to neutral pose
initial_pose = r.joint_angles() # get current joint angles of the robot
jac = r.zero_jacobian() # get end-effector jacobian
count = 0
rate = rospy.Rate(1000)
rospy.loginfo("Commanding...\n")
joint_names = r.joint_names()
vals = r.joint_angles()
while not rospy.is_shutdown():
elapsed_time_ += period
delta = 3.14 / 16.0 * (
1 - np.cos(3.14 / 5.0 * elapsed_time_.to_sec())) * 0.2
for j in joint_names:
if j == joint_names[4]:
vals[j] = initial_pose[j] - delta
else:
vals[j] = initial_pose[j] + delta
# r.set_joint_positions(vals) # for position control. Slightly jerky motion.
r.set_joint_positions_velocities([vals[j] for j in joint_names],
[0.0] * 7) # for impedance control
rate.sleep()
initial_pose = deepcopy(r.joint_ordered_angles())
raw_input("Hit Enter to Start")
print "commanding"
vals = deepcopy(initial_pose)
count = 0
while not rospy.is_shutdown():
elapsed_time_ += period
delta = 3.14 / 16.0 * (
1 - np.cos(3.14 / 5.0 * elapsed_time_.to_sec())) * 0.2
for j, _ in enumerate(vals):
if j == 4:
vals[j] = initial_pose[j] - delta
else:
vals[j] = initial_pose[j] + delta
if count % 500 == 0:
print vals, delta
print "\n ---- \n"
print " "
r.set_joint_positions_velocities(
vals, [0.0 for _ in range(7)]) # for impedance control
# r.set_joint_positions(dict(zip(r.joint_names(), vals))) # try this for position control
count += 1
rate.sleep()
