def __init__(self, node):
self.node = node
# Get robot name from parameter server, this is to ensure that the gazebo plugin subscribing to the control
# reads the same name as this code, because the topic depends on the robot name
robot_names = ut_param_server.get_robots(self.node)
self.robot_name = robot_names[0]
self.__joint_names = ut_param_server.get_joints(
self.node, self.robot_name)
joint_control_topic = '/' + self.robot_name + '/control'
self.__control_pub = self.node.create_publisher(
JointControl, joint_control_topic, qos_profile_sensor_data)
self.__joint_state_sub = self.node.create_subscription(
JointState,
"/joint_states",
self.__joint_state_subscription_callback,
qos_profile=qos_profile_parameters)
self.__contact_sub = self.node.create_subscription(
ContactsState,
f"/{self.robot_name}/contacts",
self.__contact_subscription_callback,
qos_profile=qos_profile_sensor_data)
self.__latest_contact_msg = None
self.__latest_joint_state_msg = None
self.__target_joint_state = numpy.array([0.0, 0.0, 0.0])
self.__previous_update_sim_time = rclpyTime()
self.__current_sim_time = rclpyTime()
self.__update_period_ns = 1000000000 / ut_param_server.get_update_rate(
self.node)
# Create reset client to call the reset service
self.__gazebo = GazeboConnection(self.node)
self.__time_lock = threading.Lock()
def __reset_state(self) -> None:
self.__latest_contact_msg = None
self.__latest_joint_state_msg = None
self.__target_joint_state = numpy.array([0.0, 0.0, 0.0])
self.__previous_update_sim_time = rclpyTime()
self.__current_sim_time = rclpyTime()
def __init__(self, node, state_noise_mu: float = None, state_noise_sigma: float = None):
self.node: rclpy.Node = node
self.state_noise_mu = state_noise_mu
self.state_noise_sigma = state_noise_sigma
self.__joint_state_sub = self.node.create_subscription(JointState, "/joint_states",
self.__joint_state_subscription_callback,
qos_profile=qos_profile_parameters)
self._latest_joint_state_msg = None
self._target_joint_state = numpy.array([0.0, 0.0, 0.0])
self._previous_update_sim_time = rclpyTime()
self._current_sim_time = rclpyTime()
self._update_period_ns = 1000000000 / ut_param_server.get_update_rate(self.node)
def __init__(self, node, robot_kwargs: Dict = None):
if robot_kwargs is None:
robot_kwargs = {}
self._gazebo = Gazebo()
super().__init__(node, robot_kwargs)
self._update_period_ns = 1000000000 / ut_param_server.get_update_rate(
self.node)
# Get robot name from parameter server, this is to ensure that the gazebo plugin subscribing to the control
# reads the same name as this code, because the topic depends on the robot name
robot_names = ut_param_server.get_robots(self.node)
self.robot_name = robot_names[0]
self._joint_names = ut_param_server.get_joints(self.node,
self.robot_name)
joint_control_topic = '/' + self.robot_name + '/control'
self._control_pub = self.node.create_publisher(JointControl,
joint_control_topic,
qos_profile_parameters)
self._contact_sub = self.node.create_subscription(
ContactsState,
f'/{self.robot_name}/contacts',
self.__contact_subscription_callback,
qos_profile=qos_profile_services_default)
self._latest_contact_msg = None
self._target_joint_state = numpy.array([0.0, 0.0, 0.0])
self._previous_update_sim_time = rclpyTime()
def _spin_until_update_period_over(self) -> None:
# Loop to block such that when we take observation it is the latest observation when the
# simulation is paused due to the gym training plugin
# Also have a timeout such that when the loop gets stuck it will break itself out
timeout_duration = 1.5
loop_start_time = time.time()
while True:
# spinning the node will cause self._current_sim_time to be updated
rclpy.spin_once(self.node, timeout_sec=0.5)
current_sim_time = copy.copy(self._current_sim_time)
time_diff_ns = current_sim_time.nanoseconds - self._previous_update_sim_time.nanoseconds
if time_diff_ns < 0:
# print("Negative time difference detected, probably due to a reset")
self._previous_update_sim_time = rclpyTime()
time_diff_ns = current_sim_time.nanoseconds
loop_end_time = time.time()
loop_duration = loop_end_time - loop_start_time
if time_diff_ns >= self._update_period_ns or loop_duration > timeout_duration:
break
if loop_duration >= timeout_duration:
srv_time = self._get_current_sim_time_from_srv()
self.node.get_logger().warn(
f"Wait for simulation loop timeout, getting time from service instead"
)
self.node.get_logger().warn(
f'Current sim time: {current_sim_time.nanoseconds}, time from service: {srv_time.nanoseconds}'
)
self._current_sim_time = srv_time
for i in range(20):
rclpy.spin_once(self.node, timeout_sec=0.1)
current_sim_time = copy.copy(self._current_sim_time)
self._previous_update_sim_time = current_sim_time
def __joint_state_subscription_callback(self, message: JointState) -> None:
self._latest_joint_state_msg = message
latest_msg_time = rclpyTime(seconds=message.header.stamp.sec,
nanoseconds=message.header.stamp.nanosec)
# print(f'[{message.header.stamp.sec}][{message.header.stamp.nanosec}]')
if self._current_sim_time < latest_msg_time:
self._current_sim_time = latest_msg_time
def __init__(self,
node,
state_noise_mu=None,
state_noise_sigma=None,
random_init_pos=False):
self.node: rclpy.Node = node
self.state_noise_mu = state_noise_mu
self.state_noise_sigma = state_noise_sigma
self.random_init_pos = random_init_pos
print(
f'-------------------------------Setting robot parameters-------------------------------'
)
print(
f'state_noise_mu: {self.state_noise_mu} # State noise mean (state noise follows gaussian distribution)'
)
print(
f'state_noise_sigma: {self.state_noise_sigma} # State noise standard deviation'
)
print(
f'random_init_pos: {self.random_init_pos} # Random initial positions'
)
print(
f'-------------------------------------------------------------------------------------'
)
if self.state_noise_mu is not None:
assert isinstance(
self.state_noise_mu, float
), f'state_noise_mu should be a float, current type: {type(self.state_noise_mu)}'
if self.state_noise_sigma is not None:
assert isinstance(
self.state_noise_sigma, float
), f'state_noise_sigma should be a float, current type: {type(self.state_noise_sigma)}'
qos_profile = QoSProfile(reliability=1, depth=100)
self.__joint_state_sub = self.node.create_subscription(
JointState,
'/joint_states',
self.__joint_state_subscription_callback,
qos_profile=qos_profile)
# parameters = keep_last (1000), reliable, all else default
self._latest_joint_state_msg = None
self._target_joint_state = numpy.array([0.0, 0.0, 0.0])
self._previous_update_sim_time = rclpyTime()
self._current_sim_time = rclpyTime()
self._latest_contact_msg = None
def __init__(self, node, robot_kwargs):
self.node: rclpy.Node = node
self.state_noise_mu = robot_kwargs.get('state_noise_mu', 0.0)
self.state_noise_sigma = robot_kwargs.get('state_noise_sigma', 0.0)
self.random_init_pos = robot_kwargs.get('random_init_pos', False)
qos_profile = QoSProfile(reliability=1, depth=100)
self.__joint_state_sub = self.node.create_subscription(
JointState,
'/joint_states',
self.__joint_state_subscription_callback,
qos_profile=qos_profile)
# parameters = keep_last (1000), reliable, all else default
self._latest_joint_state_msg = None
self._target_joint_state = numpy.array([0.0, 0.0, 0.0])
self._previous_update_sim_time = rclpyTime()
self._current_sim_time = rclpyTime()
self._latest_contact_msg = None
def _get_current_sim_time_from_srv(self) -> rclpyTime:
client = self.node.create_client(GetCurrentSimTime, "/get_current_sim_time")
req = GetCurrentSimTime.Request()
retry_count = 0
while not client.wait_for_service(timeout_sec=1.0) and retry_count < 10:
self.node.get_logger().info('/get_current_sim_time service not available, waiting again...')
retry_count += 1
future = client.call_async(req)
rclpy.spin_until_future_complete(self.node, future)
if future.result() is not None:
current_sim_time_sec = future.result().sec
current_sim_time_nsec = future.result().nanosec
current_sim_time = rclpyTime(seconds=current_sim_time_sec, nanoseconds=current_sim_time_nsec)
return current_sim_time
else:
self.node.get_logger().warn('/get_current_sim_time service call failed')
return rclpyTime()
def _reset_state(self) -> None:
super()._reset_state()
self._latest_contact_msg = None
# We assume that if we do random initial position, the target joint state will be set after we call the random and the positions are obtained
# So we only set target_joint_state when is not random_init_pos
if not self.random_init_pos:
print(f'Setting 0 initial positions')
self._target_joint_state = numpy.array([0.0, 0.0, 0.0])
self._previous_update_sim_time = rclpyTime()
def __joint_state_subscription_callback(self, message: JointState) -> None:
self.__latest_joint_state_msg = message
with self.__time_lock:
self.__current_sim_time = rclpyTime(
seconds=message.header.stamp.sec,
nanoseconds=message.header.stamp.nanosec)
