def setVelocity(linear=0, angular=0):
if abs(linear) > 10:
angular = copy.deepcopy(linear)
angular = angular - 10
linear = 0
#a=GetLinkState._response_class()
#a.
linkStateIni = getLinkState('base_link')
linkStateFin = LinkState()
linkStateFin = copy.deepcopy(linkStateIni.link_state)
#linkStateFin.twist=copy.deepcopy(linkStateIni.twist)
linkStateFin.link_name = 'base_link'
#print(linkState)
service = '/gazebo/set_link_state'
rospy.wait_for_service(service)
setLinkState = rospy.ServiceProxy(service, SetLinkState)
#modelState=ModelState()
#factoryModelState(vl=[1,0,0])
#linkState=LinkState()
linkStateFin.reference_frame = 'base_link'
if linear != 0:
linkStateFin.twist.linear.x = linear
else:
linkStateFin.twist.angular.z = angular
resp = setLinkState(linkStateFin)
def talker():
rospy.init_node('robot_pose_publisher', anonymous=True)
robot_pose_pub = rospy.Publisher('/gazebo/set_link_state',
LinkState,
queue_size=10)
robot_pose_msg = LinkState()
# The name of the link, as in the model-1_4.sdf file.
robot_pose_msg.link_name = 'taurob'
# The robot is placed relative to the "map" frame.
robot_pose_msg.reference_frame = 'map'
listener = tf.TransformListener()
# Updating the pose five times a second.
rate = rospy.Rate(5)
while not rospy.is_shutdown():
try:
(trans, rot) = listener.lookupTransform('/map', '/base_footprint',
rospy.Time(0))
robot_pose_msg.pose.position.x = trans[0]
robot_pose_msg.pose.position.y = trans[1]
robot_pose_msg.pose.orientation.z = rot[2]
robot_pose_msg.pose.orientation.w = rot[3]
robot_pose_pub.publish(robot_pose_msg)
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
continue
rate.sleep()
def reset(self):
self.pause()
roll, pitch, yaw = np.random.random((3, )) * np.pi
state = LinkState()
quaternion = tf.transformations.quaternion_from_euler(0., 0., yaw)
x, y = np.random.random((2, ))
x *= 0.2
x += 1.1
y *= -0.5
y += -2
state.pose.position.x = x
state.pose.position.y = y
state.pose.position.z = 0.5
state.pose.orientation.x = quaternion[0]
state.pose.orientation.y = quaternion[1]
state.pose.orientation.z = quaternion[2]
state.pose.orientation.w = quaternion[3]
state.link_name = "base_link"
#model_state = ModelState()
#model_state.model_name = "mobile_base"
#self.set_model_state(model_state=model_state)
#self.set_link_state(link_state=state)
self.index += 1
if self.index > 10:
self.index = 0
self.unpause()
def generate_poses(p=0.3):
valid_poses = False
while not valid_poses:
x_buff = np.around(np.random.uniform(-0.5, 0.5, (1, 3)), decimals=3)
y_buff = np.around(np.random.uniform(-0.15, 0.25, (1, 3)), decimals=3)
xy_buff = np.vstack((x_buff, y_buff))
d1 = np.linalg.norm(xy_buff[:, 0] - xy_buff[:, 1])
d2 = np.linalg.norm(xy_buff[:, 0] - xy_buff[:, 2])
d3 = np.linalg.norm(xy_buff[:, 1] - xy_buff[:, 2])
if d1 > 1.5 * CUBE_LENGTH and d2 > 1.5 * CUBE_LENGTH and d3 > 1.5 * CUBE_LENGTH:
valid_poses = True
print(xy_buff)
target_poses = []
for i in range(0, 3):
target = LinkState()
target.link_name = 'cube' + str(i + 1) + '_link'
target.reference_frame = "table_surface_link"
x = xy_buff[0, i]
y = xy_buff[1, i]
prob_token = random.uniform(0, 1)
if prob_token < p:
z = -0.2
else:
z = CUBE_LENGTH / 4 + 0.0127
yaw = round(random.uniform(pi / 2, 3 * pi / 2), 5)
quaternion = tf.transformations.quaternion_from_euler(pi, 0, yaw)
target.pose.position.x = x
target.pose.position.y = y
target.pose.position.z = z
target.pose.orientation.x = quaternion[0]
target.pose.orientation.y = quaternion[1]
target.pose.orientation.z = quaternion[2]
target.pose.orientation.w = quaternion[3]
target_poses.append(target)
return target_poses
def generate_poses(n_cubes):
valid_poses = False
while not valid_poses:
x_buff = np.around(np.random.uniform(-0.45, 0.45, (1, 3)), decimals=3)
y_buff = np.around(np.random.uniform(-0.15, 0.23, (1, 3)), decimals=3)
xy_buff = np.vstack((x_buff, y_buff))
d1 = np.linalg.norm(xy_buff[:, 0] - xy_buff[:, 1])
d2 = np.linalg.norm(xy_buff[:, 0] - xy_buff[:, 2])
d3 = np.linalg.norm(xy_buff[:, 1] - xy_buff[:, 2])
if d1 > 1.5 * CUBE_LENGTH and d2 > 1.5 * CUBE_LENGTH and \
d3 > 1.5 * CUBE_LENGTH:
valid_poses = True
new_row = []
for i in range(0, 3):
target = LinkState()
target.link_name = 'cube' + str(i + 1) + '_link'
target.reference_frame = "table_surface_link"
x = xy_buff[0, i]
y = xy_buff[1, i]
if i < n_cubes:
z = CUBE_LENGTH / 4 + 0.0127
else:
z = -0.2
yaw = round(random.uniform((pi * 3) / 5, (8 * pi / 5)), 5)
quaternion = tf.transformations.quaternion_from_euler(pi, 0, yaw)
new_row = new_row + [x, y, z] + list(quaternion)
return new_row
def drake_state_publisher(x_cmd):
rate = rospy.Rate(100)
times=0
xmsg=LinkState()
while not rospy.is_shutdown():
xmsg.link_name='cartpole::link_chassis'
# hello_str = "hello world %s" % rospy.get_time()
buff=Pose
position_buff=Point
orientation_buff=Quaternion
position_buff.x=x_cmd[times]
position_buff.y=0
position_buff.z=0
orientation_buff.x=0
orientation_buff.y=0
orientation_buff.z=0
orientation_buff.w=1
buff.position=position_buff
buff.orientation=orientation_buff
#xmsg.pose = buffpose.position.x
#xmsg.twist = [1,0,0,0,0,0]
xmsg.pose.position.x = x_cmd[times]
xmsg.pose.position.z = 2
drake_state_pub.publish(xmsg)
rospy.loginfo(x_cmd[times])
drake_state_pub_echo.publish(x_cmd[times])
times=times+1
#'current x state is :{}'.format(statex)
rate.sleep()
def go_straight_ahead(des_pos):
global pub, state_, z_back
err_pos = math.sqrt(
pow(des_pos.y - position_.y, 2) + pow(des_pos.x - position_.x, 2))
if des_pos.z != z_back:
link_state_msg = LinkState()
link_state_msg.link_name = "ball_link"
link_state_msg.pose.position.x = position_.x
link_state_msg.pose.position.y = position_.y
link_state_msg.pose.position.z = des_pos.z
z_back = des_pos.z
pubz.publish(link_state_msg)
if err_pos > dist_precision_:
twist_msg = Twist()
twist_msg.linear.x = kp_d * (des_pos.x - position_.x)
if twist_msg.linear.x > ub_d:
twist_msg.linear.x = ub_d
elif twist_msg.linear.x < -ub_d:
twist_msg.linear.x = -ub_d
twist_msg.linear.y = kp_d * (des_pos.y - position_.y)
if twist_msg.linear.y > ub_d:
twist_msg.linear.y = ub_d
elif twist_msg.linear.y < -ub_d:
twist_msg.linear.y = -ub_d
pub.publish(twist_msg)
else:
print('Position error: [%s]' % err_pos)
change_state(1)
def object_picking(self): global PICKING if PICKING: angle = quaternion_from_euler(1.57, 0.0, 0.0) object_picking = LinkState() object_picking.link_name = self.string object_picking.pose = Pose(self.model_pose_picking.position, self.model_pose_picking.orientation) object_picking.reference_frame = "wrist_3_link" self.pub_model_position.publish(object_picking)
def update_link(self):
""" not working currently """
msg = LinkState()
msg.link_name = "cart_front_steer::wheel_front_left_spin"
msg.reference_frame = "world"
# msg.twist.linear.x = 0.2
msg.twist.angular.z = 0.05
self.link_pub.publish(msg)
msg.link_name = "cart_front_steer::wheel_front_right_spin"
self.link_pub.publish(msg)
def get_ball_home_link_state():
state_msg = LinkState()
state_msg.link_name = 'ball_link'
state_msg.reference_frame = 'wrist_3_link'
state_msg.pose.position.x = 0
state_msg.pose.position.y = 0.2
state_msg.pose.position.z = 0
state_msg.pose.orientation.x = 0
state_msg.pose.orientation.y = 0
state_msg.pose.orientation.z = 0
state_msg.pose.orientation.w = 0
return state_msg
def change_link_state(t, quat):
try:
f = rospy.ServiceProxy('/gazebo/set_link_state', SetLinkState)
state = LinkState()
state.link_name = 'chessboard::chessboard'
state.pose.position = Point(t[0], t[1], t[2])
state.pose.orientation = Quaternion(quat[0], quat[1], quat[2], quat[3])
status = f(state)
print(status)
except rospy.ServiceException as e:
print('Service call failed: %s' % e)
def sendObsPose():
global y1, y2, y3
p_0 = Pose(Point(8.0, y1, 0.0), Quaternion(0.0, 0.0, 0.0, 0.0))
t_0 = Twist(Vector3(0.0, 0.0, 0.0), Vector3(0.0, 0.0, 0.0))
obj_0 = LinkState('link_0', p_0, t_0, 'world')
setLinkState(obj_0)
p_1 = Pose(Point(8.0, y2, 0.0), Quaternion(0.0, 0.0, 0.0, 0.0))
t_1 = Twist(Vector3(0.0, 0.0, 0.0), Vector3(0.0, 0.0, 0.0))
obj_1 = LinkState('link_1', p_1, t_1, 'world')
setLinkState(obj_1)
p_2 = Pose(Point(8.0, y3, 0.0), Quaternion(0.0, 0.0, 0.0, 0.0))
t_2 = Twist(Vector3(0.0, 0.0, 0.0), Vector3(0.0, 0.0, 0.0))
obj_2 = LinkState('link_2', p_2, t_2, 'world')
setLinkState(obj_2)
def linkStatesCb(self, msg):
'''
Callback for gazebo link states
@param msg: received message
@type msg: gazebo_msgs/LinkStates
'''
for i in range(len(msg.name)):
model_id = msg.name[i].split('::')[0]
# Adds dynamically all the links for all the models
if model_id in self._gazebo_robots:
link_state = LinkState()
link_state.link_name = msg.name[i]
link_state.pose = msg.pose[i]
link_state.twist = msg.twist[i]
# already exist?
if link_state.link_name in self._gazebo_robots[model_id][
'links']:
self._gazebo_robots[model_id]['links'][
link_state.link_name].update(link_state,
rospy.Time.now())
else:
rospy.loginfo('%s::linkStatesCb: adding new link %s',
self.node_name, link_state.link_name)
self._gazebo_robots[model_id]['links'][
link_state.link_name] = GazeboLinkState()
self._gazebo_robots[model_id]['links'][
link_state.link_name].update(link_state,
rospy.Time.now())
elif model_id in self._gazebo_objects:
link_state = LinkState()
link_state.link_name = msg.name[i]
link_state.pose = msg.pose[i]
link_state.twist = msg.twist[i]
# already exist?
if link_state.link_name in self._gazebo_objects[model_id][
'links']:
self._gazebo_objects[model_id]['links'][
link_state.link_name].update(link_state,
rospy.Time.now())
else:
rospy.loginfo('%s::linkStatesCb: adding new link %s',
self.node_name, link_state.link_name)
self._gazebo_objects[model_id]['links'][
link_state.link_name] = GazeboLinkState()
self._gazebo_objects[model_id]['links'][
link_state.link_name].update(link_state,
rospy.Time.now())
def main():
global last_attach_link, force_on
new_link_state = LinkState()
force = Wrench()
pose = Point()
new_link_state.reference_frame = 'palletizer_robot::prisos_point'
#rate = rospy.Rate(100)
dists = []
centre_box = []
while not rospy.is_shutdown():
if (not vacuum_on):
dists, centre_box = find_dists(pose_cubs_list, pos_orient_prisos)
if (force_on):
force.force.z = 0
force_srv(last_attach_link, 'world', pose, force,
rospy.Time.from_sec(0),
rospy.Duration.from_sec(-1.0))
force_on = False
else:
id_grasp_cub = find_aviable_cub(centre_box)
if (not id_grasp_cub == -1):
new_link_state.link_name = link_name[id_cubs[id_grasp_cub]]
force.force.z = 0.01
last_attach_link = link_name[id_cubs[id_grasp_cub]]
dist = dists[id_grasp_cub]
new_link_state.pose.orientation.x = dist[3]
new_link_state.pose.orientation.y = dist[4]
new_link_state.pose.orientation.z = dist[5]
new_link_state.pose.orientation.w = dist[6]
new_link_state.pose.position.x = dist[0]
new_link_state.pose.position.y = dist[1]
new_link_state.pose.position.z = dist[2] + 0.003
new_link_state.twist = twist
pub.publish(new_link_state)
if (not force_on):
force_srv(last_attach_link, 'world', pose, force,
rospy.Time.from_sec(0),
rospy.Duration.from_sec(-1.0))
force_on = True
else:
dists, centre_box = find_dists(pose_cubs_list,
pos_orient_prisos)
if (force_on):
force_srv(last_attach_link, 'world', pose, force,
rospy.Time.from_sec(0),
rospy.Duration.from_sec(-1.0))
force_on = False
#rate.sleep()
time.sleep(0.04)
def __init__(self):
rospy.init_node('Cartpole')
self._sub_invpend_states = rospy.Subscriber('/invpend/joint_states',
JointState,
self.jstates_callback)
self._pub_vel_cmd = rospy.Publisher(
'/invpend/joint1_velocity_controller/command',
Float64,
queue_size=50)
self._pub_pole_position_cmd = rospy.Publisher(
'/invpend/joint2_position_controller/command',
Float64,
queue_size=50)
self._pub_set_pole = rospy.Publisher('/gazebo/set_link_state',
LinkState,
queue_size=50)
self._pub_set_cart = rospy.Publisher('/gazebo/set_link_state',
LinkState,
queue_size=50)
# init observation parameters
self.pos_cart = 0
self.vel_cart = 0
self.pos_pole = 0
self.vel_pole = 0
self.reward = 0
self.out_range = False
self.reset_stamp = time.time()
self.time_elapse = 0.
# init reset_env parameters
self.reset_dur = .2 # reset duration, sec
self.freq = 1000 # topics pub and sub frequency, Hz
## pole
self.PoleState = LinkState()
self.PoleState.link_name = 'pole'
self.PoleState.pose.position = Point(
0.0, -0.275, 0.0) # pole's position w.r.t. world
self.PoleState.reference_frame = 'cart'
self.pole_length = 0.5
## cart
self.CartState = LinkState()
self.CartState.link_name = 'cart'
self.CartState.pose.position = Point(
0.0, 0.0, 0.0) # pole's position w.r.t. world
self.CartState.reference_frame = 'slidebar'
# velocity control command
self.cmd = 0
self.positionpid = pid_controller(10**-3, 5, -5, 2.1, 0.0, 0.4, 15)
self.velocitypid = pid_controller(10**-3, 5, -5, 1.5, 0.0, 0.0, 15)
def __init__(self):
self.state = np.zeros(20)
controllers = "shoulder_pan_controller shoulder_lift_controller elbow_controller wrist1_controller wrist2_controller wrist3_controller \
j11_controller j12_controller j13_controller j21_controller j22_controller j23_controller j32_controller j33_controller"
self.controllers = controllers.split(" ")
self._pub_cmds = {}
for controller in self.controllers:
self._pub_cmds[controller] = rospy.Publisher('/ur5bh/%s/command' % controller, Float64, queue_size=50)
self.reward = 0
self.reset_stamp = time.time()
self.time_elapse = 0.
self.reset_dur = .2 # reset duration, sec
self._pub_set_obj = rospy.Publisher('/gazebo/set_link_state', LinkState, queue_size=50)
self.freq = 50
self.ObjState = LinkState()
self.ObjState.link_name = 'cube'
self.ObjState.pose.position = Point(0.78, 0.0, 0)
self.ObjState.pose.orientation = Quaternion(0, 0, 0, 1)
self.ObjState.reference_frame = "world"
self._sub_states = rospy.Subscriber('/ur5bh/joint_states', JointState, self.jstates_callback)
self._sub_model_states = rospy.Subscriber('/gazebo/model_states', ModelStates, self.mstates_callback)
def __init__(self, joint_values=None):
rospy.init_node('publish_objects_path')
##################
# Gazebo Related #
##################
# Publisher
self.pub_model_position = rospy.Publisher('/gazebo/set_link_state',
LinkState,
queue_size=1)
# Service
self.get_model_coordinates = rospy.ServiceProxy(
'/gazebo/get_link_state', GetLinkState)
# Subscriers
rospy.Subscriber('grasp_started',
Bool,
self.grasping_flag_callback,
queue_size=1)
rospy.Subscriber('grasp_object_name',
String,
self.grasping_object_name_callback,
queue_size=1)
rospy.Subscriber('grasp_object_position',
Pose,
self.object_pose_callback,
queue_size=1)
# Initialize some variables
self.grasping_flag = False
self.object_name = ''
self.object_picking_obj = LinkState()
def randomizeTargetPose(self, obj_name):
EE_POS_TGT = np.asmatrix([[round(np.random.uniform(-0.62713, -0.29082), 5), round(np.random.uniform(-0.15654, 0.15925), 5), 0.72466]])
roll = 0.0
pitch = 0.0
yaw = np.random.uniform(-1.57, 1.57)
q = quat.from_euler_angles(roll, pitch, yaw)
EE_ROT_TGT = rot_matrix = quat.as_rotation_matrix(q)
self.target_orientation = EE_ROT_TGT
EE_POINTS = np.asmatrix([[0, 0, 0]])
ee_tgt = np.ndarray.flatten(get_ee_points(EE_POINTS, EE_POS_TGT, EE_ROT_TGT).T)
self.realgoal = ee_tgt
ms = ModelState()
ms.pose.position.x = EE_POS_TGT[0,0]
ms.pose.position.y = EE_POS_TGT[0,1]
ms.pose.position.z = EE_POS_TGT[0,2]
ms.pose.orientation.x = q.x
ms.pose.orientation.y = q.y
ms.pose.orientation.z = q.z
ms.pose.orientation.w = q.w
self._pub_link_state.publish( LinkState(link_name="target_link", pose=ms.pose, reference_frame="world") )
ms.model_name = obj_name
rospy.wait_for_service('gazebo/set_model_state')
try:
self.set_model_pose(ms)
except (rospy.ServiceException) as e:
print("Error setting the pose of " + obj_name)
def __init__(self):
self._sub_invpend_states = rospy.Subscriber('/invpend/joint_states',
JointState, self.jsCB)
self._pub_vel_cmd = rospy.Publisher(
'/invpend/joint1_velocity_controller/command',
Float64,
queue_size=1)
self._pub_pos_cmd_joint_2 = rospy.Publisher(
'/invpend/joint2_position_controller/command',
Float64,
queue_size=1)
# self._joint_2_state = rospy.Subscriber('/invpend/joint2_positon_controler/state',Joint2State,
self._pub_set_pole = rospy.Publisher(
'/gazebo/set_link_state', LinkState) # publish to link states
self.unpause = rospy.ServiceProxy('/gazebo/unpause_physics', Empty)
self.pause = rospy.ServiceProxy('/gazebo/pause_physics', Empty)
self.reset_sim = rospy.ServiceProxy('/gazebo/reset_simulation', Empty)
# where the the robot starts
self.pos_cart = 0.001
self.vel_cart = 0.001
self.pos_pole = 0.0
self.vel_pole = 0
self.PoleState = LinkState()
self.PoleState.link_name = 'pole'
self.PoleState.pose.position = Point(0.0, -0.25, 2.0)
self.PoleState.reference_frame = 'world'
def _reset(self):
# # Resets the state of the environment and returns an initial observation.
# rospy.wait_for_service('/gazebo/reset_simulation')
# try:
# self.reset_proxy()
# except (rospy.ServiceException) as e:
# print ("/gazebo/reset_simulation service call failed")
# # Unpause simulation to make observation
rospy.wait_for_service('/gazebo/pause_physics')
try:
#resp_pause = pause.call()
self.pause()
except (rospy.ServiceException) as e:
print("/gazebo/unpause_physics service call failed")
rospy.wait_for_service('/gazebo/set_link_state')
self.set_link(LinkState(link_name='pole'))
self.set_link(LinkState(link_name='cart'))
rospy.wait_for_service('/gazebo/unpause_physics')
try:
#resp_pause = pause.call()
self.unpause()
except (rospy.ServiceException) as e:
print("/gazebo/pause_physics service call failed")
#read laser data
self.data = None
while self.data is None:
try:
self.data = rospy.wait_for_message('/cart_pole/joint_states',
JointState,
timeout=5)
except:
pass
angle = math.atan(math.tan(self.data.position[0]))
state = [
self.data.position[1], self.data.velocity[1], angle,
self.data.velocity[0]
]
# state = [self.data.position[1], 0, self.data.position[0], 0]
self.steps_beyond_done = None
self.current_vel = 0
return state
def __init__(self):
self._state = LinkState()
# Saves the properties of the link directly from the response
self._properties = GetLinkPropertiesResponse()
self._is_received = False
self._last_time_received = rospy.Time(0)
self._timeout_msg = 5.0 # seconds without receiving
def randomizeTargetPose(self, obj_name, centerPoint=False):
ms = ModelState()
if not centerPoint:
EE_POS_TGT = np.asmatrix([
round(np.random.uniform(-0.62713, -0.29082), 5),
round(np.random.uniform(-0.15654, 0.15925), 5),
self.realgoal[2]
])
roll = 0.0
pitch = 0.0
yaw = np.random.uniform(-1.57, 1.57)
#tf.taitbryan.euler2quat(z, y, x) --> [w, x, y, z]
q = tf.taitbryan.euler2quat(yaw, pitch, roll)
EE_ROT_TGT = tf.quaternions.quat2mat(q)
self.target_orientation = EE_ROT_TGT
ee_tgt = np.ndarray.flatten(
get_ee_points(self.environment['end_effector_points'],
EE_POS_TGT, EE_ROT_TGT).T)
ms.pose.position.x = EE_POS_TGT[0, 0]
ms.pose.position.y = EE_POS_TGT[0, 1]
ms.pose.position.z = EE_POS_TGT[0, 2]
ms.pose.orientation.x = q[1]
ms.pose.orientation.y = q[2]
ms.pose.orientation.z = q[3]
ms.pose.orientation.w = q[0]
if obj_name != "target":
ms.model_name = obj_name
rospy.wait_for_service('gazebo/set_model_state')
try:
self.set_model_pose(ms)
except (rospy.ServiceException) as e:
print("Error setting the pose of " + obj_name)
self.spawnModel(obj_name, self.obj_path, ms.pose)
else:
EE_POS_TGT = np.asmatrix(
[self.realgoal[0], self.realgoal[1], centerPoint])
ee_tgt = np.ndarray.flatten(
get_ee_points(self.environment['end_effector_points'],
EE_POS_TGT, self.target_orientation).T)
ms.pose.position.x = EE_POS_TGT[0, 0]
ms.pose.position.y = EE_POS_TGT[0, 1]
ms.pose.position.z = EE_POS_TGT[0, 2]
ms.pose.orientation.x = 0
ms.pose.orientation.y = 0
ms.pose.orientation.z = 0
ms.pose.orientation.w = 0
self._pub_link_state.publish(
LinkState(link_name="target_link",
pose=ms.pose,
reference_frame="world"))
self.realgoal = ee_tgt
def read_cube_pose_file(self, sample_idx):
cube_poses = self.cube_pose_file.iloc[sample_idx, :].values
# print(cube_poses)
cube_poses = cube_poses.reshape((3, -1))
target_poses = []
for i in range(0, 3):
target = LinkState()
target.link_name = 'cube' + str(i + 1) + '_link'
target.reference_frame = "table_surface_link"
target.pose.position.x = cube_poses[i, 0]
target.pose.position.y = cube_poses[i, 1]
target.pose.position.z = cube_poses[i, 2]
target.pose.orientation.x = cube_poses[i, 3]
target.pose.orientation.y = cube_poses[i, 4]
target.pose.orientation.z = cube_poses[i, 5]
target.pose.orientation.w = cube_poses[i, 6]
target_poses.append(target)
return target_poses
def __init__(self):
"""
:param cubes_name: a list of string type of all cubes
"""
self.cubes_pose = LinkState()
self.cubes_state = dict()
# pos publisher
self.pose_pub = rospy.Publisher("/gazebo/set_link_state", LinkState, queue_size=1)
self.pose_sub = rospy.Subscriber("/gazebo/cubes", LinkStates, callback=self.callback_state, queue_size=1)
def reset_client(self, angle):
q = tf.transformations.quaternion_from_euler(0.0, angle, 0.0)
request = LinkState()
request.link_name = "myrrbot7::myrrbot7_link2"
request.pose.position.x = 0.0 + 2.0
request.pose.position.y = 0.1 - 2.0
request.pose.position.z = 1.45
request.pose.orientation.x = q[0]
request.pose.orientation.y = q[1]
request.pose.orientation.z = q[2]
request.pose.orientation.w = q[3]
# print "request.pose : "
# print request.pose
try:
reset = rospy.ServiceProxy('/gazebo/set_link_state', SetLinkState)
resp = reset(request)
# print "resp : ", resp
except rospy.ServiceException, e:
print "Service call faild : %s" % e
def move_cube_to_target(self, link_name, target_pose):
rospy.wait_for_service('/gazebo/set_link_state')
target = LinkState()
target.link_name = link_name
target.reference_frame = "link"
target.pose.position.x = target_pose[0]
target.pose.position.y = target_pose[1]
target.pose.position.z = target_pose[2]
target.pose.orientation.x = target_pose[3]
target.pose.orientation.y = target_pose[4]
target.pose.orientation.z = target_pose[5]
target.pose.orientation.w = target_pose[6]
try:
set_cube_position = rospy.ServiceProxy('/gazebo/set_link_state',
SetLinkState)
move_status = set_cube_position(target)
return move_status.success
except rospy.ServiceException, e:
print "Service call failed: %s"
def go_straight_ahead(des_pos):
global pub, state_, z_back, message
if des_pos.z >= 0: #in case the ball appears in the arena
message = "start play"
## publisher that publishes a message whenever is given the command for which the ball is in the arena
pub_start_play = rospy.Publisher("/ball/chatter",
String,
queue_size=10)
pub_start_play.publish(message)
err_pos = math.sqrt(
pow(des_pos.y - position_.y, 2) + pow(des_pos.x - position_.x, 2))
if des_pos.z != z_back:
link_state_msg = LinkState()
link_state_msg.link_name = "ball_link"
link_state_msg.pose.position.x = position_.x
link_state_msg.pose.position.y = position_.y
link_state_msg.pose.position.z = des_pos.z
z_back = des_pos.z
pubz.publish(link_state_msg)
if err_pos > dist_precision_:
twist_msg = Twist()
twist_msg.linear.x = kp_d * (des_pos.x - position_.x)
if twist_msg.linear.x > ub_d:
twist_msg.linear.x = ub_d
elif twist_msg.linear.x < -ub_d:
twist_msg.linear.x = -ub_d
twist_msg.linear.y = kp_d * (des_pos.y - position_.y)
if twist_msg.linear.y > ub_d:
twist_msg.linear.y = ub_d
elif twist_msg.linear.y < -ub_d:
twist_msg.linear.y = -ub_d
pub.publish(twist_msg)
else:
print('Position error: [%s]' % err_pos)
change_state(1)
def randomizeTargetPose(self, obj_name, centerPoint=None):
ms = ModelState()
if centerPoint is None:
EE_POS_TGT = np.asmatrix([
round(np.random.uniform(-0.62713, -0.29082), 5),
round(np.random.uniform(-0.15654, 0.15925), 5),
self.realgoal[2]
])
roll = 0.0
pitch = 0.0
yaw = np.random.uniform(-1.57, 1.57)
q = quat.from_euler_angles(roll, pitch, yaw)
EE_ROT_TGT = rot_matrix = quat.as_rotation_matrix(q)
self.target_orientation = EE_ROT_TGT
ee_tgt = np.ndarray.flatten(
get_ee_points(self.environment['end_effector_points'],
EE_POS_TGT, EE_ROT_TGT).T)
ms.pose.position.x = EE_POS_TGT[0, 0]
ms.pose.position.y = EE_POS_TGT[0, 1]
ms.pose.position.z = EE_POS_TGT[0, 2]
ms.pose.orientation.x = q.x
ms.pose.orientation.y = q.y
ms.pose.orientation.z = q.z
ms.pose.orientation.w = q.w
ms.model_name = obj_name
rospy.wait_for_service('gazebo/set_model_state')
try:
self.set_model_pose(ms)
except (rospy.ServiceException) as e:
print("Error setting the pose of " + obj_name)
else:
EE_POS_TGT = np.asmatrix(
[self.realgoal[0], self.realgoal[1], centerPoint])
ee_tgt = np.ndarray.flatten(
get_ee_points(self.environment['end_effector_points'],
EE_POS_TGT, self.target_orientation).T)
ms.pose.position.x = EE_POS_TGT[0, 0]
ms.pose.position.y = EE_POS_TGT[0, 1]
ms.pose.position.z = EE_POS_TGT[0, 2]
ms.pose.orientation.x = 0
ms.pose.orientation.y = 0
ms.pose.orientation.z = 0
ms.pose.orientation.w = 0
self._pub_link_state.publish(
LinkState(link_name="target_link",
pose=ms.pose,
reference_frame="world"))
self.realgoal = ee_tgt
def talker():
pub = rospy.Publisher('/gazebo/set_link_state', LinkState, queue_size=10)
ppp = LinkState()
rospy.init_node('talker', anonymous=True)
rate = rospy.Rate(100) # 10hz
i = 1
while not rospy.is_shutdown():
ppp.link_name = "platform"
ppp.pose.position.x = 0.1
ppp.pose.position.y = 0.1
ppp.pose.position.z = 1
ppp.pose.orientation.x = 0
ppp.pose.orientation.y = 0
ppp.pose.orientation.z = 0
ppp.pose.orientation.w = 0
i = i + 1
rospy.loginfo(ppp)
pub.publish(ppp)
rate.sleep()
def reset(self):
# Reset world
rospy.wait_for_service('/gazebo/set_link_state')
self.set_link(LinkState(link_name='pole'))
self.set_link(LinkState(link_name='cart'))
# Unpause simulation to make observation
rospy.wait_for_service('/gazebo/unpause_physics')
try:
self.unpause()
except (rospy.ServiceException) as e:
print ("/gazebo/unpause_physics service call failed")
# Wait for data
data = self.data
while data is None:
data = self.data
# Pause simulation
rospy.wait_for_service('/gazebo/pause_physics')
try:
self.pause()
except (rospy.ServiceException) as e:
print ("/gazebo/pause_physics service call failed")
# Process state
x = self.data.position[1]
x_dot = self.data.velocity[1]
theta = math.atan(math.tan(self.data.position[0]))
theta_dot = self.data.velocity[0]
state = [round(x, 2), round(x_dot, 1), round(theta, 2), round(theta_dot, 0)]
# Limit state space
state[0] = 0
state[1] = 0
self.current_vel = 0
# Reset data
self.data = None
return state
