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-15 00:55:01 +00:00
parent 17759be84a
commit 7a4fd3133a
9 changed files with 222 additions and 27 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

0
brick/List of theoretical brick poses Normal file
View File

BIN
brick/meshes/Brick.STL Executable file
View File

Binary file not shown.

BIN
brick/meshes/Brick_1.STL Executable file
View File

Binary file not shown.

19
brick/model.config Executable file
View File

@ -0,0 +1,19 @@
<?xml version="1.0"?>
<model>
<name>Brick</name>
<version>1.0</version>
<sdf version="1.2">model-1_2.sdf</sdf>
<sdf version="1.3">model-1_3.sdf</sdf>
<sdf version="1.4">model-1_4.sdf</sdf>
<sdf version="1.5">model.sdf</sdf>
<author>
<name>RON</name>
<email>r.saputra@imperial.ac.uk</email>
</author>
<description>
Brick model
</description>
</model>

62
brick/model.sdf Executable file
View File

@ -0,0 +1,62 @@
<?xml version="1.0" ?>
<sdf version='1.6'>
<model name='Brick'>
<link name='Brick'>
<pose frame=''>0 0 0 0 -0 0</pose>
<inertial>
<pose frame=''> 0 0 0.0 -0 0</pose>
<mass>0.1</mass>
<inertia>
<ixx>0.0017</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.0017</iyy>
<iyz>0</iyz>
<izz>0</izz>
</inertia>
</inertial>
<collision name='Brick_collision'>
<pose frame=''>0 0 0 1.5708 -0 0</pose>
<geometry>
<box>
<size>0.192 0.062 0.086</size>
</box>
</geometry>
<surface>
<friction>
<ode>
<mu>100</mu>
<mu2>50</mu2>
<fdir1>0 0 1</fdir1>
<slip1>0.0</slip1>
<slip2>0.0</slip2>
</ode>
</friction>
<contact>
<ode>
<kp>100000.000000</kp>
<kd>10.00000</kd>
<max_vel>2.000000</max_vel>
<min_depth>0.0001</min_depth>
</ode>
</contact>
</surface>
</collision>
<visual name='Brick_visual'>
<pose frame=''>0 0 0 1.5708 -0 0</pose>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>model://Brick/meshes/Brick_1.STL</uri>
</mesh>
</geometry>
<material>
<ambient>0.698 0.25 0.25 1</ambient>
<diffuse>0.698 0.25 0.25 1</diffuse>
<specular>0.1 0.1 0.1 1</specular>
<emissive>0 0 0 0</emissive>
</material>
</visual>
</link>
</model>
</sdf>

62
brick/model.sdfbk Executable file
View File

@ -0,0 +1,62 @@
<?xml version="1.0" ?>
<sdf version='1.6'>
<model name='Brick'>
<link name='Brick'>
<pose frame=''>0 0 0.033 0 -0 0</pose>
<inertial>
<pose frame=''>0 -0 0.031 0 -0 0</pose>
<mass>0.1</mass>
<inertia>
<ixx>0.0017</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.0017</iyy>
<iyz>0</iyz>
<izz>0.0017</izz>
</inertia>
</inertial>
<collision name='Brick_collision'>
<pose frame=''>0 0 0 1.5708 -0 0</pose>
<geometry>
<box>
<size>0.192 0.062 0.086</size>
</box>
</geometry>
<surface>
<friction>
<ode>
<mu>100</mu>
<mu2>50</mu2>
<fdir1>0 0 1</fdir1>
<slip1>0.0</slip1>
<slip2>0.0</slip2>
</ode>
</friction>
<contact>
<ode>
<kp>100000.000000</kp>
<kd>10.00000</kd>
<max_vel>2.000000</max_vel>
<min_depth>0.0001</min_depth>
</ode>
</contact>
</surface>
</collision>
<visual name='Brick_visual'>
<pose frame=''>0 0 0 1.5708 -0 0</pose>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>model://Brick/meshes/Brick_1.STL</uri>
</mesh>
</geometry>
<material>
<ambient>0.698 0.25 0.25 1</ambient>
<diffuse>0.698 0.25 0.25 1</diffuse>
<specular>0.1 0.1 0.1 1</specular>
<emissive>0 0 0 0</emissive>
</material>
</visual>
</link>
</model>
</sdf>

73
new_main.py
View File

