def set_model_pose(self, pose, twist=None, model='sub8'):
'''
Set the position of 'model' to 'pose'.
It may be helpful to kill the sub before moving it.
TODO:
- Deprecate kill stuff
'''
set_state = yield self.nh.get_service_client('/gazebo/set_model_state',
SetModelState)
if twist is None:
twist = numpy_to_twist([0, 0, 0], [0, 0, 0])
model_state = ModelState()
model_state.model_name = model
model_state.pose = pose
model_state.twist = twist
if model == 'sub8':
# TODO: Deprecate kill stuff (Zach's PR upcoming)
kill = self.nh.get_service_client('/set_kill', SetKill)
yield kill(SetKillRequest(kill=Kill(id='initial', active=False)))
yield kill(SetKillRequest(kill=Kill(active=True)))
yield self.nh.sleep(.1)
yield set_state(SetModelStateRequest(model_state))
yield self.nh.sleep(.1)
yield kill(SetKillRequest(kill=Kill(active=False)))
else:
set_state(SetModelStateRequest(model_state))
def move_circle(self, center, radius):
# move on all parts of the polygon
yaw_step = math.asin(1.0 / self.node_frequency * self.vel / radius)
yaw = 0.0
while not rospy.is_shutdown():
if yaw >= 2 * math.pi:
rospy.loginfo("starting new round")
yaw = 0.0
object_new_pose = Pose()
object_new_pose.position.x = center[0] + radius * math.sin(yaw)
object_new_pose.position.y = center[1] + radius * math.cos(yaw)
quat = tf.transformations.quaternion_from_euler(0.0, 0.0, -yaw)
object_new_pose.orientation.x = quat[0]
object_new_pose.orientation.y = quat[1]
object_new_pose.orientation.z = quat[2]
object_new_pose.orientation.w = quat[3]
# spawn new model
model_state = ModelState()
model_state.model_name = self.name
model_state.pose = object_new_pose
model_state.reference_frame = 'world'
# publish message
self.pub.publish(model_state)
# call service
req = SetModelStateRequest()
req.model_state = model_state
#res = self.srv_set_model_state(req)
#if not res.success:
# print "something went wrong in service call"
yaw += yaw_step
self.rate.sleep()
def teleport(self, pose, robot_name):
model_state_req = SetModelStateRequest()
model_state_req.model_state = ModelState()
model_state_req.model_state.model_name = robot_name
model_state_req.model_state.pose = pose
model_state_req.model_state.twist.linear.x = 0.0
model_state_req.model_state.twist.linear.y = 0.0
model_state_req.model_state.twist.linear.z = 0.0
model_state_req.model_state.twist.angular.x = 0.0
model_state_req.model_state.twist.angular.y = 0.0
model_state_req.model_state.twist.angular.z = 0.0
model_state_req.model_state.reference_frame = 'world'
self.model_state_proxy(model_state_req)
def capture_sample():
get_model_state_prox = rospy.ServiceProxy('gazebo/get_model_state',
GetModelState)
model_state = get_model_state_prox('training_model', 'world')
set_model_state_prox = rospy.ServiceProxy('gazebo/set_model_state',
SetModelState)
roll = random.uniform(0, 2 * math.pi)
pitch = random.uniform(0, 2 * math.pi)
yaw = random.uniform(0, 2 * math.pi)
quaternion = tf.transformations.quaternion_from_euler(roll, pitch, yaw)
model_state.pose.orientation.x = quaternion[0]
model_state.pose.orientation.y = quaternion[1]
model_state.pose.orientation.z = quaternion[2]
model_state.pose.orientation.w = quaternion[3]
sms_req = SetModelStateRequest()
sms_req.model_state.pose = model_state.pose
sms_req.model_state.twist = model_state.twist
sms_req.model_state.model_name = 'training_model'
sms_req.model_state.reference_frame = 'world'
set_model_state_prox(sms_req)
return rospy.wait_for_message('/camera/depth_registered/points',
PointCloud2)
def initialize(self):
if self._initialized:
return
try:
for i in range(self._num_markers):
#M = tx.compose_matrix(translate = self._xyz[i], angles=self._rpy[i])
#txn = tx.translation_from_matrix(M)
#rxn = tx.quaternion_from_matrix(M)
txn = self._xyz[i]
rxn = tx.quaternion_from_euler(*(self._rpy[i]))
req = SetModelStateRequest()
req.model_state.model_name = 'a_{}'.format(i)
req.model_state.reference_frame = 'map' # TODO : wait for base_link for correctness. for now, ok.
fill_pose_msg(req.model_state.pose, txn, rxn)
res = self._gsrv(req)
if not res.success:
rospy.logerr_throttle(
1.0, 'Set Model Request Failed : {}'.format(
res.status_message))
return
except rospy.ServiceException as e:
rospy.logerr_throttle(1.0, 'Initialization Failed : {}'.format(e))
return
self._initialized = True
def rosMover():
fp = open('piano_states.txt', 'r')
print "Finished opening state file..."
rospy.wait_for_service('/gazebo/set_model_state')
print "Finished waiting for setter service..."
set_model = rospy.ServiceProxy('/gazebo/set_model_state', SetModelState)
print "Moving piano..."
for line in fp:
line.strip()
states = line.split("|")
for s in states:
elements = s.split(",")
config = [float(e) for e in elements]
tx, ty, tz, rx, ry, rz, rw = config
req = SetModelStateRequest()
req.model_state.model_name = 'piano2'
req.model_state.pose.position.x = tx
req.model_state.pose.position.y = ty
req.model_state.pose.position.z = tz
req.model_state.pose.orientation.x = rx
req.model_state.pose.orientation.y = ry
req.model_state.pose.orientation.z = rz
req.model_state.pose.orientation.w = rw
set_model(req)
time.sleep(2)
print "Finsihed moving piano"
return
def execute(self, inputs, outputs, gvm):
base = gvm.get_variable("robot")[0]
simulated = gvm.get_variable("simulation")
self.logger.info(simulated)
map = gvm.get_variable("map")
pose = map.get_pose(inputs["pose_name"])
if not pose:
self.logger.error("No pose named {}".format(inputs["pose_name"]))
return "aborted"
if simulated:
from gazebo_msgs.srv import SetModelState, SetModelStateRequest
from tf.transformations import quaternion_from_euler
set_model_state = rospy.ServiceProxy("/gazebo/set_model_state", SetModelState)
data = SetModelStateRequest()
data.model_state.model_name = "fetch"
data.model_state.pose.position.x = pose.x
data.model_state.pose.position.y = pose.y
data.model_state.pose.position.y = pose.y
quat = quaternion_from_euler(0, 0, pose.theta)
data.model_state.pose.orientation.x = quat[0]
data.model_state.pose.orientation.y = quat[1]
data.model_state.pose.orientation.z = quat[2]
data.model_state.pose.orientation.w = quat[3]
set_model_state.call(data)
# TODO(nickswalker): Send updated pose to /initialpose so AMCL is corrected
return "success"
else:
base.navigate_to(pose.x, pose.y, pose.theta)
self.logger.info("Moving to x={} y={} t={}".format(pose.x, pose.y, pose.theta))
result = base.wait_for_navigation_result()
self.logger.info(result)
# TODO(nickswalker): report failure if failed to reach goal
return "success"
def make_request(traj_point, next_traj_point, model_name, time_warp):
t1, x1, y1, z1, q01, q11, q21, q31 = next_traj_point
t0, x0, y0, z0, q00, q10, q20, q30 = traj_point
request = SetModelStateRequest()
request.model_state.model_name = model_name
request.model_state.reference_frame = "world"
request.model_state.pose.position.x = x0
request.model_state.pose.position.y = y0
request.model_state.pose.position.z = z0
request.model_state.pose.orientation.x = q00
request.model_state.pose.orientation.y = q10
request.model_state.pose.orientation.z = q20
request.model_state.pose.orientation.w = q30
dt = (t1 - t0) / time_warp
R0, P0, Y0 = tf.transformations.euler_from_quaternion([q00, q10, q20, q30])
R1, P1, Y1 = tf.transformations.euler_from_quaternion([q01, q11, q21, q31])
request.model_state.twist.linear.x = (x1 - x0) / dt
request.model_state.twist.linear.y = (y1 - y0) / dt
request.model_state.twist.linear.z = (z1 - z0) / dt
request.model_state.twist.angular.x = smallest_angle(R0, R1) / dt
request.model_state.twist.angular.x = smallest_angle(P0, P1) / dt
request.model_state.twist.angular.x = smallest_angle(Y0, Y1) / dt
return request
def capture_sample():
""" Captures a PointCloud2 using the RGBD camera
Args: None
Returns:
PointCloud2: a single point cloud from the RGBD camrea
"""
get_model_state_prox = rospy.ServiceProxy('gazebo/get_model_state',
GetModelState)
model_state = get_model_state_prox('training_model', 'world')
set_model_state_prox = rospy.ServiceProxy('gazebo/set_model_state',
SetModelState)
roll = random.uniform(0, 2 * math.pi)
pitch = random.uniform(0, 2 * math.pi)
yaw = random.uniform(0, 2 * math.pi)
quaternion = tf.transformations.quaternion_from_euler(roll, pitch, yaw)
model_state.pose.orientation.x = quaternion[0]
model_state.pose.orientation.y = quaternion[1]
model_state.pose.orientation.z = quaternion[2]
model_state.pose.orientation.w = quaternion[3]
sms_req = SetModelStateRequest()
sms_req.model_state.pose = model_state.pose
sms_req.model_state.twist = model_state.twist
sms_req.model_state.model_name = 'training_model'
sms_req.model_state.reference_frame = 'world'
set_model_state_prox(sms_req)
return rospy.wait_for_message('/komodo_perception/point_cloud',
PointCloud2)
def capture_sample():
""" Captures a PointCloud2 using the sensor stick RGBD camera
Args: None
Returns:
PointCloud2: a single point cloud from the RGBD camrea
"""
get_model_state_prox = rospy.ServiceProxy('gazebo/get_model_state',GetModelState)
model_state = get_model_state_prox('training_model','world')
set_model_state_prox = rospy.ServiceProxy('gazebo/set_model_state', SetModelState)
roll = random.uniform(0, 2*math.pi)
pitch = random.uniform(0, 2*math.pi)
yaw = random.uniform(0, 2*math.pi)
quaternion = tf.transformations.quaternion_from_euler(roll, pitch, yaw)
model_state.pose.orientation.x = quaternion[0]
model_state.pose.orientation.y = quaternion[1]
model_state.pose.orientation.z = quaternion[2]
model_state.pose.orientation.w = quaternion[3]
sms_req = SetModelStateRequest()
sms_req.model_state.pose = model_state.pose
sms_req.model_state.twist = model_state.twist
sms_req.model_state.model_name = 'training_model'
sms_req.model_state.reference_frame = 'world'
set_model_state_prox(sms_req)
d = rospy.Duration(0,100000000) # 100ms
rospy.sleep(d) # make sure there is time for the object to be rendered before we pay attn to the point_cloud
return rospy.wait_for_message('/sensor_stick/point_cloud', PointCloud2)
def capture_sample():
'''
Captures a PointCloud2 object using the sensor stick RGBD camera.
:return: PointCloud2 - a single RGBD point cloud measurement
'''
get_model_state_prox = rospy.ServiceProxy('gazebo/get_model_state',
GetModelState)
model_state = get_model_state_prox('training_model', 'world')
set_model_state_prox = rospy.ServiceProxy('gazebo/set_model_state',
SetModelState)
roll = random.uniform(0, 2 * math.pi)
pitch = random.uniform(0, 2 * math.pi)
yaw = random.uniform(0, 2 * math.pi)
quaternion = tf.transformations.quaternion_from_euler(roll, pitch, yaw)
model_state.pose.orientation.x = quaternion[0]
model_state.pose.orientation.y = quaternion[1]
model_state.pose.orientation.z = quaternion[2]
model_state.pose.orientation.w = quaternion[3]
sms_req = SetModelStateRequest()
sms_req.model_state.pose = model_state.pose
sms_req.model_state.twist = model_state.twist
sms_req.model_state.model_name = 'training_model'
sms_req.model_state.reference_frame = 'world'
set_model_state_prox(sms_req)
return rospy.wait_for_message('/pr2/world/points', PointCloud2)
def freezeBase(self, flag):
#toggle gravity
req_reset_gravity = SetPhysicsPropertiesRequest()
#ode config
req_reset_gravity.time_step = 0.001
req_reset_gravity.max_update_rate = 1000
req_reset_gravity.ode_config.sor_pgs_iters = 50
req_reset_gravity.ode_config.sor_pgs_w = 1.3
req_reset_gravity.ode_config.contact_surface_layer = 0.001
req_reset_gravity.ode_config.contact_max_correcting_vel = 100
req_reset_gravity.ode_config.erp = 0.2
req_reset_gravity.ode_config.max_contacts = 20
if (flag):
req_reset_gravity.gravity.z = 0.0
else:
req_reset_gravity.gravity.z = -9.81
self.reset_gravity(req_reset_gravity)
# create the message
req_reset_world = SetModelStateRequest()
#create model state
model_state = ModelState()
model_state.model_name = "hyq"
model_state.pose.position.x = 0.0
model_state.pose.position.y = 0.0
model_state.pose.position.z = 0.8
model_state.pose.orientation.w = 1.0
model_state.pose.orientation.x = 0.0
model_state.pose.orientation.y = 0.0
model_state.pose.orientation.z = 0.0
model_state.twist.linear.x = 0.0
model_state.twist.linear.y = 0.0
model_state.twist.linear.z = 0.0
model_state.twist.angular.x = 0.0
model_state.twist.angular.y = 0.0
model_state.twist.angular.z = 0.0
req_reset_world.model_state = model_state
# send request and get response (in this case none)
self.reset_world(req_reset_world)
def move_on_line(self, start, goal):
dx = goal[0] - start[0]
dy = goal[1] - start[1]
segment_length = math.sqrt(dx**2 + dy**2)
segment_time = segment_length / self.vel
yaw = math.atan2(dy, dx)
segment_step_count = int(segment_time * self.node_frequency)
if segment_step_count == 0:
return
segment_time = segment_length / self.vel / segment_step_count
for step in numpy.linspace(0, segment_length, segment_step_count):
step_x = start[0] + step * math.cos(yaw)
step_y = start[1] + step * math.sin(yaw)
object_new_pose = Pose()
object_new_pose.position.x = step_x
object_new_pose.position.y = step_y
quat = tf.transformations.quaternion_from_euler(0.0, 0.0, yaw)
object_new_pose.orientation.x = quat[0]
object_new_pose.orientation.y = quat[1]
object_new_pose.orientation.z = quat[2]
object_new_pose.orientation.w = quat[3]
# spawn new model
model_state = ModelState()
model_state.model_name = self.name
model_state.pose = object_new_pose
model_state.reference_frame = 'world'
# publish message
self.pub.publish(model_state)
# call service
req = SetModelStateRequest()
req.model_state = model_state
#res = self.srv_set_model_state(req)
#if not res.success:
# print "something went wrong in service call"
# sleep until next step
self.rate.sleep()
def teleportModel(self, modelName, x, y): # used as workaround to reset turtlebot model
rospy.wait_for_service('gazebo/set_model_state')
apparate = rospy.ServiceProxy('gazebo/set_model_state', SetModelState)
smsReq = SetModelStateRequest()
smsReq.model_state.model_name = modelName
smsReq.model_state.pose.position.x = x
smsReq.model_state.pose.position.y = y
if modelName != "goal":
self.actionPublisher.publish(Twist()) # stop current twist command
apparate(smsReq)
def main():
colorama.init(autoreset=True)
parser = argparse.ArgumentParser()
parser.add_argument("info", type=pathlib.Path)
args = parser.parse_args()
with args.info.open("r") as info_file:
info = json.load(info_file)
rospy.init_node("forward_tf_to_gazebo")
set_state_srv = rospy.ServiceProxy("/gazebo/set_model_state", SetModelState)
buffer = tf2_ros.Buffer()
listener = tf2_ros.TransformListener(buffer)
broadcaster = tf2_ros.TransformBroadcaster()
while not rospy.is_shutdown():
for model_name, tf_info in info.items():
gazebo_model_frame = f"{model_name}_gazebo"
translation = default_if_none(tf_info['translation'], [0, 0, 0])
yaw = tf_info['yaw']
t = geometry_msgs.msg.TransformStamped()
t.header.stamp = rospy.Time.now()
t.header.frame_id = tf_info['parent_frame']
t.child_frame_id = gazebo_model_frame
t.transform.translation.x = translation[0]
t.transform.translation.y = translation[1]
t.transform.translation.z = translation[2]
if yaw is not None:
q = transformations.quaternion_from_euler(0, 0, yaw)
else:
q = [0, 0, 0, 1]
t.transform.rotation.x = q[0]
t.transform.rotation.y = q[1]
t.transform.rotation.z = q[2]
t.transform.rotation.w = q[3]
broadcaster.sendTransform(t)
try:
transform = buffer.lookup_transform("robot_root", gazebo_model_frame, rospy.Time(), rospy.Duration(0.1))
except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException):
rospy.logwarn(f"failed to lookup transform between world and {gazebo_model_frame}")
continue
set_request = SetModelStateRequest()
set_request.model_state.model_name = model_name
set_request.model_state.pose.position = transform.transform.translation
set_request.model_state.pose.orientation = transform.transform.rotation
set_state_srv(set_request)
sleep(1.0)
def set_model_pose(self, position, orientation):
request = SetModelStateRequest()
request.model_state.model_name = self._model_name
request.model_state.pose.position = position
request.model_state.pose.orientation = orientation
request.model_state.reference_frame = '' # self._world_link makes it relative
response = self._set_model_state_service(request)
if not response.success:
rospy.logwarn(
'Problem when changing pose of model %s. Details: %s' %
(self._model_name, response.status_message))
return False
return True
def __init__(self):
self.length = 1
self.width = 1
self.height = 1
self.size = 0.1
self.radius = 0.035
self.num_particle = 0
self.z_max = 0.26
self.x_min = 0
self.sand_box_x = 0.35
self.sand_box_y = 0.301
self.sand_box_z = 0.0
self.sand_box_height = 0.25
self.center_x = self.sand_box_x / 2
self.center_z = self.sand_box_z / 2
self.HALF_KOMODO = 0.53 / 2
self.spawner = Spawner()
self.spawn_srv = rospy.ServiceProxy('/gazebo/spawn_sdf_model',
SpawnModel)
self.model_state_proxy = rospy.ServiceProxy('/gazebo/set_model_state',
SetModelState)
self.get_model_state_proxy = rospy.ServiceProxy(
'/gazebo/get_model_state', GetModelState)
# Spawn Box
box_req = self.spawner.create_box_request('sand_box', self.sand_box_x,
self.sand_box_y,
self.sand_box_z, 0.0, 0.0,
0.0)
self.spawn_srv(box_req)
self.pile_box_req = SetModelStateRequest()
self.pile_box_req.model_state = ModelState()
self.pile_box_req.model_state.model_name = 'sand_box'
self.pile_box_req.model_state.pose.position.x = self.sand_box_x
self.pile_box_req.model_state.pose.position.y = self.sand_box_y
self.pile_box_req.model_state.pose.position.z = self.sand_box_z
self.pile_box_req.model_state.pose.orientation.x = 0.0
self.pile_box_req.model_state.pose.orientation.y = 0.0
self.pile_box_req.model_state.pose.orientation.z = 0.0
self.pile_box_req.model_state.pose.orientation.w = 0.0
self.pile_box_req.model_state.twist.linear.x = 0.0
self.pile_box_req.model_state.twist.linear.y = 0.0
self.pile_box_req.model_state.twist.linear.z = 0.0
self.pile_box_req.model_state.twist.angular.x = 0.0
self.pile_box_req.model_state.twist.angular.y = 0.0
self.pile_box_req.model_state.twist.angular.z = 0.0
self.pile_box_req.model_state.reference_frame = 'world'
def make_request(traj_point, model_name):
x, y, z, q0, q1, q2, q3 = traj_point
request = SetModelStateRequest()
request.model_state.model_name = model_name
request.model_state.reference_frame = "world"
request.model_state.pose.position.x = x
request.model_state.pose.position.y = y
request.model_state.pose.position.z = z
request.model_state.pose.orientation.x = q0
request.model_state.pose.orientation.y = q1
request.model_state.pose.orientation.z = q2
request.model_state.pose.orientation.w = q3
return request
def resetRobot(self):
robot_reset_request = SetModelStateRequest()
robot_reset_request.model_state.model_name = 'turtlebot' + str(self.robot_number)
robot_reset_request.model_state.pose.position.x = self.initial_pose["x_init"]
robot_reset_request.model_state.pose.position.y = self.initial_pose["y_init"]
robot_reset_request.model_state.pose.orientation.x = self.initial_pose["x_rot_init"]
robot_reset_request.model_state.pose.orientation.y = self.initial_pose["y_rot_init"]
robot_reset_request.model_state.pose.orientation.z = self.initial_pose["z_rot_init"]
robot_reset_request.model_state.pose.orientation.w = self.initial_pose["w_rot_init"]
rospy.wait_for_service('/gazebo/set_model_state')
try:
self.reset_robot(robot_reset_request)
except rospy.ServiceException as e:
print ("/gazebo/set_model_state service call failed")
def set_position():
set_position_re = SetModelStateRequest()
pose = Pose()
pose.position.x = 0.7
pose.position.y = -0.5
pose.position.z = 1.28
pose.orientation.x = 0
pose.orientation.y = 0
pose.orientation.z = np.sin(math.pi / 2)
pose.orientation.w = np.cos(math.pi / 2)
set_position_re.model_state.model_name = 'ping_pang_ball'
set_position_re.model_state.pose = pose
try:
resp = set_state(set_position_re)
except rospy.ServiceException, e:
print "Service call failed: %s" % e
def respown(self, req):
rospy.logwarn(SetIntResponse)
pose = self.respown_point[req.data]
print pose
teleport_req = SetModelStateRequest()
teleport_req.model_state.pose.position.x = pose[0]
teleport_req.model_state.pose.position.y = pose[1]
teleport_req.model_state.pose.position.z = 0.0
q = tf.transformations.quaternion_from_euler(0.0, 0.0,
pose[2] * math.pi / 180)
teleport_req.model_state.pose.orientation.x = q[0]
teleport_req.model_state.pose.orientation.y = q[1]
teleport_req.model_state.pose.orientation.z = q[2]
teleport_req.model_state.pose.orientation.w = q[3]
tele_res = self.teleport(teleport_req)
res = SetIntResponse()
res.success = tele_res.success
res.message = tele_res.status_message
return res
def _move_continuously(self,
model_name,
dx=0.,
dy=0.,
dz=0.,
smoothness=50):
"""
Moves a gazebo object around in a smooth way in global coordinates.
:param model_name: The name of the model to move around.
:param dx, dy, dz: Displacement along the (x,y,z)-axes.
:param smoothness: How smooth the movement should be.
The higher the smoother, the lower the faster.
:returns None.
"""
# Smoothness higher than 1 might cause some slight position errors
# due to floating point errors
smoothness = max(1, smoothness)
if self.movement_rate is not None:
rate = rospy.Rate(self.movement_rate)
else:
class Rate(object):
def sleep(self):
pass
rate = Rate()
for i in range(smoothness):
model_state = self._model_state_proxy(model_name, 'world')
msg = SetModelStateRequest()
msg.model_state.model_name = model_name
msg.model_state.pose = model_state.pose
msg.model_state.pose.position.x += float(dx) / smoothness
msg.model_state.pose.position.y += float(dy) / smoothness
msg.model_state.pose.position.z += float(dz) / smoothness
msg.model_state.twist = model_state.twist
msg.model_state.scale = model_state.scale
msg.model_state.reference_frame = 'world'
self._move_proxy(msg)
rate.sleep()
def set_a_model_state(self,
model_name,
position,
numpy_orientation_quat,
linear_vel=np.array([0.0, 0, 0]),
angular_vel=np.array([0.0, 0, 0]),
reference_frame='map'):
msg = SetModelStateRequest()
vals = msg.model_state
pos, orientation = vals.pose.position, vals.pose.orientation
pos.x, pos.y, pos.z = position
orientation.x, orientation.y, orientation.z, orientation.w = numpy_orientation_quat
lin_vel, angular = vals.twist.linear, vals.twist.angular
lin_vel.x, lin_vel.y, lin_vel.z = linear_vel
angular.x, angular.y, angular.z = angular_vel
vals.model_name = model_name
vals.reference_frame = reference_frame
self.set_model_state(msg)
def set_pile(self):
count = 0
l = int(self.length / self.size)
w = int(self.width / self.size)
h = int(self.height / self.size)
self.model_state_proxy(self.pile_box_req)
eps = 0.001
for k in range(h):
#w = w - 1
l = l - 1
for j in range(-w / 2, w / 2):
for i in range(0, l):
count += 1
self.pile_state_req = SetModelStateRequest()
self.pile_state_req.model_state = ModelState()
self.pile_state_req.model_state.model_name = 'particle' + str(
count)
# self.pile_state_req.model_state.pose.position.x = i*self.size*0.25
# self.pile_state_req.model_state.pose.position.y = j*self.size*0.25
# self.pile_state_req.model_state.pose.position.z = self.radius*(1+2*k)
self.pile_state_req.model_state.pose.position.x = (
2 * i + 1) * (self.radius + eps)
self.pile_state_req.model_state.pose.position.y = (
self.radius + eps) * (1 + 2 * j)
self.pile_state_req.model_state.pose.position.z = self.radius * (
1 + 2 * k)
self.pile_state_req.model_state.pose.orientation.x = 0.0
self.pile_state_req.model_state.pose.orientation.y = 0.0
self.pile_state_req.model_state.pose.orientation.z = 0.0
self.pile_state_req.model_state.pose.orientation.w = 0.0
self.pile_state_req.model_state.twist.linear.x = 0.0
self.pile_state_req.model_state.twist.linear.y = 0.0
self.pile_state_req.model_state.twist.linear.z = 0.0
self.pile_state_req.model_state.twist.angular.x = 0.0
self.pile_state_req.model_state.twist.angular.y = 0.0
self.pile_state_req.model_state.twist.angular.z = 0.0
self.pile_state_req.model_state.reference_frame = 'world'
self.model_state_proxy(self.pile_state_req)
def set_ball_position(self, px=0.7, py=-0.5):
rospy.wait_for_service('/gazebo/set_model_state')
try:
self.px = px
self.py = py
set_state = rospy.ServiceProxy('/gazebo/set_model_state',
SetModelState)
set_position_re = SetModelStateRequest()
pose = Pose()
pose.position.x = px
pose.position.y = py
pose.position.z = 0.85
pose.orientation.x = 0
pose.orientation.y = 0
pose.orientation.z = np.sin(np.pi / 2)
pose.orientation.w = np.cos(np.pi / 2)
set_position_re.model_state.model_name = 'Ping Pong Ball'
set_position_re.model_state.pose = pose
set_state(set_position_re)
time.sleep(4)
except rospy.ServiceException, e:
print("Service call failed: %s" % e)
def __init__(self):
rospy.init_node('hydrus_position_reset')
self.respawn_pos_x = rospy.get_param('~respawn_pos_x')
self.respawn_pos_y = rospy.get_param('~respawn_pos_y')
self.tf_prefix = rospy.get_param('~tf_prefix')
self.tf_buffer = tf2_ros.Buffer()
self.tf_listener = tf2_ros.TransformListener(self.tf_buffer)
rospy.sleep(1)
client = rospy.ServiceProxy('/gazebo/set_model_state', SetModelState)
req = SetModelStateRequest()
trans = self.tf_buffer.lookup_transform(self.tf_prefix + '/root', self.tf_prefix + '/fc', rospy.Time(), rospy.Duration(10))
req.model_state.pose.position.x = self.respawn_pos_x - trans.transform.translation.x
req.model_state.pose.position.y = self.respawn_pos_y - trans.transform.translation.y
req.model_state.model_name = self.tf_prefix
result = client(req)
print(result)
rospy.loginfo("position reset ok")
rospy.signal_shutdown('hydrus position reset finished')
def _set_obj_position(self, obj_name, position):
rospy.wait_for_service('/gazebo/set_model_state')
#rospy.loginfo("set model_state for " + obj_name + " available")
sms_req = SetModelStateRequest()
sms_req.model_state.pose.position.x = position[0]
sms_req.model_state.pose.position.y = position[1]
sms_req.model_state.pose.position.z = position[2]
sms_req.model_state.pose.orientation.x = 0.
sms_req.model_state.pose.orientation.y = 0.
sms_req.model_state.pose.orientation.z = 0.
sms_req.model_state.pose.orientation.w = 1.
sms_req.model_state.twist.linear.x = 0.
sms_req.model_state.twist.linear.y = 0.
sms_req.model_state.twist.linear.z = 0.
sms_req.model_state.twist.angular.x = 0.
sms_req.model_state.twist.angular.y = 0.
sms_req.model_state.twist.angular.z = 0.
sms_req.model_state.model_name = obj_name
sms_req.model_state.reference_frame = 'world'
result = self.set_obj_state(sms_req)
return result.success
def reset_robot(reached):
global time_list, start_time, td, start_d
reset_robot = rospy.ServiceProxy('/gazebo/set_model_state', SetModelState)
robot_reset_request = SetModelStateRequest()
time_taken = rospy.get_time() - start_time
distance_travelled = td - start_d
if time_taken > 0.1:
time_list.append(
[round(time_taken, 2),
round(distance_travelled, 2),
reached]) #, odom_obj.TotalDistance])
start_time = rospy.get_time()
start_d = td
robot_reset_request.model_state.model_name = 'turtlebot' + str(
robot_number)
robot_reset_request.model_state.pose.position.x = initial_pose["x_init"]
robot_reset_request.model_state.pose.position.y = initial_pose["y_init"]
robot_reset_request.model_state.pose.orientation.x = initial_pose[
"x_rot_init"]
robot_reset_request.model_state.pose.orientation.y = initial_pose[
"y_rot_init"]
robot_reset_request.model_state.pose.orientation.z = initial_pose[
"z_rot_init"]
robot_reset_request.model_state.pose.orientation.w = initial_pose[
"w_rot_init"]
rospy.wait_for_service('/gazebo/set_model_state')
try:
reset_robot(robot_reset_request)
except rospy.ServiceException as e:
print("/gazebo/set_model_state service call failed")
start_time = rospy.get_time()
def __init__(self):
rospy.init_node('joint_position_node')
self.nb_joints = 12
self.nb_links = 13
self.state_shape = (self.nb_joints * 2 + 4 + 3 + 3,
) #joint states + orientation
self.action_shape = (self.nb_joints, )
self.link_name_lst = [
'quadruped::base_link', 'quadruped::front_right_leg1',
'quadruped::front_right_leg2', 'quadruped::front_right_leg3',
'quadruped::front_left_leg1', 'quadruped::front_left_leg2',
'quadruped::front_left_leg3', 'quadruped::back_right_leg1',
'quadruped::back_right_leg2', 'quadruped::back_right_leg3',
'quadruped::back_left_leg1', 'quadruped::back_left_leg2',
'quadruped::back_left_leg3'
]
self.leg_link_name_lst = self.link_name_lst[1:]
self.joint_name_lst = [
'front_right_leg1_joint', 'front_right_leg2_joint',
'front_right_leg3_joint', 'front_left_leg1_joint',
'front_left_leg2_joint', 'front_left_leg3_joint',
'back_right_leg1_joint', 'back_right_leg2_joint',
'back_right_leg3_joint', 'back_left_leg1_joint',
'back_left_leg2_joint', 'back_left_leg3_joint'
]
self.all_joints = AllJoints(self.joint_name_lst)
self.starting_pos = np.array([
-0.01, 0.01, 0.01, -0.01, 0.01, -0.01, -0.01, 0.01, -0.01, -0.01,
0.01, 0.01
])
self.pause_proxy = rospy.ServiceProxy('/gazebo/pause_physics', Empty)
self.unpause_proxy = rospy.ServiceProxy('/gazebo/unpause_physics',
Empty)
self.model_config_proxy = rospy.ServiceProxy(
'/gazebo/set_model_configuration', SetModelConfiguration)
self.model_config_req = SetModelConfigurationRequest()
self.model_config_req.model_name = 'quadruped'
self.model_config_req.urdf_param_name = 'robot_description'
self.model_config_req.joint_names = self.joint_name_lst
self.model_config_req.joint_positions = self.starting_pos
self.model_state_proxy = rospy.ServiceProxy('/gazebo/set_model_state',
SetModelState)
self.model_state_req = SetModelStateRequest()
self.model_state_req.model_state = ModelState()
self.model_state_req.model_state.model_name = 'quadruped'
self.model_state_req.model_state.pose.position.x = 0.0
self.model_state_req.model_state.pose.position.y = 0.0
self.model_state_req.model_state.pose.position.z = 0.25
self.model_state_req.model_state.pose.orientation.x = 0.0
self.model_state_req.model_state.pose.orientation.y = 0.0
self.model_state_req.model_state.pose.orientation.z = 0.0
self.model_state_req.model_state.pose.orientation.w = 0.0
self.model_state_req.model_state.twist.linear.x = 0.0
self.model_state_req.model_state.twist.linear.y = 0.0
self.model_state_req.model_state.twist.linear.z = 0.0
self.model_state_req.model_state.twist.angular.x = 0.0
self.model_state_req.model_state.twist.angular.y = 0.0
self.model_state_req.model_state.twist.angular.z = 0.0
self.model_state_req.model_state.reference_frame = 'world'
self.get_model_state_proxy = rospy.ServiceProxy(
'/gazebo/get_model_state', GetModelState)
self.get_model_state_req = GetModelStateRequest()
self.get_model_state_req.model_name = 'quadruped'
self.get_model_state_req.relative_entity_name = 'world'
self.last_pos = np.zeros(3)
self.last_ori = np.zeros(4)
self.joint_pos_high = np.array(
[1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0])
self.joint_pos_low = np.array([
-1.0, -1.0, -1.0, -1.0, -1.0, -1.0, -1.0, -1.0, -1.0, -1.0, -1.0,
-1.0
])
self.joint_pos_coeff = 3.0
self.joint_pos_range = self.joint_pos_high - self.joint_pos_low
self.joint_pos_mid = (self.joint_pos_high + self.joint_pos_low) / 2.0
self.joint_pos = self.starting_pos
self.joint_state = np.zeros(self.nb_joints)
self.joint_state_subscriber = rospy.Subscriber(
'/quadruped/joint_states', JointState,
self.joint_state_subscriber_callback)
self.orientation = np.zeros(4)
self.angular_vel = np.zeros(3)
self.linear_acc = np.zeros(3)
self.imu_subscriber = rospy.Subscriber('/quadruped/imu', Imu,
self.imu_subscriber_callback)
self.normed_sp = self.normalize_joint_state(self.starting_pos)
self.state = np.zeros(self.state_shape)
self.diff_state_coeff = 4.0
self.reward_coeff = 20.0
self.reward = 0.0
self.done = False
self.episode_start_time = 0.0
self.max_sim_time = 15.0
self.pos_z_limit = 0.18
self.action_coeff = 1.0
self.linear_acc_coeff = 0.1
self.last_action = np.zeros(self.nb_joints)
def __init__(self):
rospy.init_node('RL_Node')
# TODO: Pile information
self.pile = Pile() # (1.75, 2.8, 1.05, 0.34)
self.pile.length = 1.75
self.pile.width = 2.8
self.pile.height = 1.05
self.pile.size = 0.34
self.pile_flag = True
# TODO: Robot information
self.bucket_init_pos = 0
self.arm_init_pos = 0
self.vel_init = 0
self.HALF_KOMODO = 0.53 / 2
self.particle = 0
self.x_tip = 0
self.z_tip = 0
self.bucket_link_x = 0
self.bucket_link_z = 0
self.velocity = 0
self.wheel_vel = 0
self.joint_name_lst = [
'arm_joint', 'bucket_joint', 'front_left_wheel_joint',
'front_right_wheel_joint', 'rear_left_wheel_joint',
'rear_right_wheel_joint'
]
self.last_pos = np.zeros(3)
self.last_ori = np.zeros(4)
self.max_limit = np.array([0.1, 0.32, 0.9])
self.min_limit = np.array([-0.1, -0.1, -0.5])
self.orientation = np.zeros(3)
self.angular_vel = np.zeros(3)
self.linear_acc = np.zeros(3)
# TODO: RL information
self.nb_actions = 3 # base , arm , bucket
self.state_shape = (self.nb_actions * 2 + 6, )
self.action_shape = (self.nb_actions, )
self.actions = Actions()
self.starting_pos = np.array(
[self.vel_init, self.arm_init_pos, self.bucket_init_pos])
self.action_range = self.max_limit - self.min_limit
self.action_mid = (self.max_limit + self.min_limit) / 2.0
self.last_action = np.zeros(self.nb_actions)
self.joint_state = np.zeros(self.nb_actions)
self.joint_pos = self.starting_pos
self.state = np.zeros(self.state_shape)
self.reward = 0.0
self.done = False
self.reward_done = False
self.reach_target = False
self.episode_start_time = 0.0
self.max_sim_time = 8.0
# TODO: Robot information Subscribers
self.joint_state_subscriber = rospy.Subscriber(
'/joint_states', JointState, self.joint_state_subscriber_callback)
self.velocity_subscriber = rospy.Subscriber(
'/mobile_base_controller/odom', Odometry,
self.velocity_subscriber_callback)
self.imu_subscriber = rospy.Subscriber('/IMU', Imu,
self.imu_subscriber_callback)
self.reward_publisher = rospy.Publisher('/reward',
Float64,
queue_size=10)
# TODO: Torque related
# self.torque_subscriber = rospy.Subscriber('/bucket_torque_sensor',WrenchStamped,self.torque_subscriber_callback)
# self.controller_state_sub = rospy.Subscriber('/bucket_position_controller/state',JointControllerState,self.controller_subscriber_callback)
# self.torque_publisher = rospy.Publisher('/t1',Float64,queue_size=10)
# self.controller_publisher = rospy.Publisher('/t2',Float64,queue_size=10)
#
# self.torque_sum = 0
# self.torque_y = 0
# self.smooth_command = 0
# self.ft_out = WrenchStamped()
# self.ft_out.header.stamp = rospy.Time.now()
# self.ft_out.wrench.force.x = 0
# self.ft_out.wrench.force.y = 0
# self.ft_out.wrench.force.z = 0
# self.ft_out.wrench.torque.x = 0
# self.ft_out.wrench.torque.y = 0
# self.ft_out.wrench.torque.z = 0
# TODO: Gazebo stuff
self.pause_proxy = rospy.ServiceProxy('/gazebo/pause_physics', Empty)
self.unpause_proxy = rospy.ServiceProxy('/gazebo/unpause_physics',
Empty)
self.reset_world = rospy.ServiceProxy('/gazebo/reset_world', Empty)
self.tfl = TransformListener()
self.model_config_proxy = rospy.ServiceProxy(
'/gazebo/set_model_configuration', SetModelConfiguration)
self.model_config_req = SetModelConfigurationRequest()
self.model_config_req.model_name = 'komodo2'
self.model_config_req.urdf_param_name = 'robot_description'
self.model_config_req.joint_names = self.joint_name_lst
self.model_config_req.joint_positions = self.starting_pos
self.model_state_proxy = rospy.ServiceProxy('/gazebo/set_model_state',
SetModelState)
self.model_state_req = SetModelStateRequest()
self.model_state_req.model_state = ModelState()
self.model_state_req.model_state.model_name = 'komodo2'
self.model_state_req.model_state.pose.position.x = 1.0
self.model_state_req.model_state.pose.position.y = 0.0
self.model_state_req.model_state.pose.position.z = 0.0
self.model_state_req.model_state.pose.orientation.x = 0.0
self.model_state_req.model_state.pose.orientation.y = 0.0
self.model_state_req.model_state.pose.orientation.z = 0.0
self.model_state_req.model_state.pose.orientation.w = 0.0
self.model_state_req.model_state.twist.linear.x = 0.0
self.model_state_req.model_state.twist.linear.y = 0.0
self.model_state_req.model_state.twist.linear.z = 0.0
self.model_state_req.model_state.twist.angular.x = 0.0
self.model_state_req.model_state.twist.angular.y = 0.0
self.model_state_req.model_state.twist.angular.z = 0.0
self.model_state_req.model_state.reference_frame = 'world'
self.get_model_state_proxy = rospy.ServiceProxy(
'/gazebo/get_model_state', GetModelState)
self.get_model_state_req = GetModelStateRequest()
self.get_model_state_req.model_name = 'komodo2'
self.get_model_state_req.relative_entity_name = 'world'
self.get_link_state_proxy = rospy.ServiceProxy(
'/gazebo/get_link_state', GetLinkState)
self.get_link_state_req = GetLinkStateRequest()
self.get_link_state_req.link_name = 'bucket'
self.get_link_state_req.reference_frame = 'world'
