def test_joint_space_streaming():
rospy.wait_for_service("plan_runner/init_joint_space_streaming")
sp = rospy.ServiceProxy('plan_runner/init_joint_space_streaming',
robot_msgs.srv.StartStreamingPlan)
init = robot_msgs.srv.StartJointSpaceStreamingPlanRequest()
init.force_guard.append(make_force_guard_msg())
print sp(init)
pub = rospy.Publisher('plan_runner/joint_space_streaming_setpoint',
sensor_msgs.msg.JointState,
queue_size=1)
robotSubscriber = ros_utils.JointStateSubscriber("/joint_states")
print("Waiting for full kuka state...")
while len(robotSubscriber.joint_positions.keys()) < 3:
rospy.sleep(0.1)
print("got full state")
start_time = time.time()
while (time.time() - start_time < 1.):
current_joint_positions = robotSubscriber.get_position_vector_from_joint_names(
control_utils.getIiwaJointNames())
new_msg = sensor_msgs.msg.JointState()
new_msg.name = control_utils.getIiwaJointNames()
new_msg.position = current_joint_positions
new_msg.velocity = np.zeros(7)
new_msg.effort = np.zeros(7)
new_msg.position[0] += 0.01
pub.publish(new_msg)
rospy.wait_for_service("plan_runner/stop_plan")
sp = rospy.ServiceProxy('plan_runner/stop_plan', std_srvs.srv.Trigger)
init = std_srvs.srv.TriggerRequest()
print sp(init)
def __init__(self, robotSystem):
self.robotSystem = robotSystem
self.endEffectorLinkName = 'iiwa_link_ee'
self.jointNames = controlUtils.getIiwaJointNames()
self.numJoints = len(self.jointNames)
self.config = dict()
self.config['ikservice_name'] = "robot_control/IkService"
def make_joint_trajectory_msg(joint_position):
traj = trajectory_msgs.msg.JointTrajectory()
traj.joint_names = control_utils.getIiwaJointNames()
num_joints = 7
traj_start = trajectory_msgs.msg.JointTrajectoryPoint()
traj_start.positions = [0] * 7
traj_start.time_from_start = rospy.Duration(0.0)
traj_end = trajectory_msgs.msg.JointTrajectoryPoint()
traj_end.positions = joint_position
traj_end.time_from_start = rospy.Duration(3.0)
traj.points.append(traj_start)
traj.points.append(traj_end)
return traj
def make_joint_trajectory(q, v):
traj = trajectory_msgs.msg.JointTrajectory()
traj.joint_names = control_utils.getIiwaJointNames()
num_joints = 7
traj_start = trajectory_msgs.msg.JointTrajectoryPoint()
traj_start.positions = q
traj_start.velocities = v
traj_start.time_from_start = rospy.Duration(0.0)
traj_end = trajectory_msgs.msg.JointTrajectoryPoint()
traj_end.positions = q + v
traj_end.velocities = v
traj_end.time_from_start = rospy.Duration(0.2)
traj.points.append(traj_start)
traj.points.append(traj_end)
return traj
def moveToJointPosition():
maxJointDegreesPerSecond = 30
jointPosition = [0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 1.0]
jointState = sensor_msgs.msg.JointState()
jointState.header.stamp = rospy.Time.now()
jointState.position = jointPosition
jointState.name = controlUtils.getIiwaJointNames()
numJoints = len(jointState.name)
jointState.velocity = [0] * numJoints
jointState.effort = [0] * numJoints
rospy.wait_for_service('robot_control/MoveToJointPosition')
s = rospy.ServiceProxy('robot_control/MoveToJointPosition',
robot_msgs.srv.MoveToJointPosition)
response = s(jointState, maxJointDegreesPerSecond)
print "response ", response
def setupRobot(self):
self.jointNames = controlUtils.getIiwaJointNames()
rospy.wait_for_service("plan_runner/init_joint_space_streaming")
sp = rospy.ServiceProxy('plan_runner/init_joint_space_streaming',
robot_msgs.srv.StartJointSpaceStreamingPlan)
init = robot_msgs.srv.StartJointSpaceStreamingPlanRequest()
#init.force_guard.append(make_force_guard_msg())
sp(init)
pub = rospy.Publisher('plan_runner/joint_space_streaming_setpoint',
sensor_msgs.msg.JointState,
queue_size=1)
smoothed_position_est = None
try:
while (1):
current_joint_positions = robotSubscriber.get_position_vector_from_joint_names(
control_utils.getIiwaJointNames())
current_joint_velocities = robotSubscriber.get_velocity_vector_from_joint_names(
control_utils.getIiwaJointNames())
current_joint_efforts = robotSubscriber.get_effort_vector_from_joint_names(
control_utils.getIiwaJointNames())
if smoothed_position_est is None:
smoothed_position_est = current_joint_positions
else:
smoothed_position_est = smoothed_position_est * 0.5 + current_joint_positions * 0.5
print smoothed_position_est, current_joint_velocities, current_joint_efforts
new_msg = sensor_msgs.msg.JointState()
new_msg.name = control_utils.getIiwaJointNames()
new_msg.position = smoothed_position_est
new_msg.velocity = current_joint_velocities
new_msg.effort = current_joint_efforts * 0.