@ -16,6 +16,8 @@ import baxter_interface
import target_poses as tps import target_poses as tps
import tuck_arms
brickstuff = tps.brick_directions_notf brickstuff = tps.brick_directions_notf
class PickAndPlace(object): class PickAndPlace(object):
@ -146,28 +148,67 @@ class PickAndPlace(object):
# retract to clear object # retract to clear object
self._retract() self._retract()
brick_ids = ['b1','b2','b3','b4','b5','b6','b7','b8','b9']
print(brickstuff[1]['pose'].position.x) with open ("models/brick/model.sdf", "r") as brick_file:brick_sdf=brick_file.read().replace('\n', '')
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)
def cleanup():
for obj in brick_ids:
delete_model(obj)
def spawn_brick(vertical=True):
brick_pose = Pose()
if vertical:
brick_pose.position.x = 0.4896
brick_pose.position.y = 0.7067
brick_pose.position.z = 0.8576
brick_pose.orientation.x = 0
brick_pose.orientation.y = 0.707
brick_pose.orientation.z = 0
brick_pose.orientation.w = 0.707
else:
pass
brick_reference_frame = 'world'
brick_id = brick_ids.pop()
spawn_sdf(brick_id, brick_sdf, "/", brick_pose, brick_reference_frame)
tuck_arms.init_arms()
rospy.init_node("I_still_have_some_hope") # Am I wrong?? rospy.init_node("I_still_have_some_hope") # Am I wrong??
hover_distance = 0.2 hover_distance = 0.2
left_pnp = PickAndPlace('left', hover_distance) left_pnp = PickAndPlace('left', hover_distance)
spawn_brick(vertical=True)
left_pnp.pick(brickstuff[0]['pose']) left_pnp.pick(brickstuff[0]['pose'])
left_pnp.place(brickstuff[2]['pose']) left_pnp.place(brickstuff[2]['pose'])
left_pnp.pick(brickstuff[0]['pose']) # spawn_brick(vertical=True)
left_pnp.place(brickstuff[3]['pose']) # left_pnp.pick(brickstuff[0]['pose'])
left_pnp.pick(brickstuff[0]['pose']) # left_pnp.place(brickstuff[3]['pose'])
left_pnp.place(brickstuff[4]['pose']) # spawn_brick(vertical=True)
# left_pnp.pick(brickstuff[0]['pose'])
# left_pnp.place(brickstuff[4]['pose'])
# spawn_brick(vertical=False)
left_pnp.pick(brickstuff[1]['pose']) left_pnp.pick(brickstuff[1]['pose'])
left_pnp.place(brickstuff[5]['pose']) # left_pnp.place(brickstuff[5]['pose'])
left_pnp.pick(brickstuff[1]['pose']) # spawn_brick(vertical=False)
left_pnp.place(brickstuff[6]['pose']) # left_pnp.pick(brickstuff[1]['pose'])
left_pnp.pick(brickstuff[0]['pose']) # left_pnp.place(brickstuff[6]['pose'])
left_pnp.place(brickstuff[7]['pose']) # spawn_brick(vertical=True)
left_pnp.pick(brickstuff[0]['pose']) # left_pnp.pick(brickstuff[0]['pose'])
left_pnp.place(brickstuff[8]['pose']) # left_pnp.place(brickstuff[7]['pose'])
left_pnp.pick(brickstuff[1]['pose']) # spawn_brick(vertical=True)
left_pnp.place(brickstuff[9]['pose']) # left_pnp.pick(brickstuff[0]['pose'])
left_pnp.pick(brickstuff[0]['pose']) # left_pnp.place(brickstuff[8]['pose'])
left_pnp.place(brickstuff[10]['pose']) # spawn_brick(vertical=False)
# left_pnp.pick(brickstuff[1]['pose'])
# left_pnp.place(brickstuff[9]['pose'])
# spawn_brick(vertical=True)
# left_pnp.pick(brickstuff[0]['pose'])
# left_pnp.place(brickstuff[10]['pose'])

4
redundant/brick_data.py
View File

@ -24,6 +24,10 @@ def etq(roll, pitch, yaw):
def q_extrapolator(obj): def q_extrapolator(obj):
return obj.orientation.x, obj.orientation.y, obj.orientation.z, obj.orientation.w return obj.orientation.x, obj.orientation.y, obj.orientation.z, obj.orientation.w
print(etq(-0.2347, 1.5707, -0.2347))
exit(0)
##DELETE THIS ##DELETE THIS
class Position(): class Position():
def __init__(self): def __init__(self):

