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.