def _load_controller(self, controller_name):
if not self._is_controller_loaded(LBR_JOINT_TRAJECTORY_CONTROLLER):
request = LoadControllerRequest()
request.name = controller_name
response = self._load_service(request)
if response.ok:
rospy.loginfo('Loaded controller %s' % controller_name)
else:
rospy.loginfo('Loading controller %s failed' % controller_name)
return response.ok
return True
def load_controllers(self, load_controllers_name):
rospy.wait_for_service(self.load_service_name)
try:
load_request_object = LoadControllerRequest()
load_request_object.name = load_controllers_name
load_result = self.load_service(load_request_object)
"""
[controller_manager_msgs/LoadController]
string name
---
bool ok
"""
rospy.logdebug("Load Result==>" + str(load_result.ok))
return load_result.ok
except rospy.ServiceException as e:
print(self.load_service_name + " service call failed")
return None
def ensure_controllers_are_loaded():
currently_loaded = cont_list_server() # type: ListControllersResponse
loaded_controllers = {
x.name for x in currently_loaded.controller if x.name in NEED_CONTROLLERS
}
if loaded_controllers < NEED_CONTROLLERS: # do we need more controllers loaded?
need_to_load = NEED_CONTROLLERS - loaded_controllers
rospy.loginfo("need to load {}".format(", ".join(need_to_load)))
for i in need_to_load:
req = LoadControllerRequest(name=i)
if not cont_load(req).ok:
rospy.logerr("couldn't start controller {}".format(i))
exit(1)
else:
rospy.loginfo("all controllers loaded")
# roslib.load_manifest("rosparam")
# import rosparam
from rospkg import RosPack
from controller_manager_msgs.srv import LoadController, LoadControllerRequest
if __name__ == '__main__':
rospy.init_node('activate_observer_node')
rospack = RosPack()
# param = rosparam.load_file(rospack.get_path('metacontrol_sim')+'/yaml/goal.yaml')
observer_dict = load(
file(
rospack.get_path('metacontrol_sim') + '/yaml/observers.yaml', 'r'))
for observer_entry in observer_dict:
print observer_entry
observer_req = LoadControllerRequest()
observer_req.name = observer_dict[observer_entry]['name']
print observer_req.name
# Wait for the observer service to become available.
rospy.loginfo("Waiting for service...")
rospy.wait_for_service('/load_observer')
try:
# Create a service proxy.
observer_srv_proxy = rospy.ServiceProxy('/load_observer',
LoadController)
# Call the service here.
service_response = observer_srv_proxy(observer_req)
print("The load observer service call is completed!")
except rospy.ServiceException, e:
print "load observer Service call failed: %s" % e
def __init__(self):
self.goal_distance_threshold = 0.05
self.distance_reward_coeff = 1
self._action_spec = array_spec.BoundedArraySpec(
shape=(4, ),
dtype=np.float32,
minimum=[-0.5, -0.5, -0.5, -0.5],
maximum=[-0.5, -0.5, -0.5, 0.5],
name='action')
self._observation_spec = array_spec.BoundedArraySpec(
shape=(7, ),
dtype=np.float32,
minimum=[-1.5, -1.5, -1.5, -2.5, -0.4, -0.4, 0],
maximum=[1.5, 1.5, 1.5, 2.5, 0.4, 0.4, 0.4],
name='observation')
self.joint_angles = np.zeros(4)
self._episode_ended = False
self._reached_goal = False
self.joint_names = [
'plat_joint', 'shoulder_joint', 'forearm_joint', 'wrist_joint'
]
self.all_joints = AllJoints(self.joint_names)
# Physics Service Proxies
self.pause_physics_proxy = rospy.ServiceProxy('/gazebo/pause_physics',
Empty)
self.unpause_physics_proxy = rospy.ServiceProxy(
'/gazebo/unpause_physics', Empty)
# Sim Reset Proxy
self.reset_proxy = rospy.ServiceProxy('gazebo/reset_simulation', Empty)
# Controller Loading Service
self.load_controller_proxy = rospy.ServiceProxy(
'/arm/controller_manager/load_controller', LoadController)
self.joint_state_controller_load = LoadControllerRequest()
self.joint_state_controller_load.name = 'joint_state_controller'
self.arm_controller_load = LoadControllerRequest()
self.arm_controller_load.name = 'arm_controller'
# Controller Switching Service
self.switch_controller_proxy = rospy.ServiceProxy(
'/arm/controller_manager/switch_controller', SwitchController)
self.switch_controller = SwitchControllerRequest()
self.switch_controller.start_controllers.append(
'joint_state_controller')
self.switch_controller.start_controllers.append('arm_controller')
self.switch_controller.strictness = 2
# Model Deleting and Spawning Services, Defining Arm Model Params
self.del_model = rospy.ServiceProxy('/gazebo/delete_model',
DeleteModel)
self.spawn_model_proxy = rospy.ServiceProxy('/gazebo/spawn_urdf_model',
SpawnModel)
self.arm_urdf = open(arm_model_dir, "r").read()
self.arm_model = SpawnModelRequest()
self.arm_model.model_name = 'arm' # the same with sdf name
self.arm_model.model_xml = self.arm_urdf
self.arm_model.robot_namespace = 'arm'
self.initial_pose = Pose()
self.initial_pose.position.z = 0.0305
self.arm_model.initial_pose = self.initial_pose
self.arm_model.reference_frame = 'world'
# Defining Sphere (Goal Position Indicator) Model Params
self.sphere_urdf = open(sphere_dir, "r").read()
self.sphere = SpawnModelRequest()
self.sphere.model_name = 'simple_ball' # the same with sdf name
self.sphere.model_xml = self.sphere_urdf
self.sphere.robot_namespace = 'arm'
self.sphere_initial_pose = Pose()
# self.sphere_initial_pose.position.x = self.goal_pos[0]
# self.sphere_initial_pose.position.y = self.goal_pos[1]
# self.sphere_initial_pose.position.z = self.goal_pos[2]
# self.sphere.initial_pose = self.sphere_initial_pose
self.sphere.reference_frame = 'world'
self.set_new_goal()
self.tf_buffer = tf2_ros.Buffer()
self.tf_listener = tf2_ros.TransformListener(self.tf_buffer)
self.joint_pos_high = np.array([1.5, 1.5, 1.5, 2.5])
self.joint_pos_low = np.array([-1.5, -1.5, -1.5, -2.5])
self.joint_pos_range = self.joint_pos_high - self.joint_pos_low
self._state = np.zeros(7)
print("INIT SHAPE BOIII", np.shape(self._state))
self.joint_state_subscriber = rospy.Subscriber(
'/joint_states', JointState, self.joint_state_subscriber_callback)
def __init__(self, static_goal, slow_step=False, larger_radius=False):
self.max_sim_time = 15
if (larger_radius):
self.goal_radius = 0.06 #sphere radius in ball.urdf is slightly bigger to improve visuals
else:
self.goal_radius = 0.05
self.distance_reward_coeff = 5
self.static_goal_pos = np.array([0.2, 0.1, 0.15])
self.static_goal = static_goal
self.slow_step = slow_step
self.eef_pos = np.zeros(3)
self.zero = np.array([0, 0, 0, 0])
#rospy.init_node('joint_position_node')
self.num_joints = 4
self.observation_space = spaces.Box(
np.array([-1.5, -1.5, -1.5, -2.5, -0.4, -0.4, 0]),
np.array([1.5, 1.5, 1.5, 2.5, 0.4, 0.4,
0.4])) #(self.num_joints + 3,)
self.action_space = spaces.Box(np.array([-0.2, -0.2, -0.2, -0.2]),
np.array([0.2, 0.2, 0.2, 0.2]))
self.joint_names = [
'plat_joint', 'shoulder_joint', 'forearm_joint', 'wrist_joint'
]
self.all_joints = AllJoints(self.joint_names)
self.starting_pos = np.array([0, 0, 0, 0])
self.last_goal_distance = 0
rospy.loginfo("Defining a goal position...")
if (self.static_goal):
self.goal_pos = self.static_goal_pos
else:
while (True):
x_y = np.random.uniform(low=-0.4, high=0.4, size=2)
z = np.random.uniform(low=0, high=0.4, size=1)
self.goal_pos = np.concatenate([x_y, z], axis=0)
if (np.linalg.norm(self.goal_pos) < 0.4):
break
rospy.loginfo("Goal position defined")
self.pause_physics_proxy = rospy.ServiceProxy('/gazebo/pause_physics',
Empty,
persistent=False)
self.unpause_physics_proxy = rospy.ServiceProxy(
'/gazebo/unpause_physics', Empty, persistent=False)
self.reset_proxy = rospy.ServiceProxy('gazebo/reset_simulation',
Empty,
persistent=False)
#self.config_proxy = rospy.ServiceProxy('/gazebo/set_model_configuration', SetModelConfiguration)
# self.config_request = SetModelConfigurationRequest()
# self.config_request.model_name = 'arm'
# self.config_request.urdf_param_name = 'arm/robot_description'
# self.config_request.joint_names = self.joint_names
# self.config_request.joint_positions = self.starting_pos
self.load_controller_proxy = rospy.ServiceProxy(
'/arm/controller_manager/load_controller',
LoadController,
persistent=False)
self.joint_state_controller_load = LoadControllerRequest()
self.joint_state_controller_load.name = 'joint_state_controller'
self.arm_controller_load = LoadControllerRequest()
self.arm_controller_load.name = 'arm_controller'
self.switch_controller_proxy = rospy.ServiceProxy(
'/arm/controller_manager/switch_controller',
SwitchController,
persistent=False)
self.switch_controller = SwitchControllerRequest()
self.switch_controller.start_controllers.append(
'joint_state_controller')
self.switch_controller.start_controllers.append('arm_controller')
self.switch_controller.strictness = 2
self.del_model = rospy.ServiceProxy('/gazebo/delete_model',
DeleteModel,
persistent=False)
self.spawn_model_proxy = rospy.ServiceProxy('/gazebo/spawn_urdf_model',
SpawnModel,
persistent=False)
self.arm_urdf = open(arm_model_dir, "r").read()
self.arm_model = SpawnModelRequest()
self.arm_model.model_name = 'arm' # the same with sdf name
self.arm_model.model_xml = self.arm_urdf
self.arm_model.robot_namespace = 'arm'
self.initial_pose = Pose()
self.initial_pose.position.z = 0.0305
self.arm_model.initial_pose = self.initial_pose
self.arm_model.reference_frame = 'world'
self.state_proxy = rospy.ServiceProxy('/gazebo/set_model_state',
SetModelState,
persistent=False)
self.config_request = SetModelStateRequest()
self.config_request.model_state.model_name = 'simple_ball'
sphere_pose = Pose()
sphere_pose.position.x = 1
sphere_pose.position.y = 2
sphere_pose.position.z = 3
self.config_request.model_state.pose = sphere_pose
self.config_request.model_state.reference_frame = 'world'
# self.config_request.urdf_param_name = 'arm/robot_description'
# self.config_request.joint_names = self.joint_names
# self.config_request.joint_positions = self.starting_pos
self.sphere_urdf = open(sphere_dir, "r").read()
self.sphere = SpawnModelRequest()
self.sphere.model_name = 'simple_ball' # the same with sdf name
self.sphere.model_xml = self.sphere_urdf
self.sphere.robot_namespace = 'arm'
self.sphere_initial_pose = Pose()
self.sphere_initial_pose.position.x = self.goal_pos[0]
self.sphere_initial_pose.position.y = self.goal_pos[1]
self.sphere_initial_pose.position.z = self.goal_pos[2]
self.sphere.initial_pose = self.sphere_initial_pose
self.sphere.reference_frame = 'world'
# rospy.wait_for_service('/gazebo/spawn_urdf_model')
# try:
# self.spawn_model_proxy(self.sphere.model_name, self.sphere.model_xml, self.sphere.robot_namespace, self.sphere.initial_pose, 'world')
# except (rospy.ServiceException) as e:
# print("/gazebo/failed to build the target")
self.unpause_physics()
#rospy.wait_for_service('/gazebo/delete_model')
# self.del_model('arm')
#self.del_model('simple_ball')
self.spawn_model(self.sphere)
#self.tf_listener = tf.TransformListener()
self.tf_buffer = tf2_ros.Buffer()
self.tf_listener = tf2_ros.TransformListener(self.tf_buffer)
self.joint_pos_high = np.array([1.5, 1.5, 1.5, 2.5])
self.joint_pos_low = np.array([-1.5, -1.5, -1.5, -2.5])
self.joint_pos_range = self.joint_pos_high - self.joint_pos_low
self.joint_pos_mid = self.joint_pos_range / 2.0
self.joint_pos = np.zeros(4)
self.joint_state = np.zeros(self.num_joints)
self.joint_state_subscriber = rospy.Subscriber(
'arm/arm_controller/state',
JointTrajectoryControllerState,
self.joint_state_subscriber_callback,
queue_size=1)
self.normed_sp = self.normalize_joint_state(self.starting_pos)
self.clock_subscriber = rospy.Subscriber(
'/clock', Clock, self.clock_subscriber_callback, queue_size=1)
self.hit_floor = False
def change_to_controller(controller_to_start, start_wrist_in_position):
list_controller_response = get_controllers_list()
from_controller = get_running_controller(list_controller_response)
if controller_to_start.startswith('w'): # wholebodycontrol
to_controller = 'whole_body_kinematic_controller'
elif controller_to_start.startswith('g'): # position arm
to_controller = 'gravity_compensation'
elif controller_to_start.startswith('p'): # gravity compensation
to_controller = 'position'
else:
rospy.logerr(
"Given a controller to start that isn't one of:" +
"'[w]hole_body_kinematic_controler' or '[g]ravity_compensation' or '[p]osition'"
)
return False
if from_controller == to_controller:
rospy.logwarn(
"Asked to change to the mode we already are, doing nothing.")
return False
# position control (head, arm, torso).")
rospy.loginfo("Changing from '" + str(from_controller) + "' to '" +
to_controller + "'")
if (to_controller != 'position'):
if not is_controller_in_state(to_controller, 'stopped',
list_controller_response):
rospy.loginfo("Loading controller '" + to_controller + "'")
load_srv = rospy.ServiceProxy(CONTROLLER_MANAGER_LOAD_SRV,
LoadController)
load = LoadControllerRequest()
load.name = str(to_controller)
resp_load = load_srv.call(load)
if not resp_load.ok:
rospy.logerr(
"Could not load controller. Check if the parameters are loaded properly."
)
return False
switch_srv = rospy.ServiceProxy(CONTROLLER_MANAGER_SWITCH_SRV,
SwitchController)
if from_controller == 'whole_body_kinematic_controller':
if to_controller == 'gravity_compensation':
go_to_position_arm_head_torso_stop_wbc(switch_srv)
go_to_gravity_compensation(switch_srv, start_wrist_in_position)
elif to_controller == 'position':
go_to_position_arm_head_torso_stop_wbc(switch_srv)
else:
rospy.logerr("Unexpected controller switch.")
elif from_controller == 'gravity_compensation':
if to_controller == 'whole_body_kinematic_controller':
go_to_position(switch_srv, start_wrist_in_position)
go_to_whole_body_kinematics(switch_srv)
elif to_controller == 'position':
go_to_position(switch_srv, start_wrist_in_position)
else:
rospy.logerr("Unexpected controller switch.")
elif from_controller == 'position':
if to_controller == 'gravity_compensation':
go_to_gravity_compensation(switch_srv, start_wrist_in_position)
elif to_controller == 'whole_body_kinematic_controller':
go_to_whole_body_kinematics(switch_srv)
else:
rospy.logerr("Unexpected controller switch.")
else:
rospy.logerr("Unexpected controller switch.")
rospy.loginfo("Done.")