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