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 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 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 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 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 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 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 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)
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]
state.pose.orientation.w = quat[3]
