def test_typechecker_serialize_deserialize_time_inverse(value):
""""""
# sending
buff = BytesIO()
value.serialize(buff)
serialized = buff.getvalue()
buff.close()
# receiving
received = std_msgs.Time()
received.deserialize(serialized)
assert received == value
def __init__(self, sweep_joint_dict, debug=False, goal_type='API', state_type='OL'):
self.debug = debug
self.goal_type = goal_type #default API
self.state_type = state_type #default OL = open loop
if self.goal_type == 'trajectory':
super(RolloutExecuter, self).__init__()
#create our action server clients
self._left_client = actionlib.SimpleActionClient(
'robot/limb/left/follow_joint_trajectory',
FollowJointTrajectoryAction,
)
self._right_client = actionlib.SimpleActionClient(
'robot/limb/right/follow_joint_trajectory',
FollowJointTrajectoryAction,
)
#param namespace
self._param_ns = '/rsdk_joint_trajectory_action_server/'
#create our goal request
self._l_goal = FollowJointTrajectoryGoal()
self._r_goal = FollowJointTrajectoryGoal()
#gripper goal trajectories
self._l_grip = FollowJointTrajectoryGoal()
self._r_grip = FollowJointTrajectoryGoal()
#limb interface - current angles needed for start move
self._l_arm = baxter_interface.Limb('left')
self._r_arm = baxter_interface.Limb('right')
#flag to signify the arm trajectories have begun executing
self._arm_trajectory_started = False
#reentrant lock to prevent same-thread lockout
self._lock = threading.RLock()
#vicon subscription and world frame pose dictionary for rewards
self.vicon_sub = rospy.Subscriber('/vicon/marker1/pose', geometry_msgs.PoseStamped, self.vicon_callback)
self.end_effector_pos = None
self.end_effector_desired = dict()
#robot subscription for state information (dictionary of lists indexed by timestep)
self.robot_state_sub = rospy.Subscriber('/robot/limb/right/gravity_compensation_torques', baxter_core_msgs.SEAJointState, self.robot_state_callback)
self.robot_state_current = dict()
# self.robot_joint_names = None
self.robot_joint_names = ['right_s0', 'right_s1', 'right_e0', 'right_e1', 'right_w0', 'right_w1', 'right_w2']
self.state_keys = ['theta_commanded', 'theta_measured', 'velocity', 'torque']
if self.state_type == 'CL':
self.state_keys.append('world_pose')
#world frame error in current end effector position and desired from parsed file
self.world_error = []
#publish timing topics
self.timing_pub_state = rospy.Publisher('/cycle_bool', std_msgs.Bool, queue_size = 1)
self.timing_msg_state = std_msgs.Bool()
self.timing_pub_time = rospy.Publisher('/cycle_time', std_msgs.Time, queue_size = 1)
self.timing_msg_time = std_msgs.Time()
#maintain dictionaries of variables to dump at the end or query
self.vicon_dump = []
self.state_dump = dict()
self.action_dump = []
self.reward_dump = []
self.average_error = [0, 0, 0]
#programmed trajectories
self.current = 0
self.trajectory_matrix = None
self.sweep_joint_dict = sweep_joint_dict
self.iterations = self.sweep_joint_dict['iterations']
self.stop = False
self.joint_scale = np.array([1/2.5, 1/2.5, 1.5/2.5, 1.5/2.5, 3/2.5, 2/2.5])
self.joint_shift = np.array([-np.pi/6, -np.pi/6, -np.pi/4, np.pi/4, 0, 0])
self.joint_min = [-np.pi/3, -np.pi/3, -np.pi/2, 0, -np.pi/2, -np.pi/3, -np.pi]
self.joint_max = [0, 0, 0, np.pi/2, np.pi/2, np.pi/3, np.pi]
self.random_walk = OrnsteinUhlenbeckProcess(
dimension=6, num_steps=self.iterations,
seed_vec=self.sweep_joint_dict['fixed'][1:-1])
self.random_trajectory = self.random_walk.make_new_walk(length=self.iterations, scale_vec=self.joint_scale, shift_vec=self.joint_shift)
if self.debug:
print("random walk trajectory: \n")
print("shape: ")
print(self.random_trajectory.shape)
print("------- \n")
print("max: ")
print(np.max(self.random_trajectory, 0))
print("\n")
print("min: ")
print(np.min(self.random_trajectory, 0))
print("\n")
print("------- \n")
self.random_trajectory = np.hstack((self.random_trajectory, np.ones((self.iterations+1, 1))*2.7))
def __init__(self, debug=False, goal_type='API', state_type='OL'):
self.debug = debug
self.goal_type = goal_type #default API
self.state_type = state_type #default OL = open loop
if self.goal_type == 'trajectory':
super(RolloutExecuter, self).__init__()
#create our action server clients
self._left_client = actionlib.SimpleActionClient(
'robot/limb/left/follow_joint_trajectory',
FollowJointTrajectoryAction,
)
self._right_client = actionlib.SimpleActionClient(
'robot/limb/right/follow_joint_trajectory',
FollowJointTrajectoryAction,
)
#param namespace
self._param_ns = '/rsdk_joint_trajectory_action_server/'
#create our goal request
self._l_goal = FollowJointTrajectoryGoal()
self._r_goal = FollowJointTrajectoryGoal()
#gripper goal trajectories
self._l_grip = FollowJointTrajectoryGoal()
self._r_grip = FollowJointTrajectoryGoal()
#limb interface - current angles needed for start move
self._l_arm = baxter_interface.Limb('left')
self._r_arm = baxter_interface.Limb('right')
#flag to signify the arm trajectories have begun executing
self._arm_trajectory_started = False
#reentrant lock to prevent same-thread lockout
self._lock = threading.RLock()
#vicon subscription and world frame pose dictionary for rewards
self.vicon_sub = rospy.Subscriber('/vicon/marker_new/pose',
geometry_msgs.PoseStamped,
self.vicon_callback)
self.end_effector_pos = None
self.end_effector_desired = dict()
#robot subscription for state information (dictionary of lists indexed by timestep)
self.robot_state_sub = rospy.Subscriber('robot/joint_states',
sensor_msgs.JointState,
self.robot_state_callback)
self.robot_state_current = dict()
self.robot_joint_names = None
self.state_keys = [
'theta_commanded', 'theta_desired', 'theta_measured', 'velocity',
'torque'
]
if self.state_type == 'CL':
self.state_keys.append('world_pose')
#world frame error in current end effector position and desired from parsed file
self.world_error = []
#publish timing topics
self.timing_pub_state = rospy.Publisher('/cycle_bool',
std_msgs.Bool,
queue_size=1)
self.timing_msg_state = std_msgs.Bool()
self.timing_pub_time = rospy.Publisher('/cycle_time',
std_msgs.Time,
queue_size=1)
self.timing_msg_time = std_msgs.Time()
#maintain dictionaries of variables to dump at the end or query
self.vicon_dump = []
self.state_dump = dict()
self.action_dump = []
self.reward_dump = []
self.average_error = [0, 0, 0]
#stop flag
self.stop = False
def test_msgtime_py():
yield gen_pymsg_test, create(std_msgs.Time), std_msgs.Time(rospy.Time(secs=42, nsecs=31)), {'data': {'secs': 42, 'nsecs': 31}}
def __init__(self):
#create our action server clients
self._left_client = actionlib.SimpleActionClient(
'robot/limb/left/follow_joint_trajectory',
FollowJointTrajectoryAction,
)
self._right_client = actionlib.SimpleActionClient(
'robot/limb/right/follow_joint_trajectory',
FollowJointTrajectoryAction,
)
#verify joint trajectory action servers are available
l_server_up = self._left_client.wait_for_server(rospy.Duration(10.0))
r_server_up = self._right_client.wait_for_server(rospy.Duration(10.0))
if not l_server_up or not r_server_up:
msg = ("Action server not available."
" Verify action server availability.")
rospy.logerr(msg)
rospy.signal_shutdown(msg)
sys.exit(1)
#create our goal request
self._l_goal = FollowJointTrajectoryGoal()
self._r_goal = FollowJointTrajectoryGoal()
#limb interface - current angles needed for start move
self._l_arm = baxter_interface.Limb('left')
self._r_arm = baxter_interface.Limb('right')
#gripper interface - for gripper command playback
self._l_gripper = baxter_interface.Gripper('left', CHECK_VERSION)
self._r_gripper = baxter_interface.Gripper('right', CHECK_VERSION)
#flag to signify the arm trajectories have begun executing
self._arm_trajectory_started = False
#reentrant lock to prevent same-thread lockout
self._lock = threading.RLock()
# Verify Grippers Have No Errors and are Calibrated
if self._l_gripper.error():
self._l_gripper.reset()
if self._r_gripper.error():
self._r_gripper.reset()
if (not self._l_gripper.calibrated()
and self._l_gripper.type() != 'custom'):
self._l_gripper.calibrate()
if (not self._r_gripper.calibrated()
and self._r_gripper.type() != 'custom'):
self._r_gripper.calibrate()
#gripper goal trajectories
self._l_grip = FollowJointTrajectoryGoal()
self._r_grip = FollowJointTrajectoryGoal()
# Timing offset to prevent gripper playback before trajectory has started
self._slow_move_offset = 0.0
self._trajectory_start_offset = rospy.Duration(0.0)
self._trajectory_actual_offset = rospy.Duration(0.0)
#param namespace
self._param_ns = '/rsdk_joint_trajectory_action_server/'
#gripper control rate
self._gripper_rate = 20.0 # Hz
#publish timing topics
self.timing_pub_state = rospy.Publisher('/board_pose/cycle_on',
std_msgs.Bool,
queue_size=0)
self.timing_msg_state = std_msgs.Bool()
self.timing_pub_time = rospy.Publisher('/board_pose/cycle_time',
std_msgs.Time,
queue_size=1)
self.timing_msg_time = std_msgs.Time()