#! /usr/bin/env python
import math, rospy
from utilities import set_model_state
from geometry_msgs.msg import Pose, Point, Quaternion
from tf.transformations import quaternion_about_axis, quaternion_multiply
position = Point(1, 0, 0)
q = quaternion_about_axis(math.radians(90), (0, 0, 1))
orientation = Quaternion(*q)
set_model_state('table', Pose(position, orientation))
import math, rospy
from utilities import set_model_state, get_model_state, \
pause_physics, unpause_physics
from geometry_msgs.msg import Pose, Point, Quaternion
from tf.transformations import quaternion_about_axis
from tf.transformations import quaternion_multiply
#for postion the table
position = Point(1,0,0)
q = quaternion_about_axis(math.radians(90), (0,0,1))
orientation = Quaternion(*q)
set_model_state('table', Pose(position, orientation))
rospy.sleep(0.1)
#for position the coke cans
pause_physics()
x, y, z = 0.4, 0, 1.05
for angle in range(0,360,30):
i = angle/30
print(i)
theta = math.radians(angle)
q = quaternion_about_axis(math.radians(90), (0,1,0))
# orientation = Quaternion(*q)
q_z = quaternion_about_axis(theta, (0,0,1))
q_yz = quaternion_multiply(q_z, q)
orientation = Quaternion(*q_yz)
xp = 0.4 * math.cos(theta)+1 #equation of the circle
yp = 0.4 * math.sin(theta)
position = Point(xp, yp, z)
set_model_state('coke_can_'+str(i), Pose(position, orientation))
rospy.sleep(0.1)
import math, rospy
from utilities import set_model_state, get_model_state, \
pause_physics, unpause_physics
from geometry_msgs.msg import Pose, Point, Quaternion
from tf.transformations import quaternion_about_axis
pause_physics()
position = Point(x=0.5, y=0, z=0.5)
for angle in range(0,360,10):
theta = math.radians(angle)
q = quaternion_about_axis(theta, (0,0,1))
orientation = Quaternion(*q)
print("orinetation", orientation)
pose = Pose(position, orientation)
print("pose", pose)
set_model_state('coke_can', Pose(position, orientation))
rospy.sleep(0.1)
import math, rospy
from utilities import set_model_state, get_model_state
from geometry_msgs.msg import Pose, Point, Quaternion
x, y, z = 1, 0, 0.22
for angle in range(0, 360, 10):
theta = math.radians(angle)
xp = x * math.cos(theta) - y * math.sin(theta)
yp = x * math.sin(theta) + y * math.cos(theta)
set_model_state('coke_can', Pose(position=Point(xp, yp, z)))
rospy.sleep(0.1)
#! /usr/bin/env python
from utilities import set_model_state
from geometry_msgs.msg import Pose, Point
items = ['cabinet', 'cabinet_0', 'cabinet_1', 'cabinet_2', 'cabinet_3',
'cabinet_4', 'table', 'table_0', 'table_1', 'bookshelf',
'bookshelf_1', 'bookshelf_2', 'bookshelf_3', 'bookshelf_4',
'bookshelf_5', 'bookshelf_6', 'bookshelf_0']
newpos = [(4.5,1.5,0), (4.5,-4.5,0), (-4.5,-3.5,0), (-4.5,-4.5,0), (4.5,-2.5,0),
(-4.5,-2.5,0), (-3,-3.6,0), (4,-3.6,0), (-1,-12.5,0), (1,-12.5,0),
(-4.5,1.8,0), (-4.5,-12.5,0), (2.25,-12.5,0), (3.5,-12.5,0),
(-3,-12.5,0), (-1,-3.2,0), (-3.5, -2.25, 0)]
for item, pos in zip(items, newpos):
x, y, z = pos
set_model_state(item, Pose(position=Point(x,y,z)))
import math, rospy
from utilities import set_model_state, get_model_state
from geometry_msgs.msg import Pose, Point, Quaternion
position = Point(x=0, y=0, z=0)
for angle in range(0, 360, 10):
theta = math.radians(angle)
orientation = Quaternion(x=0,
y=0,
z=math.sin(theta / 2),
w=math.cos(theta / 2))
print("orientation", orientation)
set_model_state('mobile_base', Pose(position, orientation))
pose = Pose(position, orientation)
print("Pose", pose)
rospy.sleep(0.1)
import math, rospy
from utilities import set_model_state, get_model_state, \
spawn_coke_can
from geometry_msgs.msg import Pose, Point, Quaternion
if get_model_state('coke_can').success == False:
spawn_coke_can('coke_can', Pose(position=Point(0, 1, 0.22)))
print("pose is", Pose)
model_state = get_model_state('coke_can')
print(model_state.pose.position)
new_pose = Pose(position=Point(1, 0, 0.22))
set_model_state('coke_can', new_pose)
#! /usr/bin/env python
import math, rospy
from tf.transformations import quaternion_about_axis, quaternion_multiply
from utilities import set_model_state
from geometry_msgs.msg import Pose, Point, Quaternion
for angle in range(0,360,30):
can = angle/30
theta = math.radians(angle)
xp = 0.2 * math.cos(theta) + 1
yp = 0.2 * math.sin(theta)
model_name = 'coke_can_' + str(can)
position = Point(xp,yp,1.05)
q_z = quaternion_about_axis(theta, (0,0,1))
q_y = quaternion_about_axis(math.radians(90), (0,1,0))
q_zy = quaternion_multiply(q_z, q_y)
orientation = Quaternion(*q_zy)
set_model_state(model_name, Pose(position, Quaternion(*q_zy)))
rospy.sleep(0.1)