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 talker():
pub = rospy.Publisher('/gazebo/set_link_state', LinkState, queue_size=10)
rospy.init_node('dd_ctrl', anonymous=True)
rate = rospy.Rate(10) # 10hz
while not rospy.is_shutdown():
buff = LinkState()
buff.link_name = "dd_robot::left_wheel"
buff.reference_frame = "dd_robot::chassis"
buff.twist.linear.z = 10.0
buff.reference_frame = "dd_robot::left_wheel"
buff.link_name = "dd_robot::chassis"
rospy.loginfo(buff)
pub.publish(buff)
rate.sleep()
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 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 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 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 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 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 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 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 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 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 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 setmodel():
pub = rospy.Publisher('/gazebo/set_model_state', ModelState, queue_size=10)
pub1 = rospy.Publisher('/gazebo/set_link_state', LinkState, queue_size=10)
rospy.init_node('set_model_state', anonymous=True)
rate = rospy.Rate(100) # 10hz
while not rospy.is_shutdown():
modelstate = get_state_call("crumb")
#print modelstate
cmd = ModelState()
link = LinkState()
link.link_name = "wheel_left_link"
link.twist.angular.y = 2
link.reference_frame = "wheel_left_link"
link1 = LinkState()
link1.link_name = "wheel_right_link"
link1.twist.angular.y = 2
link1.reference_frame = "wheel_right_link"
pub1.publish(link)
#pub1.publish(link1)
print link
cmd.pose.position = modelstate.pose.position
cmd.twist.linear.x = 0
cmd.twist.linear.y = 0
cmd.twist.linear.z = 0
cmd.twist.angular.x = 0.0
cmd.twist.angular.y = 0.0
cmd.twist.angular.z = 0
cmd.model_name = "crumb"
#print cmd
#pub.publish(cmd)
rate.sleep()
def links_cb(self, data):
pos = None
try:
index = data.name.index(self.robot_base_name)
pos = data.pose[index]
vel = data.twist[index]
except Exception as e:
print e
if pos:
ls = LinkState()
ls.reference_frame = "/world"
ls.pose = pos
ls.twist = vel
ls.link_name = self.robot_base_name
self.pub.publish(ls)
def setPoseOfObjectToPickUp(self, pose):
# sets pose and twist of a link. All children link poses/twists of the URDF tree are not updated accordingly, but should be.
newLinkState = LinkState()
# link name, link_names are in gazebo scoped name notation, [model_name::body_name]
newLinkState.link_name = "objectToPickUp::objectToPickUp_link"
newLinkState.pose = pose # desired pose in reference frame
newTwist = Twist()
newTwist.linear.x = 0.0
newTwist.linear.y = 0.0
newTwist.linear.z = 0.0
newTwist.angular.x = 0.0
newTwist.angular.y = 0.0
newTwist.angular.z = 0.0
newLinkState.twist = newTwist # desired twist in reference frame
# set pose/twist relative to the frame of this link/body, leave empty or "world" or "map" defaults to world-frame
newLinkState.reference_frame = "world"
self.link_state_pub.publish(newLinkState)
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 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 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 bat(self, name):
def callback(link_msg):
self.links = link_msg
self.j = 5
i = 0
while self.links.name[i] != name:
i = i + 1
self.current_pose = self.links.pose[i]
self.current_twist = self.links.twist[i]
for i in range(20000):
sub = rospy.Subscriber("/gazebo/link_states", LinkStates, callback)
batting = LinkState()
batting.pose = self.current_pose
batting.twist = self.current_twist
batting.link_name = name
batting.pose.orientation.y = batting.pose.orientation.y + 0.4
print(self.current_pose)
print(batting)
self.pub_link.publish(batting)
def go_straight_ahead(des_pos):
""" Moves the ball towards the specified coordinates
Computes the distance to the goal, and moves the ball accordingly.
Limits the velocity if needed, and checks for the yaw error.
Once it arrives changes to state 1
"""
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 shoot(self):
print("Shooting")
if self.ammo > 0:
#spawn projectile
rospy.wait_for_service("gazebo/spawn_sdf_model")
spawn = rospy.ServiceProxy("gazebo/spawn_sdf_model", SpawnModel)
projectile_path = os.getcwd() + "/maps/projectile.sdf"
projectile_name = "projectile" + str(13 - self.ammo)
pose = Pose()
posePos = Point()
posePos.x = self.position[0]
posePos.y = self.position[1]
posePos.z = self.position[2]
poseAng = Quaternion()
poseAng.x = 0
poseAng.y = 0
poseAng.z = 0
poseAng.w = 1
pose.position = posePos
pose.orientation = poseAng
pose.position.z += 0.25
f = open(projectile_path)
sdf = f.read()
spawn(projectile_name, sdf, "", pose, "")
link_name = projectile_name + "::projectile_link"
twist = Twist()
velocity = 3.0
twist.linear.x = velocity * np.cos(self.zangle * np.pi / 180)
twist.linear.y = velocity * np.sin(self.zangle * np.pi / 180)
ls = LinkState()
ls.link_name = link_name
ls.pose = pose
ls.twist = twist
rospy.wait_for_service("gazebo/set_link_state")
set_state = rospy.ServiceProxy("gazebo/set_link_state",
SetLinkState)
set_state(ls)
self.ammo = self.ammo - 1
def change_pose():
rospy.init_node('set_pose')
state_msg = LinkState()
state_msg.link_name = 'box'
#Setting the required pose
state_msg.pose.position.x = -2.0
state_msg.pose.position.y = -1.0
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
rospy.wait_for_service('/gazebo/set_link_state')
try:
set_state = rospy.ServiceProxy('/gazebo/set_link_state', SetLinkState)
#Publishing the message
resp = set_state(state_msg)
except rospy.ServiceException, e:
print "Service call failed: %s" % e
# topic = "/gazebo_11355/set_link_state"
topic = "/gazebo/set_link_state"
pub = rospy.Publisher(topic, LinkState, queue_size=1)
rospy.init_node('change_obj_pose', anonymous=True)
rospy.wait_for_service('/gazebo/set_link_state')
# rospy.wait_for_service('/gazebo_11355/set_link_state')
print("service is ready")
serv = rospy.ServiceProxy('/gazebo/set_link_state', SetLinkState)
# serv = rospy.ServiceProxy('/gazebo_11355/set_link_state', SetLinkState)
state = LinkState()
#state.link_name = "cylinderHandle::left_wheel"
state.link_name = "shapetoExplore3::sphere1"
# state.pose.position.x = -0.59
# state.pose.position.y = - 0.1
# state.pose.position.z = 0.07
x = -0.59
y = -0.1
z = 0.07
x_range = 0.1
y_range = 0.1
state.pose.position.x = random.uniform(x - x_range, x + x_range)
state.pose.position.y = random.uniform(y - y_range, y + y_range)
state.pose.position.z = z
print(state.pose.position)
z_rot = random.uniform(-pi / 4, pi / 4)
print(z_rot)
serv = rospy.ServiceProxy('/gazebo/set_link_state', SetLinkState)
# serv = rospy.ServiceProxy('/gazebo_11355/set_link_state', SetLinkState)
state = LinkState()
numobj = range(3)
rotation_list = random.sample(numobj, len(numobj))
for j in range(3):
object_list = random.sample(numobj, len(numobj))
for i in range(0, 3):
for k in range(50):
print('\033[;40m ')
object_tograsp = object_list[i]
state.pose.position.x = 0.5
state.pose.position.y = 0.5
state.pose.position.z = 0.5
state.link_name = "cylinderHandle::left_wheel"
sstate = SetLinkStateRequest()
sstate.link_state = state
serv(sstate)
time.sleep(0.5)
state.pose.position.x = 0.7
state.pose.position.y = 0.7
state.pose.position.z = 0.7
state.link_name = "cross_joint_part::link"
sstate = SetLinkStateRequest()
sstate.link_state = state
serv(sstate)
time.sleep(0.5)
state.pose.position.x = 0.9
state.pose.position.y = 0.9
state.pose.position.z = 0.9
rate = rospy.Rate(1)
radius = 3.0
time_period = 12.0
t = 0
traj = OrbitTrajectory(radius, time_period)
print 'Waiting for Gazebo. Start the roslaunch?'
rospy.wait_for_service('/gazebo/set_link_state')
set_link_service = rospy.ServiceProxy('/gazebo/set_link_state',
SetLinkState)
rospy.wait_for_service('/gazebo/set_physics_properties')
pause_physics_service = rospy.ServiceProxy('/gazebo/pause_physics', Empty)
pause_physics_service()
state = LinkState()
state.pose.position.x = -3
state.link_name = 'simple_rgbd_line_cam::link1'
state.reference_frame = 'world'
state.pose.orientation.w = 1
xs = []
ys = []
while not rospy.is_shutdown():
try:
pose = traj.get_current_pose(t)
pos = pose[:3, 3]
quat = tf.transformations.quaternion_from_matrix(pose)
state.pose.position.x = pos[0]
state.pose.position.y = pos[1]
state.pose.position.z = pos[2]
def set_track_speeds(lefttrack_speed, righttrack_speed):
global old_lefttrack_speed
global old_righttrack_speed
global old_lefttrack_position
global old_righttrack_position
global old_timestamp
# Compute deltas and new track positions
new_timestamp = rospy.get_time()
delta_t = new_timestamp - old_timestamp
#print("Delta time [ms]: " + "{:.0f}".format(1000*delta_t))
lefttrack_position = old_lefttrack_position + 0.5 * (
old_lefttrack_speed + lefttrack_speed) * delta_t
righttrack_position = old_righttrack_position + 0.5 * (
old_righttrack_speed + righttrack_speed) * delta_t
#print("Left track position [m]: "+"{:.4f}".format(lefttrack_position))
# Iterate
old_timestamp = new_timestamp
old_lefttrack_position = lefttrack_position
old_righttrack_position = righttrack_position
old_lefttrack_speed = lefttrack_speed
old_righttrack_speed = righttrack_speed
# Setting the new state of the wheels
leftwheels_newlinkstate = LinkState()
rightwheels_newlinkstate = LinkState()
leftwheels_newlinkstate.pose.position = initial_wheel_state.pose.position
rightwheels_newlinkstate.pose.position = initial_wheel_state.pose.position
leftwheels_quaternion = tf_transform.quaternion_from_euler(
lefttrack_position / 0.030, 0, 0, axes='syxz')
leftwheels_newlinkstate.pose.orientation.x = leftwheels_quaternion[0]
leftwheels_newlinkstate.pose.orientation.y = leftwheels_quaternion[1]
leftwheels_newlinkstate.pose.orientation.z = leftwheels_quaternion[2]
leftwheels_newlinkstate.pose.orientation.w = leftwheels_quaternion[3]
rightwheels_quaternion = tf_transform.quaternion_from_euler(
righttrack_position / 0.030, 0, 0, axes='syxz')
rightwheels_newlinkstate.pose.orientation.x = rightwheels_quaternion[0]
rightwheels_newlinkstate.pose.orientation.y = rightwheels_quaternion[1]
rightwheels_newlinkstate.pose.orientation.z = rightwheels_quaternion[2]
rightwheels_newlinkstate.pose.orientation.w = rightwheels_quaternion[3]
leftwheels_newlinkstate.twist.angular.y = lefttrack_speed / 0.030
rightwheels_newlinkstate.twist.angular.y = righttrack_speed / 0.030
#TODO replace wheel radius with ros parameter
try:
#print("You got the service handle !")
# Repeat for each of the 8 legs
for ii in range(4):
leftwheels_newlinkstate.link_name = "link_wheel" + str(ii + 1)
leftwheels_newlinkstate.reference_frame = "link_leg" + str(ii + 1)
rightwheels_newlinkstate.link_name = "link_wheel" + str(ii + 5)
rightwheels_newlinkstate.reference_frame = "link_leg" + str(ii + 5)
rospy.wait_for_service('/gazebo/set_link_state')
set_link_state = rospy.ServiceProxy('/gazebo/set_link_state',
SetLinkState)
leftresponse = set_link_state(leftwheels_newlinkstate)
rospy.wait_for_service('/gazebo/set_link_state')
set_link_state = rospy.ServiceProxy('/gazebo/set_link_state',
SetLinkState)
rightresponse = set_link_state(rightwheels_newlinkstate)
#print("Last left wheel response: "+leftresponse.status_message)
return True
except rospy.ServiceException as e:
print("Service call failed: %s" % e)