29
tuck_arms.py
View File

@ -136,7 +136,7 @@ class Tuck(object):
at_goal = lambda: (abs(head.pan()) <= at_goal = lambda: (abs(head.pan()) <=
baxter_interface.settings.HEAD_PAN_ANGLE_TOLERANCE) baxter_interface.settings.HEAD_PAN_ANGLE_TOLERANCE)
rospy.loginfo("Moving head to neutral position") # rospy.loginfo("Moving head to neutral position")
while not at_goal() and not rospy.is_shutdown(): while not at_goal() and not rospy.is_shutdown():
if start_disabled: if start_disabled:
[pub.publish(Empty()) for pub in self._disable_pub.values()] [pub.publish(Empty()) for pub in self._disable_pub.values()]
@ -182,16 +182,16 @@ class Tuck(object):
# If arms are already tucked, report this to user and exit. # If arms are already tucked, report this to user and exit.
if all(self._arm_state['tuck'][limb] == 'tuck' if all(self._arm_state['tuck'][limb] == 'tuck'
for limb in self._limbs): for limb in self._limbs):
rospy.loginfo("Tucking: Arms already in 'Tucked' position.") # rospy.loginfo("Tucking: Arms already in 'Tucked' position.")
self._done = True self._done = True
return return
else: else:
rospy.loginfo("Tucking: One or more arms not Tucked.") # rospy.loginfo("Tucking: One or more arms not Tucked.")
any_flipped = not all(self._arm_state['flipped'].values()) any_flipped = not all(self._arm_state['flipped'].values())
if any_flipped: if any_flipped:
rospy.loginfo( # rospy.loginfo(
"Moving to neutral start position with collision %s.", # "Moving to neutral start position with collision %s.",
"on" if any_flipped else "off") # "on" if any_flipped else "off")
# Move to neutral pose before tucking arms to avoid damage # Move to neutral pose before tucking arms to avoid damage
self._check_arm_state() self._check_arm_state()
actions = dict() actions = dict()
@ -203,7 +203,7 @@ class Tuck(object):
self._move_to(actions, disabled) self._move_to(actions, disabled)
# Disable collision and Tuck Arms # Disable collision and Tuck Arms
rospy.loginfo("Tucking: Tucking with collision avoidance off.") # rospy.loginfo("Tucking: Tucking with collision avoidance off.")
actions = {'left': 'tuck', 'right': 'tuck'} actions = {'left': 'tuck', 'right': 'tuck'}
disabled = {'left': True, 'right': True} disabled = {'left': True, 'right': True}
self._move_to(actions, disabled) self._move_to(actions, disabled)
@ -214,8 +214,8 @@ class Tuck(object):
else: else:
# If arms are tucked disable collision and untuck arms # If arms are tucked disable collision and untuck arms
if any(self._arm_state['flipped'].values()): if any(self._arm_state['flipped'].values()):
rospy.loginfo("Untucking: One or more arms Tucked;" # rospy.loginfo("Untucking: One or more arms Tucked;"
" Disabling Collision Avoidance and untucking.") # " Disabling Collision Avoidance and untucking.")
self._check_arm_state() self._check_arm_state()
suppress = deepcopy(self._arm_state['flipped']) suppress = deepcopy(self._arm_state['flipped'])
actions = {'left': 'untuck', 'right': 'untuck'} actions = {'left': 'untuck', 'right': 'untuck'}
@ -224,8 +224,8 @@ class Tuck(object):
return return
# If arms already untucked, move to neutral location # If arms already untucked, move to neutral location
else: else:
rospy.loginfo("Untucking: Arms already Untucked;" # rospy.loginfo("Untucking: Arms already Untucked;"
" Moving to neutral position.") # " Moving to neutral position.")
self._check_arm_state() self._check_arm_state()
suppress = deepcopy(self._arm_state['flipped']) suppress = deepcopy(self._arm_state['flipped'])
actions = {'left': 'untuck', 'right': 'untuck'} actions = {'left': 'untuck', 'right': 'untuck'}
@ -264,3 +264,10 @@ def main():
if __name__ == "__main__": if __name__ == "__main__":
main() main()
def init_arms():
tuck = False
tucker = Tuck(tuck)
rospy.on_shutdown(tucker.clean_shutdown)
tucker.supervised_tuck()