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])