max/robotics
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Update

Browse Source
This commit is contained in:
Max Hunt 2020-02-08 17:34:03 +00:00
parent 0e2915501b
commit 8fe55e470e
2 changed files with 31 additions and 65 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

82
main.py
View File

@ -342,23 +342,19 @@ def load_objects():
# Load Table SDFs # Load Table SDFs
with open ("models/L3-table/model.sdf", "r") as table_file:table_xml=table_file.read().replace('\n', '') with open ("models/L3-table/model.sdf", "r") as table_file:table_xml=table_file.read().replace('\n', '')
with open ("models/brick/model.sdf", "r") as brick_file:brick_xml=brick_file.read().replace('\n', '') with open ("models/brick/model.sdf", "r") as brick_file:brick_xml=brick_file.read().replace('\n', '')
# Spawn Table SDF # Spawn Table SDF
rospy.wait_for_service('/gazebo/spawn_sdf_model') rospy.wait_for_service('/gazebo/spawn_sdf_model')
spawn_sdf = rospy.ServiceProxy('/gazebo/spawn_sdf_model', SpawnModel) spawn_sdf = rospy.ServiceProxy('/gazebo/spawn_sdf_model', SpawnModel)
delete_model = rospy.ServiceProxy('/gazebo/delete_model', DeleteModel) delete_model = rospy.ServiceProxy('/gazebo/delete_model', DeleteModel)
table_pose = Pose() table_pose = Pose()
table_pose.position = Point(x=table['x'], y=table['y'], z=table['z']) table_pose.position = Point(x=table['x'], y=table['y'], z=table['z'])
tqo = quaternion_from_euler(table['roll'], table['pitch'], table['yaw']) tqo = quaternion_from_euler(table['roll'], table['pitch'], table['yaw'])
table_pose.orientation = Quaternion(tqo[0], tqo[1], tqo[2], tqo[3]) table_pose.orientation = Quaternion(tqo[0], tqo[1], tqo[2], tqo[3])
table_reference_frame="world" table_reference_frame="world"
spawn_sdf(table['id'], table_xml, "/", table_pose, table_reference_frame) spawn_sdf(table['id'], table_xml, "/", table_pose, table_reference_frame)
brick_ents = [] brick_ents = []
for ar in [bricks_start, bricks_end]:
for brick in bricks: for brick in ar:
ber = [brick['roll'], brick['pitch'], brick['yaw']] #brick_euler_rotation ber = [brick['roll'], brick['pitch'], brick['yaw']] #brick_euler_rotation
brick_pose = Pose(position=Point(x=brick['x'], y=brick['y'], z=brick['z'])) brick_pose = Pose(position=Point(x=brick['x'], y=brick['y'], z=brick['z']))
brick_pose.position = Point(x=brick['x'], y=brick['y'], z=brick['z']) brick_pose.position = Point(x=brick['x'], y=brick['y'], z=brick['z'])
@ -371,57 +367,27 @@ def load_objects():
rospy.init_node("ik_pick_and_place_demo")
hover_distance = 0.1 # meters
# Starting Pose for left arm
class debug_functions(): left_pose = Pose()
with open ("models/brick/model.sdf", "r") as brick_file:brick_xml=brick_file.read().replace('\n', '') left_pose.position.x = 0.579679836383
left_pose.position.y = 0.283311769707
def t(): left_pose.position.z = 0.213676720426
delete_model(table['id']) left_pose.orientation.x = -0.0249590815779
tableLoader() left_pose.orientation.y = 0.999649402929
left_pose.orientation.z = 0.00737916180073
def tableLoader(): left_pose.orientation.w = 0.00486450832011
with open ("models/L3-table/model.sdf", "r") as table_file:table_xml=table_file.read().replace('\n', '')
table_pose = Pose() # Starting Pose for right arm
table_pose.position = Point(x=table['x'], y=table['y'], z=table['z']) right_pose = Pose()
tqo = quaternion_from_euler(table['roll'], table['pitch'], table['yaw']) right_pose.position.x = 0.579679836383
table_pose.orientation = Quaternion(tqo[0], tqo[1], tqo[2], tqo[3]) right_pose.position.y = -0.283311769707
table_reference_frame="world" right_pose.position.z = 0.213676720426
spawn_sdf(table['id'], table_xml, "/", table_pose, table_reference_frame) right_pose.orientation.x = -0.0249590815779
right_pose.orientation.y = 0.999649402929
def make(num = 0, arr = bricks_i): right_pose.orientation.z = -0.00737916180073
if num == 0: num = len(arr) right_pose.orientation.w = 0.00486450832011
for ix in range(0, num):#brick in arr:
brick = arr[ix]
ber = [brick['roll'], brick['pitch'], brick['yaw']] #brick_euler_rotation
brick_pose = Pose()
brick_pose.position.x = brick['x']
brick_pose.position.y = brick['y']
brick_pose.position.z = brick['z']
bqo = quaternion_from_euler(ber[0], ber[1], ber[2])
brick_pose.orientation = Quaternion(bqo[0], bqo[1], bqo[2], bqo[3])
brick_reference_frame=brick['rframe']
brick_id = brick['id']
brick_ents.append(spawn_sdf(brick_id, brick_xml, "/", brick_pose, brick_reference_frame))
def remove(arr2 = bricks_i):
for i in arr2:
delete_model(i['id'])
def x_place_bricks():
rospy.wait_for_service('/gazebo/spawn_sdf_model')
spawn_sdf = rospy.ServiceProxy('/gazebo/spawn_sdf_model', SpawnModel)
delete_model = rospy.ServiceProxy('/gazebo/delete_model', DeleteModel)
with open ("models/brick/model.sdf", "r") as brick_file:brick_xml=brick_file.read().replace('\n', '')
for brick in bricks:
ber = [brick['roll'], brick['pitch'], brick['yaw']] #brick_euler_rotation
brick_pose = Pose(position=Point(x=brick['x'], y=brick['y'], z=brick['z']))
brick_pose.position = Point(x=brick['x'], y=brick['y'], z=brick['z'])
bqo = quaternion_from_euler(brick['roll'], brick['pitch'], brick['yaw'])
brick_pose.orientation = Quaternion(bqo[0], bqo[1], bqo[2], bqo[3])
spawn_sdf(brick['id'], brick_xml, "/", brick_pose, brick['rframe'])

0
r
View File