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

96
main.py
View File

@ -342,86 +342,52 @@ def load_objects():
# Load Table SDFs
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', '')
# Spawn Table SDF
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)
table_pose = Pose()
table_pose.position = Point(x=table['x'], y=table['y'], z=table['z'])
tqo = quaternion_from_euler(table['roll'], table['pitch'], table['yaw'])
table_pose.orientation = Quaternion(tqo[0], tqo[1], tqo[2], tqo[3])
table_reference_frame="world"
spawn_sdf(table['id'], table_xml, "/", table_pose, table_reference_frame)
brick_ents = []
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])
brick_reference_frame=brick['rframe']
brick_id = brick['id']
brick_ents.append(spawn_sdf(brick_id, brick_xml, "/", brick_pose, brick_reference_frame))
class debug_functions():
with open ("models/brick/model.sdf", "r") as brick_file:brick_xml=brick_file.read().replace('\n', '')
def t():
delete_model(table['id'])
tableLoader()
def tableLoader():
with open ("models/L3-table/model.sdf", "r") as table_file:table_xml=table_file.read().replace('\n', '')
table_pose = Pose()
table_pose.position = Point(x=table['x'], y=table['y'], z=table['z'])
tqo = quaternion_from_euler(table['roll'], table['pitch'], table['yaw'])
table_pose.orientation = Quaternion(tqo[0], tqo[1], tqo[2], tqo[3])
table_reference_frame="world"
spawn_sdf(table['id'], table_xml, "/", table_pose, table_reference_frame)
def make(num = 0, arr = bricks_i):
if num == 0: num = len(arr)
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:
for ar in [bricks_start, bricks_end]:
for brick in ar:
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'])
brick_reference_frame=brick['rframe']
brick_id = brick['id']
brick_ents.append(spawn_sdf(brick_id, brick_xml, "/", brick_pose, brick_reference_frame))
rospy.init_node("ik_pick_and_place_demo")
hover_distance = 0.1 # meters
# Starting Pose for left arm
left_pose = Pose()
left_pose.position.x = 0.579679836383
left_pose.position.y = 0.283311769707
left_pose.position.z = 0.213676720426
left_pose.orientation.x = -0.0249590815779
left_pose.orientation.y = 0.999649402929
left_pose.orientation.z = 0.00737916180073
left_pose.orientation.w = 0.00486450832011
# Starting Pose for right arm
right_pose = Pose()
right_pose.position.x = 0.579679836383
right_pose.position.y = -0.283311769707
right_pose.position.z = 0.213676720426
right_pose.orientation.x = -0.0249590815779
right_pose.orientation.y = 0.999649402929
right_pose.orientation.z = -0.00737916180073
right_pose.orientation.w = 0.00486450832011

0
r
View File