def switch_controllers(self, controllers_on, controllers_off,
strictness=1):
"""
Give the controllers you want to switch on or off.
:param controllers_on: ["name_controler_1", "name_controller2",...,"name_controller_n"]
:param controllers_off: ["name_controler_1", "name_controller2",...,"name_controller_n"]
:return:
"""
rospy.wait_for_service(self.switch_service_name)
try:
switch_request_object = SwitchControllerRequest()
switch_request_object.start_controllers = controllers_on
switch_request_object.start_controllers = controllers_off
switch_request_object.strictness = strictness
switch_result = self.switch_service(switch_request_object)
"""
[controller_manager_msgs/SwitchController]
int32 BEST_EFFORT=1
int32 STRICT=2
string[] start_controllers
string[] stop_controllers
int32 strictness
---
bool ok
"""
rospy.logdebug("Switch Result==>" + str(switch_result.ok))
return switch_result.ok
except rospy.ServiceException as e:
print(self.switch_service_name + " service call failed")
return None
def __init__(self, airskin_topic, bumper_topic, wrist_topic):
while rospy.get_time() == 0.0:
pass
rospy.loginfo(rospy.get_caller_id() + ': starting up')
self.wrist_sub = rospy.Subscriber(wrist_topic, Bool, self.wrist_callback)
self.bumper_sub = rospy.Subscriber(bumper_topic, Bool, self.bumper_callback)
self.airskin_sub = rospy.Subscriber(airskin_topic, Bool, self.airskin_callback)
self.safety_pub = rospy.Publisher('/squirrel_safety', Safety, queue_size=10)
self.safety_reset_sub = rospy.Subscriber('/squirrel_safety/reset', Bool, self.safety_reset_callback)
self.controller_reset_ = rospy.ServiceProxy('/arm_controller/controller_manager/switch_controller', SwitchController)
rate = rospy.Rate(10)
self.safety_msg = Safety()
self.safety_msg.emergency_state = False
self.safety_msg.airskin_stop = False
self.safety_msg.wrist_stop = False
self.safety_msg.bumper_stop = False
while not rospy.is_shutdown():
if self.safety_msg.wrist_stop or self.safety_msg.bumper_stop or self.safety_msg.airskin_stop:
self.safety_msg.emergency_state = True
req = SwitchControllerRequest()
req.start_controllers = []
req.stop_controllers = ['joint_trajectory_controller']
req.strictness = 2
if self.controller_reset_(req):
rospy.loginfo('Safety set.')
else:
rospy.logerror('Failed to set safety. Please kick it again or manually shut down and panic!')
self.safety_pub.publish(self.safety_msg)
self.safety_msg.emergency_state = False
rate.sleep()
def switch_on_controller(self, controller_name):
"""Switches on the given controller stopping all other known controllers with best_effort
strategy."""
srv = SwitchControllerRequest()
srv.stop_controllers = ALL_CONTROLLERS
srv.start_controllers = [controller_name]
srv.strictness = SwitchControllerRequest.BEST_EFFORT
result = self.switch_controllers_client(srv)
self.assertTrue(result.ok)
def start_ros_controllers():
start_controllers_client = rospy.ServiceProxy(SWITCH_CONTROLLER,
SwitchController)
req = SwitchControllerRequest()
# this two controller is same as the ur joint trajectory controller
req.start_controllers.append("ur5_cartesian_velocity_controller_sim")
req.start_controllers.append("joint_state_controller")
req.strictness = req.BEST_EFFORT
rospy.wait_for_service(SWITCH_CONTROLLER)
res = start_controllers_client(req)
def safety_reset_callback(self, msg):
if msg.data == True:
req = SwitchControllerRequest()
req.stop_controllers = []
req.start_controllers = ['joint_trajectory_controller']
req.strictness = 2
if self.controller_reset_(req):
rospy.loginfo('Safety released.')
else:
rospy.logerror('Failed to release safety. Please try again or manually restart both the controller and the safety node!')
def start_ros_controllers():
start_controllers_client = rospy.ServiceProxy(SWITCH_CONTROLLER, SwitchController)
req = SwitchControllerRequest()
req.start_controllers.append("ur5_cartesian_velocity_controller_sim")
req.start_controllers.append("joint_state_controller")
req.start_controllers.append("ur5_joint_publisher")
req.start_controllers.append("ur5_velocity_controller")
req.strictness = req.BEST_EFFORT
rospy.wait_for_service(SWITCH_CONTROLLER)
res = start_controllers_client(req)
def switch_to_traj_ctrl(self):
'''
call service /controller_manager/switch_controller
start trajectory ctrl, stop position + velocity ctrl
'''
req = SwitchControllerRequest()
req.start_controllers = self.trajectory_ctrler_start_stop_list[0]
req.stop_controllers = self.trajectory_ctrler_start_stop_list[1]
req.strictness = req.BEST_EFFORT
self.switch_ctrler_srv(req)
# to keep in memory the running controller
self.running_ctler = 'trajectory'
rospy.loginfo('Automatically switched to trajectory ctrl')
def _deactivate_controller(self, controller_name):
if self._is_controller_running(controller_name):
request = SwitchControllerRequest()
request.stop_controllers = [controller_name]
request.strictness = request.STRICT
response = self._switch_controller_service(request)
if response.ok:
rospy.loginfo('Deactivated controller %s' % controller_name)
else:
rospy.loginfo('Deactivating controller %s failed' %
controller_name)
return response.ok
return True
def start_ros_controllers():
start_controllers_client = rospy.ServiceProxy(SWITCH_CONTROLLER,
SwitchController)
req = SwitchControllerRequest()
# this two controller is same as the ur joint trajectory controller
req.start_controllers.append("joint_state_controller")
# req.start_controllers.append("my_cartesian_compliance_controller")
# req.start_controllers.append("my_cartesian_force_controller")
req.start_controllers.append("arm_controller")
# req.start_controllers.append("my_cartesian_motion_controller")
# req.start_controllers.append("my_motion_control_handle")
req.strictness = req.BEST_EFFORT
rospy.wait_for_service(SWITCH_CONTROLLER)
res = start_controllers_client(req)
def execute(self, userdata):
if self.__enable_orientation_ff:
self.__orientation_init.call(EmptyRequest())
req = SwitchControllerRequest()
req.stop_controllers = []
req.start_controllers = ["compliance_controller"]
req.strictness = 2
self.__switcher.call(req)
grip_req = SetBoolRequest()
grip_req.data = True
self.__clients.call(grip_req)
return "linked"
def execute(self, userdata):
grip_req = SetBoolRequest()
grip_req.data = False
self.__clients.call(grip_req)
rospy.sleep(2)
req = SwitchControllerRequest()
req.stop_controllers = ["compliance_controller"]
req.start_controllers = ["position_joint_controller"]
req.strictness = 2
self.__switcher.call(req)
if self.__enable_orientation_ff:
self.__orientation_init.call(EmptyRequest())
return "released"
def btn_left_set_load_zero_on_click(self):
req = SetLoadRequest(mass=0.0)
stop = SwitchControllerRequest(
stop_controllers=['assembly_triple_controller'], strictness=2)
start = SwitchControllerRequest(
start_controllers=['assembly_triple_controller'], strictness=2)
try:
self.switch_proxy(stop)
rospy.sleep(0.5)
self.left_load_proxy(req)
self.switch_proxy(start)
self._widget.textBrowser.append("LEFT: set_load_zero")
except:
self._widget.textBrowser.append(
"SERVER NOT WORKING -- LEFT: set_load_zero")
def switch_ctrl(self, name=0):
print "switch_controller"
self.switch_controller_req = SwitchControllerRequest()
if name == 0:
self.switch_controller_req.start_controllers = ['move_1_position_ctrlr', 'move_2_position_ctrlr',
'move_3_position_ctrlr','joint_state_ctrlr', 'arm_controller']
self.switch_controller_req.stop_controllers = ['move_1_velocity_ctrlr', 'move_2_velocity_ctrlr',
'move_3_velocity_ctrlr',
'move_1_effort_ctrlr', 'move_2_effort_ctrlr',
'move_3_effort_ctrlr']
elif name == 1:
self.switch_controller_req.start_controllers = ['move_1_velocity_ctrlr', 'move_2_velocity_ctrlr',
'move_3_velocity_ctrlr','joint_state_ctrlr','arm_controller']
self.switch_controller_req.stop_controllers = ['move_1_position_ctrlr', 'move_2_position_ctrlr',
'move_3_position_ctrlr'
'move_1_effort_ctrlr',
'move_2_effort_ctrlr', 'move_3_effort_ctrlr']
else:
self.switch_controller_req.start_controllers = ['move_1_effort_ctrlr', 'move_2_effort_ctrlr',
'move_3_effort_ctrlr','joint_state_ctrlr','arm_controller']
self.switch_controller_req.stop_controllers = ['move_1_position_ctrlr', 'move_2_position_ctrlr',
'move_3_position_ctrlr',
'move_1_velocity_ctrlr', 'move_2_velocity_ctrlr',
'move_3_velocity_ctrlr']
self.switch_controller_req.strictness = 1
try:
self.switch_controller.call(self.switch_controller_req)
except rospy.ServiceException, e:
print "Service call failed: %s" % e
def btn_left_set_load_on_click(self):
item = self._widget.combo_left_load_item.currentText()
req = self.load_dict[item]
stop = SwitchControllerRequest(
stop_controllers=['assembly_triple_controller'], strictness=2)
start = SwitchControllerRequest(
start_controllers=['assembly_triple_controller'], strictness=2)
try:
self.switch_proxy(stop)
rospy.sleep(0.5)
self.left_load_proxy(req)
self.switch_proxy(start)
self._widget.textBrowser.append("LEFT: set_load_" + item)
self._widget.textBrowser.append(str(self.load_dict[item]))
except:
self._widget.textBrowser.append(
"SERVER NOT WORKING -- LEFT: set_load_" + item)
def set_controller_off_request(self):
# Request to switch off controllers.
self.switch_controllers_off_request = SwitchControllerRequest()
self.switch_controllers_off_request.start_controllers = []
# self.switch_controllers_off_request.stop_controllers = ['joint_state_controller','left_joint_position_controller','right_joint_position_controller']
self.switch_controllers_off_request.stop_controllers = [
'joint_state_controller'
]
self.switch_controllers_off_request.strictness = 2
def switch_controller(self, mode=None):
req = SwitchControllerRequest()
res = SwitchControllerResponse()
req.start_asap = False
req.timeout = 0.0
if mode == 'velocity':
req.start_controllers = ['joint_group_vel_controller']
req.stop_controllers = ['scaled_pos_traj_controller']
req.strictness = req.STRICT
elif mode == 'position':
req.start_controllers = ['scaled_pos_traj_controller']
req.stop_controllers = ['joint_group_vel_controller']
req.strictness = req.STRICT
else:
rospy.logwarn('Unkown mode for the controller!')
res = self.switch_controller_cli.call(req)
def execute(self, userdata):
req = SwitchControllerRequest()
req.stop_controllers = ["position_joint_controller"]
req.start_controllers = ["cartesian_controller"]
req.strictness = 2
self.__switcher.call(req)
srv = rospy.Service("~trigger", Empty, self.__callback__)
while not self.__enable:
rospy.sleep(self.__timeout)
self.__enable = False
srv.shutdown()
req = SwitchControllerRequest()
req.stop_controllers = ["cartesian_controller"]
req.start_controllers = []
req.strictness = 2
self.__switcher.call(req)
return "adjusted"
def set_controller_on_request(self):
# Creating service request to switch controllers on and off separately.
# Request to switch on controllers.
self.switch_controllers_on_request = SwitchControllerRequest()
# self.switch_controllers_on_request.start_controllers = ['joint_state_controller','left_joint_position_controller','right_joint_position_controller']
self.switch_controllers_on_request.start_controllers = [
'joint_state_controller'
]
self.switch_controllers_on_request.stop_controllers = []
self.switch_controllers_on_request.strictness = 2
def set_manual_mode_on_arm():
cont_server(SwitchControllerRequest(
start_controllers=[
"arm_manual_controller",
"drill_controller"
],
stop_controllers=[
],
strictness=1
))
def go_to_position(switch_srv):
scr = SwitchControllerRequest()
scr.start_controllers.append('arm_controller')
scr.stop_controllers.append('gravity_compensation')
scr.stop_controllers.append('wrist_controller')
rospy.loginfo("Switch controllers: " + str(scr))
resp = switch_srv.call(scr)
if not resp.ok:
rospy.logerr(
"Could not switch. You may want to check rqt_console and try again."
)
def go_to_position_arm_head_torso_stop_wbc(switch_srv):
scr = SwitchControllerRequest()
scr.start_controllers.append('arm_controller')
scr.start_controllers.append('head_controller')
scr.start_controllers.append('torso_controller')
scr.stop_controllers.append('whole_body_kinematic_controler')
rospy.loginfo("Switch controllers: " + str(scr))
resp = switch_srv.call(scr)
if not resp.ok:
rospy.logerr(
"Could not switch. You may want to check rqt_console and try again."
)
def switch_to_position_controller():
rospy.wait_for_service("/controller_manager/list_controllers")
rate = rospy.Rate(1)
list_controllers = rospy.ServiceProxy(
"/controller_manager/list_controllers", ListControllers)
position_controller_available = False
while not position_controller_available:
res = list_controllers(ListControllersRequest())
position_controller_available = any([
controller.name == "joint_group_position_controller"
for controller in res.controller
])
rate.sleep()
rospy.logdebug("Switching controllers")
rospy.wait_for_service("/controller_manager/switch_controller")
switch_controller = rospy.ServiceProxy(
'/controller_manager/switch_controller', SwitchController)
req = SwitchControllerRequest()
req.start_controllers = ["joint_group_position_controller"]
req.stop_controllers = ["arm_controller"]
req.strictness = 2
switch_controller(req)
def __init__(self,base_namespaces,arm_namespaces):
smach.State.__init__(self, outcomes=['startup_done',"references_error"],
io_keys=['slaves']
)
self.__enable_manipulator=rospy.get_param("~enable_manipulator",False)
self.__arm_controller=self.__switcher=ClientDistributor(arm_namespaces,ServiceConfig("controller_manager/switch_controller",SwitchController))
self.__arm_namespaces=arm_namespaces
self.__base_namespaces=base_namespaces
self.__arm_request=SwitchControllerRequest()
self.__arm_request.start_controllers=["position_joint_controller"]
self.__arm_request.strictness=2
if self.__enable_manipulator:
self.__gripper_clients=ClientDistributor(arm_namespaces,ServiceConfig("grip",SetBool))
self.__formation_disabler=ClientDistributor(base_namespaces,ServiceConfig("slave_controller/disable_controller",Empty))
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)
from gazebo_msgs.srv import SetModelState, SetModelStateRequest, SetModelConfiguration, SetModelConfigurationRequest
import numpy as np
rospy.init_node("test")
joint_names = ['plat_joint','shoulder_joint','forearm_joint','wrist_joint']
starting_pos = np.array([0, 0, 1, 2])
model_config_proxy = rospy.ServiceProxy('/gazebo/set_model_configuration',SetModelConfiguration)
model_config_req = SetModelConfigurationRequest()
model_config_req.model_name = 'arm'
model_config_req.urdf_param_name = 'robot_description'
model_config_req.joint_names = joint_names
model_config_req.joint_positions = starting_pos
controller_switch_proxy = rospy.ServiceProxy('/controller_manager/switch_controller',SwitchController)
controller_switch_off = SwitchControllerRequest()
controller_switch_off.stop_controllers = ['joint_state_controller','arm_controller']
controller_switch_off.strictness = 2
controller_switch_on = SwitchControllerRequest()
controller_switch_on.start_controllers = ['joint_state_controller','arm_controller']
controller_switch_on.strictness = 2
rospy.wait_for_service('/controller_manager/switch_controller')
try:
controller_switch_proxy(controller_switch_off)
except rospy.ServiceException, e:
print('/controller_manager/switch_controller call failed')
rospy.wait_for_service('/gazebo/set_model_configuration')
if __name__ == "__main__":
try:
rospy.init_node("callback_control_floorbot", anonymous=True)
rospy.loginfo("Waiting for /floor/joint_states message")
rospy.wait_for_message("/floor/joint_states", JointState, timeout=None)
rospy.loginfo("Waiting for /floor/controller_manager/switch_controller")
rospy.wait_for_service("/floor/controller_manager/switch_controller")
# make a callable Service Proxy object (rospy interface to service)
s = rospy.ServiceProxy("/floor/controller_manager/switch_controller",
SwitchController)
# Call the switch
resp = s.call(SwitchControllerRequest(["joint_position_controller"], # Start these guys
["position_trajectory_controller"], # stop these guys
SwitchControllerRequest.STRICT))
if resp.ok == 1:
rospy.loginfo("Controller switched!")
else:
rospy.loginfo("Controller not switched!")
quit()
rospy.loginfo("Starting")
fc = FloorController()
rospy.sleep(5)
currj = fc.getJointState()
rospy.loginfo("We are at "+str(currj))
goalj = [i for i in currj] # shallow copy
def before_restore(self):
unload = rospy.ServiceProxy(
"/victor/controller_manager/switch_controller", SwitchController)
unload(
SwitchControllerRequest(
stop_controllers=["both_arms_trajectory_controller"]))
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
