def calc_torque(self): mass = 0.3 px_arr = np.zeros(self.pile.num_particle) py_arr = np.zeros(self.pile.num_particle) pz_arr = np.zeros(self.pile.num_particle) for i in range(1, self.pile.num_particle + 1): if self.particle_index[i] == 1: get_particle_state_req = GetModelStateRequest() get_particle_state_req.model_name = 'particle' + str(i) get_particle_state_req.relative_entity_name = 'bucket' # 'world' particle_state = self.get_model_state_proxy( get_particle_state_req) x = particle_state.pose.position.x y = particle_state.pose.position.y z = particle_state.pose.position.z orientation = particle_state.pose.orientation (roll, pitch, theta) = euler_from_quaternion([ orientation.x, orientation.y, orientation.z, orientation.w ]) px_arr[i - 1] = x py_arr[i - 1] = y pz_arr[i - 1] = z self.torque_sum += mass * 9.80665 * abs(x) * math.sin(roll)
def get_robot_pose(self): request = GetModelStateRequest() request.model_name = self._model_name request.relative_entity_name = self._world_link response = self._get_model_state_service.call(request) if not response.success: rospy.logwarn( 'Problem when getting pose between %s and %s. Details: %s' % (self._world_link, self._model_name, response.status_message)) return None return response.pose
def particle_location(self, num_p): px_arr = np.zeros(num_p) py_arr = np.zeros(num_p) pz_arr = np.zeros(num_p) for i in range(1, num_p + 1): get_particle_state_req = GetModelStateRequest() get_particle_state_req.model_name = 'particle' + str(i) get_particle_state_req.relative_entity_name = 'base_footprint' # 'world' particle_state = self.get_model_state_proxy(get_particle_state_req) x = abs(particle_state.pose.position.x) + self.HALF_KOMODO y = particle_state.pose.position.y z = particle_state.pose.position.z orientation = particle_state.pose.orientation (roll, pitch, theta) = euler_from_quaternion( [orientation.x, orientation.y, orientation.z, orientation.w]) px_arr[i - 1] = x py_arr[i - 1] = y pz_arr[i - 1] = z return px_arr, pz_arr, py_arr
from gazebo_msgs.srv import GetModelState, GetModelStateRequest from snake_control.msg import snake_head_rel_pos import numpy as np rospy.init_node('snake_head_pos_pub') pos_pub = rospy.Publisher('/snake_head_pos', snake_head_rel_pos) rospy.wait_for_service('/gazebo/get_model_state') get_model_srv = rospy.ServiceProxy('/gazebo/get_model_state', GetModelState) pos = snake_head_rel_pos() model = GetModelStateRequest() model.model_name = 'robot' model.relative_entity_name = 'target::link' model2 = GetModelStateRequest() #to get coordinate of snake head model2.model_name = 'robot' model2.relative_entity_name = '' # r = rospy.Rate(1/(2*np.pi)) r = rospy.Rate(30) # create training dataset # pos_log = [] # pos_num = 400 while not rospy.is_shutdown(): result = get_model_srv(model) result2 = get_model_srv(model2)
# create the needed subscribers and service clients get_model_srv = rospy.ServiceProxy('/gazebo/get_model_state', GetModelState) state_sub = rospy.Subscriber(robot_name + '/state', std_msgs.msg.Float32, get_state) counter_sub = rospy.Subscriber(robot_name + '/counter', std_msgs.msg.Int16, get_counter) # rospy.Subscriber('fetch'+str(picker_id)+'/state',std_msgs.msg.Float32, get_picker_state, picker_id) rospy.sleep(2) #Get model position in Gazebo model = GetModelStateRequest() model.model_name = robot_name model.relative_entity_name = 'ground_plane' # Make sure sim time is working while not rospy.Time.now(): pass # Setup action client move_base = MoveBaseClient(robot_name) # main loop while counter < len(trans_list): if self_state == 0: rospy.sleep(2) goal = get_goal_2d_client(counter, robot_name) print "moving to {}".format(goal) move_base.goto(goal[0], goal[1], goal[2])