Update
This commit is contained in:
Max Hunt
parent
17759be84a
commit
7a4fd3133a
0
brick/List of theoretical brick poses
Normal file
0
brick/List of theoretical brick poses
Normal file
BIN
brick/meshes/Brick.STL
Executable file
BIN
brick/meshes/Brick.STL
Executable file
Binary file not shown.
BIN
brick/meshes/Brick_1.STL
Executable file
BIN
brick/meshes/Brick_1.STL
Executable file
Binary file not shown.
19
brick/model.config
Executable file
19
brick/model.config
Executable 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
62
brick/model.sdf
Executable 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
62
brick/model.sdfbk
Executable 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
73
new_main.py
@ -16,6 +16,8 @@ import baxter_interface
|
||||
|
||||
import target_poses as tps
|
||||
|
||||
import tuck_arms
|
||||
|
||||
brickstuff = tps.brick_directions_notf
|
||||
|
||||
class PickAndPlace(object):
|
||||
@ -146,28 +148,67 @@ class PickAndPlace(object):
|
||||
# retract to clear object
|
||||
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??
|
||||
hover_distance = 0.2
|
||||
left_pnp = PickAndPlace('left', hover_distance)
|
||||
|
||||
|
||||
spawn_brick(vertical=True)
|
||||
left_pnp.pick(brickstuff[0]['pose'])
|
||||
left_pnp.place(brickstuff[2]['pose'])
|
||||
left_pnp.pick(brickstuff[0]['pose'])
|
||||
left_pnp.place(brickstuff[3]['pose'])
|
||||
left_pnp.pick(brickstuff[0]['pose'])
|
||||
left_pnp.place(brickstuff[4]['pose'])
|
||||
# spawn_brick(vertical=True)
|
||||
# left_pnp.pick(brickstuff[0]['pose'])
|
||||
# left_pnp.place(brickstuff[3]['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.place(brickstuff[5]['pose'])
|
||||
left_pnp.pick(brickstuff[1]['pose'])
|
||||
left_pnp.place(brickstuff[6]['pose'])
|
||||
left_pnp.pick(brickstuff[0]['pose'])
|
||||
left_pnp.place(brickstuff[7]['pose'])
|
||||
left_pnp.pick(brickstuff[0]['pose'])
|
||||
left_pnp.place(brickstuff[8]['pose'])
|
||||
left_pnp.pick(brickstuff[1]['pose'])
|
||||
left_pnp.place(brickstuff[9]['pose'])
|
||||
left_pnp.pick(brickstuff[0]['pose'])
|
||||
left_pnp.place(brickstuff[10]['pose'])
|
||||
# left_pnp.place(brickstuff[5]['pose'])
|
||||
# spawn_brick(vertical=False)
|
||||
# left_pnp.pick(brickstuff[1]['pose'])
|
||||
# left_pnp.place(brickstuff[6]['pose'])
|
||||
# spawn_brick(vertical=True)
|
||||
# left_pnp.pick(brickstuff[0]['pose'])
|
||||
# left_pnp.place(brickstuff[7]['pose'])
|
||||
# spawn_brick(vertical=True)
|
||||
# left_pnp.pick(brickstuff[0]['pose'])
|
||||
# left_pnp.place(brickstuff[8]['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'])
|
||||
|
||||
@ -24,6 +24,10 @@ def etq(roll, pitch, yaw):
|
||||
def q_extrapolator(obj):
|
||||
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
|
||||
class Position():
|
||||
def __init__(self):
|
||||
|
||||
29
tuck_arms.py
29
tuck_arms.py
@ -136,7 +136,7 @@ class Tuck(object):
|
||||
at_goal = lambda: (abs(head.pan()) <=
|
||||
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():
|
||||
if start_disabled:
|
||||
[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 all(self._arm_state['tuck'][limb] == 'tuck'
|
||||
for limb in self._limbs):
|
||||
rospy.loginfo("Tucking: Arms already in 'Tucked' position.")
|
||||
# rospy.loginfo("Tucking: Arms already in 'Tucked' position.")
|
||||
self._done = True
|
||||
return
|
||||
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())
|
||||
if any_flipped:
|
||||
rospy.loginfo(
|
||||
"Moving to neutral start position with collision %s.",
|
||||
"on" if any_flipped else "off")
|
||||
# rospy.loginfo(
|
||||
# "Moving to neutral start position with collision %s.",
|
||||
# "on" if any_flipped else "off")
|
||||
# Move to neutral pose before tucking arms to avoid damage
|
||||
self._check_arm_state()
|
||||
actions = dict()
|
||||
@ -203,7 +203,7 @@ class Tuck(object):
|
||||
self._move_to(actions, disabled)
|
||||
|
||||
# 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'}
|
||||
disabled = {'left': True, 'right': True}
|
||||
self._move_to(actions, disabled)
|
||||
@ -214,8 +214,8 @@ class Tuck(object):
|
||||
else:
|
||||
# If arms are tucked disable collision and untuck arms
|
||||
if any(self._arm_state['flipped'].values()):
|
||||
rospy.loginfo("Untucking: One or more arms Tucked;"
|
||||
" Disabling Collision Avoidance and untucking.")
|
||||
# rospy.loginfo("Untucking: One or more arms Tucked;"
|
||||
# " Disabling Collision Avoidance and untucking.")
|
||||
self._check_arm_state()
|
||||
suppress = deepcopy(self._arm_state['flipped'])
|
||||
actions = {'left': 'untuck', 'right': 'untuck'}
|
||||
@ -224,8 +224,8 @@ class Tuck(object):
|
||||
return
|
||||
# If arms already untucked, move to neutral location
|
||||
else:
|
||||
rospy.loginfo("Untucking: Arms already Untucked;"
|
||||
" Moving to neutral position.")
|
||||
# rospy.loginfo("Untucking: Arms already Untucked;"
|
||||
# " Moving to neutral position.")
|
||||
self._check_arm_state()
|
||||
suppress = deepcopy(self._arm_state['flipped'])
|
||||
actions = {'left': 'untuck', 'right': 'untuck'}
|
||||
@ -264,3 +264,10 @@ def main():
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
|
||||
def init_arms():
|
||||
tuck = False
|
||||
tucker = Tuck(tuck)
|
||||
rospy.on_shutdown(tucker.clean_shutdown)
|
||||
tucker.supervised_tuck()
|
||||
|
||||
|
||||
Loading…
Reference in New Issue
Block a user