def add_generated_folder(world_view_client: WorldViewClient, world_view_folder: str) -> None:
""" Adds a folder to world view, deletes content if existent"""
try:
# delete folder if it already exists
world_view_client.remove_element("generated", world_view_folder)
except Exception as e:
None
world_view_client.add_folder("generated", world_view_folder)
def calculate_pre_place_joint_values(pre_place_poses: List[Pose],
jv_home: JointValues,
move_group: MoveGroup,
world_view_client: WorldViewClient,
world_view_folder: str) \
-> List[JointValues]:
"""
Calculates the pre place joint values
Since we want the robot arm to go back and forth from the home configuration
to the poses on the grid, we use the home joint value as a const seed for
every pose to minimize the distance for the joints to make.
Calling inverse_kinematics_many with "const_seed = True" lets us exclusively
using the jv_home joint values as seed.
"const_seed = False" would use the previously calculated joint values as seed
for the next on when traversing pre_place_poses. This could be useful when
we went from pose to pose without visiting the jv_home configuration in between.
Parameters
----------
pre_place_poses : List[Pose]
A list of poses for which the joint values should be calculated
jv_home : JointValues
The seed to be used
move_group : MoveGroup
world_view_client: WorldViewClient
Returns
------
List[JointValues]
A list of joint values for every pose
"""
end_effector = move_group.get_end_effector()
try:
pre_place_jvs = example_02_3_create_joint_values_from_poses.main(
pre_place_poses,
jv_home,
end_effector)
except Exception as e:
print("The inverse kinematics operation could not be applied on all the positions.")
world_view_client.remove_element("generated", world_view_folder)
raise e
# export every calculated joint values to world view to illustrate the result
for i in range(len(pre_place_jvs)):
world_view_client.add_joint_values("joint_values_{}".format(str(i).zfill(2)),
"/{}/generated/pre_place_joint_values".format(world_view_folder),
pre_place_jvs[i])
return pre_place_jvs
async def main():
world_view_folder = "example_06_null_space"
world_view_client = WorldViewClient()
# Since we want to update the Jointvalues of the arms independently, we
# need the respective MoveGroups
left_move_group = example_utils.get_left_move_group()
right_move_group = example_utils.get_right_move_group()
# The MoveGroup of the whole robot
full_body_move_group = example_utils.get_full_body_move_group()
# To address the torso joint in particular, we need the name
torso_joint_name = example_utils.get_torso_joint_name()
# Load of the begin configuration from world view. This defines the starting
# angle of the torso
begin_jvs = world_view_client.get_joint_values("Begin", world_view_folder)
print("Go to begin configuration ")
await full_body_move_group.move_joints_collision_free(begin_jvs)
cache = Cache("I am in a", "example_06")
cache.load()
waypoints = cache.get("waypoints")
# Calculate the list of JointValues describing the movement
if waypoints == None:
waypoints = plan_null_space_torso_move(left_move_group,
right_move_group,
full_body_move_group,
torso_joint_name,
0.5)
print("Saved waypoints to file")
cache.add("waypoints", waypoints)
cache.dump()
else:
print("Loaded waypoints from file.")
add_generated_folder(world_view_client, world_view_folder)
# Save every waypoint to world view for visualization
for i in range(len(waypoints)):
joint_values = waypoints[i]
name = "joint_values_{}".format( format(i, "0>2d"))
world_view_client.add_joint_values(name,
"/{}/generated".format(world_view_folder),
joint_values)
print("Now follow waypoints")
# Wiggle two times
for i in range(2):
# Go forth ...
joint_path = JointPath(waypoints[0].joint_set, waypoints)
await full_body_move_group.move_joints(joint_path, velocity_scaling=0.4)
# ... and back
reversed_joint_path = JointPath(waypoints[0].joint_set, list(reversed(waypoints)))
await full_body_move_group.move_joints(reversed_joint_path, velocity_scaling=0.4)
# clean up
world_view_client.remove_element("generated", world_view_folder)
def generate_folders(world_view_client: WorldViewClient) -> None:
"""
Generate the folder we want to use in world view
"""
try:
# delete folder if it already exists
world_view_client.remove_element("generated", "/example_02_palletizing")
except Exception as e:
None
# Add a folder to hold calculated joint values to be accessed in world view
world_view_client.add_folder("generated", "/example_02_palletizing")
world_view_client.add_folder("collision_objects", "/example_02_palletizing/generated")
# To show all generated joint values in the world view
world_view_client.add_folder("pre_place_joint_values", "/example_02_palletizing/generated")
def main():
world_view_folder = "example_07_jogging"
world_view_client = WorldViewClient()
move_group = example_utils.get_move_group()
current_pose = move_group.get_end_effector().compute_pose(
move_group.get_current_joint_positions())
jogging_client = JoggingClient()
jogging_client.set_move_group_name(example_utils.get_move_group_name())
# register feedback function, to get alerted when an error occurs
jogging_client.register(feedback_function)
current_pose = move_group.get_end_effector().compute_pose(
move_group.get_current_joint_positions())
point_name = "TrackingPose"
# Write pose to World View, so the tracking begins with the current end effector pose
try:
world_view_client.add_pose(point_name, world_view_folder, current_pose)
except Xamla_ArgumentError:
world_view_client.update_pose(point_name, world_view_folder,
current_pose)
get_pose = lambda: world_view_client.get_pose(point_name, world_view_folder
)
#Begin tracking
jogging_client.toggle_tracking(True)
# Track for 20 seconds with a frequency of 50 Hz
track_point(time_amount=20,
frequency=50,
point_name=point_name,
get_pose=get_pose,
jogging_client=jogging_client)
# Stop tracking
jogging_client.toggle_tracking(False)
# Unregister the feedback function
jogging_client.unregister(feedback_function)
# Remove tracking pose
world_view_client.remove_element(point_name, world_view_folder)
def main():
world_view_folder = "example_07_jogging"
jogging_client = JoggingClient()
world_view_client = WorldViewClient()
move_group = example_utils.get_move_group()
current_pose = move_group.get_end_effector().compute_pose(
move_group.get_current_joint_positions())
move_group_name = "/sda10f/right_arm_torso"
jogging_client.set_move_group_name(move_group_name)
# register feedback function, to get alerted when an error occurs
jogging_client.register(feedback_function)
# Begin tracking
jogging_client.start()
# Do not allow anything to be printed on the console when navigating
fd = sys.stdin.fileno()
# Store old attributes
old_attr = termios.tcgetattr(fd)
new_attr = termios.tcgetattr(fd)
# This disable echo
new_attr[3] = new_attr[3] & ~termios.ECHO
termios.tcsetattr(fd, termios.TCSADRAIN, new_attr)
try:
interface = JoggingKeyboardInterface(jogging_client, move_group,
world_view_client)
interface.thread_start()
except Exception as e:
print(e)
finally:
# Allow characters to be entered again
termios.tcsetattr(fd, termios.TCSADRAIN, old_attr)
# Flush characters entered
termios.tcflush(sys.stdin, termios.TCIOFLUSH)
# Stop tracking python3
jogging_client.stop()
# Unregister the feedback function
jogging_client.unregister(feedback_function)
def main():
world_view_folder = ""
jogging_client = JoggingClient()
world_view_client = WorldViewClient()
move_group = example_utils.get_move_group()
current_pose = move_group.get_end_effector().compute_pose(
move_group.get_current_joint_positions())
move_group_name = "/sda10f/right_arm_torso"
jogging_client.set_move_group_name(move_group_name)
# register feedback function, to get alerted when an error occurs
jogging_client.register(feedback_function)
#Begin tracking
jogging_client.toggle_tracking(True)
# Go back and forth in x direction (linear velocity)
forth_trans_twist = Twist(linear=[0.5, 0, 0], angular=[0, 0, 0])
back_trans_twist = Twist(linear=[-0.5, 0, 0], angular=[0, 0, 0])
time_amount = 2
frequency = 50
print("Go forth along x-axis")
apply_twist(time_amount, frequency, forth_trans_twist, jogging_client)
print("Go back along x-axis")
apply_twist(time_amount, frequency, back_trans_twist, jogging_client)
# Keep rolling
forth_roll_twist = Twist([0, 0, 0], [10, 0, 0])
back_roll_twist = Twist([0, 0, 0], [-10, 0, 0])
print("Rotate forth along y-axis")
apply_twist(time_amount, frequency, forth_roll_twist, jogging_client)
print("Rotate back along y-axis")
apply_twist(time_amount, frequency, back_roll_twist, jogging_client)
# Stop tracking
jogging_client.toggle_tracking(False)
# Unregister the feedback function
jogging_client.unregister(feedback_function)
shifted_jvs = find_shifted_joint_values(joint_values, diff_pose, move_group)
shifted_jvs_list.append(shifted_jvs)
print("Success.")
except ServiceException as e:
# print(e)
print("Could not shift joint values.".format(joint_values))
# append None so we can test if this failed or not
shifted_jvs_list.append(None)
return shifted_jvs_list
if __name__ == '__main__':
# Called when running this script standalone
world_view_folder = "example_03_shifting"
world_view_client = WorldViewClient()
# Read joint values from world view
joint_values_dict = world_view_client.query_joint_values(
"{}/jointValues".format(world_view_folder))
joint_values_list = list(joint_values_dict.values())
# Read names too to associate the joint values to the poses
names = list(map( lambda posix_path: posix_path.name, list(joint_values_dict.keys())))
# Create no names for joint values
new_names = list(map( lambda name: "shifted_joint_values_of_{}".format(name), names))
# Read poses from world view
reference_pose = world_view_client.get_pose("Reference",
"/{}/poses".format(world_view_folder))
shift_pose = world_view_client.get_pose("Shift",
loop.run_until_complete(move_group.move_joints_collision_free(home))
# For every pose we want to address, do a pick and place
for i in range(len(pre_place_jvs)):
print("Placing element {}".format(i + 1))
loop.run_until_complete(place(pre_place_jvs[i], place_jvs[i]))
finally:
loop.close()
if __name__ == '__main__':
# Called when running this script standalone
world_view_folder = "example_02_palletizing/example_pick_place_poses"
move_group = example_utils.get_move_group()
world_view_client = WorldViewClient()
# Read poses from world view
pose_1 = world_view_client.get_pose("Pose_1", world_view_folder)
pose_2 = world_view_client.get_pose("Pose_2", world_view_folder)
pose_3 = world_view_client.get_pose("Pose_3", world_view_folder)
# Read poses from world view
joint_values_1 = world_view_client.get_joint_values(
"JointValues_1", world_view_folder)
joint_values_2 = world_view_client.get_joint_values(
"JointValues_2", world_view_folder)
joint_values_3 = world_view_client.get_joint_values(
"JointValues_3", world_view_folder)
home = world_view_client.get_joint_values("Home", world_view_folder)
end_effector = example_utils.get_move_group().get_end_effector()
poses = [pose_1, pose_2, pose_3]
Returns
------
CollisionObject
The grid of boxes as a CollisionObject instance
"""
createBoxOfPose = lambda pose: CollisionPrimitive.create_box(
size[0], size[1], size[2], pose)
return CollisionObject(list(map(createBoxOfPose, poses)))
if __name__ == '__main__':
# Called when running this script standalone
world_view_folder = "example_02_palletizing/example_create_collision_boxes"
world_view_client = WorldViewClient()
# Read poses from world view
poses_dict = world_view_client.query_poses(world_view_folder)
poses = list(poses_dict.values())
boxes = main(poses, (0.2, 0.2, 0.2))
# Save the generated collision boxes in world view
try:
# delete folder if it already exists
world_view_client.remove_element("generated", world_view_folder)
except Exception as e:
None
world_view_client.add_folder("generated", world_view_folder)
world_view_client.add_collision_object(
"collision_matrix", "/{}/generated".format(world_view_folder), boxes)
def main():
# create entities which should be added to and manipulated in world view
joint_set1 = JointSet(['Joint1', 'Joint2', 'Joint3'])
joint_values1 = JointValues(joint_set1, [1.0, 2.0, 3.0])
joint_set2 = JointSet(['Joint1', 'Joint2'])
joint_values2 = JointValues(joint_set2, [19.0, 20.0])
joint_set3 = JointSet(['Joint1', 'Joint4'])
joint_values3 = JointValues(joint_set3, [100.0, 30.0])
t1 = [0.502522, 0.2580, 0.3670]
q1 = Quaternion(w=0.304389, x=0.5272, y=0.68704, z=0.39666)
t2 = [0.23795, 0.46845, 0.44505]
q2 = Quaternion(w=0.212097, x=0.470916, y=0.720915, z=0.462096)
t3 = [0.6089578, 0.3406782, 0.208865]
q3 = Quaternion(w=0.231852, x=0.33222, y=0.746109, z=0.528387)
pose1 = Pose(t1, q1, 'world')
pose2 = Pose(t2, q2, 'world')
pose3 = Pose(t3, q3, 'world')
cartesian_path1 = CartesianPath([pose1, pose2, pose3])
sphere = CollisionPrimitive.create_sphere(0.1, pose2)
cylinder = CollisionPrimitive.create_cylinder(0.4, 0.05, pose2)
box = CollisionPrimitive.create_box(0.2, 0.2, 0.1, pose1)
plane = CollisionPrimitive.create_plane(1.0, 1.0, 1.0, 1.0, pose3)
cone = CollisionPrimitive.create_cone(0.2, 0.2, pose3)
collision_object1 = CollisionObject([box])
collision_object2 = CollisionObject([sphere, cylinder])
collision_object3 = CollisionObject([plane, cone])
# create a instance of WorldViewClient to get access to rosvita world view
world_view_client = WorldViewClient()
print('---------------- add folder --------------')
# add folder test at WorldView root
world_view_client.add_folder('test', '')
# add folder test at WorldView root/test
world_view_client.add_folder('joint_values', '/test')
world_view_client.add_folder('poses', '/test')
world_view_client.add_folder('cartesian_paths', '/test')
world_view_client.add_folder('collision_objects', '/test')
print('---------------- add joint values --------------')
world_view_client.add_joint_values('joint_values1', 'test/joint_values',
joint_values1)
world_view_client.add_joint_values('joint_values2', 'test/joint_values',
joint_values2)
world_view_client.add_joint_values('joint_values3', 'test/joint_values',
joint_values3)
world_view_client.add_joint_values('test', 'test/joint_values',
joint_values3)
print('---------------- update joint values --------------')
world_view_client.update_joint_values('joint_values1', 'test/joint_values',
joint_values2)
world_view_client.update_joint_values('joint_values2', 'test/joint_values',
joint_values3)
world_view_client.update_joint_values('joint_values3', 'test/joint_values',
joint_values1)
print('---------------- get joint values --------------')
get_value = world_view_client.get_joint_values('joint_values1',
'test/joint_values')
print('joint_values1 is: ' + str(get_value))
print('---------------- query joint values --------------')
# query all values which start with t under test/joint_values
queried_values = world_view_client.query_joint_values(
'test/joint_values', 't')
print('---------------- add poses --------------')
world_view_client.add_pose('pose1', 'test/poses', pose1)
world_view_client.add_pose('pose2', 'test/poses', pose2)
world_view_client.add_pose('pose3', 'test/poses', pose3)
world_view_client.add_pose('test', 'test/poses', pose3)
print('---------------- update pose --------------')
world_view_client.update_pose('pose1', 'test/poses', pose2)
world_view_client.update_pose('pose2', 'test/poses', pose3)
world_view_client.update_pose('pose3', 'test/poses', pose1)
print('---------------- get pose --------------')
get_value = world_view_client.get_pose('pose1', 'test/poses')
print('pose1 is: ' + str(get_value))
print('---------------- query poses --------------')
# query all poses which start with t under test/pose
queried_values = world_view_client.query_poses('test/poses', 't')
print('---------------- add cartesian path --------------')
world_view_client.add_cartesian_path('cartesian_path1',
'test/cartesian_paths',
cartesian_path1)
world_view_client.add_cartesian_path('test1', 'test/cartesian_paths',
cartesian_path1)
print('---------------- update cartesian path --------------')
world_view_client.update_cartesian_path('cartesian_path1',
'test/cartesian_paths',
cartesian_path1)
print('---------------- get cartesian path --------------')
get_value = world_view_client.get_cartesian_path('cartesian_path1',
'test/cartesian_paths')
print('cartesian_path1 is: ' + str(get_value))
print('---------------- query cartesian paths --------------')
# query all cartesian_paths which start with t under test/cartesian_path
queried_values = world_view_client.query_cartesian_paths(
'test/cartesian_paths', 't')
for v in queried_values:
print(str(v))
print('---------------- add collision object --------------')
world_view_client.add_collision_object('collision_object1',
'test/collision_objects',
collision_object1)
world_view_client.add_collision_object('collision_object2',
'test/collision_objects',
collision_object2)
world_view_client.add_collision_object('collision_object3',
'test/collision_objects',
collision_object3)
world_view_client.add_collision_object('test1', 'test/collision_objects',
collision_object1)
print('---------------- update collision object --------------')
world_view_client.update_collision_object('collision_object1',
'test/collision_objects',
collision_object1)
print('---------------- get collision object --------------')
get_value = world_view_client.get_collision_object(
'collision_object1', 'test/collision_objects')
print('collision_object1 is: ' + str(get_value))
print('---------------- query collision object --------------')
# query all collision_objects which start with t under test/collision_object
queried_values = world_view_client.query_collision_objects(
'test/collision_objects', 't')
for v in queried_values:
print(str(v))
input('Press enter to clean up')
print('----------------- remove folder test ---------------')
world_view_client.remove_element('test', '')
def main():
world_view_client = WorldViewClient()
input_var = input("Please type in the torch filename (with path) of the sensorhead stereo camera calibration (e.g. \"calibration/torso_cameras/stereo_cams_<serial1>_<serial2>.t7\" ): ")
cam_calib_fn = str(input_var)
stereocalib_sensorhead_cams = torchfile.load(cam_calib_fn)
end_of_right_serial = cam_calib_fn.rfind(".")
start_of_right_serial = cam_calib_fn.rfind("_")
right_serial = cam_calib_fn[start_of_right_serial+1:end_of_right_serial]
end_of_left_serial = start_of_right_serial
start_of_left_serial = cam_calib_fn.rfind("_", 0, end_of_left_serial)
left_serial = cam_calib_fn[start_of_left_serial+1:end_of_left_serial]
print("right_serial:")
print(right_serial)
print("left_serial:")
print(left_serial)
print("Please type in the exposure time of the cameras [in microseconds],")
input_var = input("or press \'enter\' to use 120000ms:")
if input_var == "" :
exposure_time = 120000
else :
exposure_time = int(input_var)
print("exposure_time:")
print(exposure_time)
print(type(exposure_time))
move_group_names = MotionService.query_available_move_groups()
move_group_name = move_group_names[2].name # left_arm_torso
flange_link_name = move_group_names[2].end_effector_link_names[0] # arm_left_link_tool0
print("For which arm will the tooltip be calibrated? Please type l or r, then press \'Enter\'.")
print("l: left arm")
print("r: right arm")
which = sys.stdin.read(1)
if which == "r" :
move_group_name = move_group_names[3].name # right_arm_torso
flange_link_name = move_group_names[3].end_effector_link_names[0] # arm_right_link_tool0
print("move_group_name:")
print(move_group_name)
move_group = MoveGroup(move_group_name)
print("flange_link_name")
print(flange_link_name)
torso_link_name = "torso_link_b1"
print("torso_link_name:")
print(torso_link_name)
last_slash = cam_calib_fn.rfind("/")
torso_to_cam_fn = cam_calib_fn[:last_slash] + "/LeftCam_torso_joint_b1.t7"
print("Please type in the name of the torchfile (with path) containing the transformation between torso joint and torso cameras,")
input_var = input("or press \'enter\' to use " + torso_to_cam_fn)
if input_var != "" :
torso_to_cam_fn = str(input_var)
print("torso_to_cam_fn:")
print(torso_to_cam_fn)
torso_to_cam_t7 = torchfile.load(torso_to_cam_fn)
torso_to_cam = Pose.from_transformation_matrix(matrix=torso_to_cam_t7, frame_id=torso_link_name, normalize_rotation=False)
pattern_localizer = patternlocalisation.PatternLocalisation()
pattern_localizer.circleFinderParams.minArea = 50
pattern_localizer.circleFinderParams.maxArea = 1000
pattern_localizer.setPatternData(8, 11, 0.003)
pattern_localizer.setStereoCalibration(stereocalib_sensorhead_cams)
#pattern_localizer.setFindCirclesGridFlag(cv.CALIB_CB_ASYMMETRIC_GRID) # Don't use "cv.CALIB_CB_CLUSTERING", because it's much more sensitive to background clutter.
pattern_localizer.setFindCirclesGridFlag(cv.CALIB_CB_ASYMMETRIC_GRID + cv.CALIB_CB_CLUSTERING)
world_view_folder = '/Calibration'
storage_folder = '/tmp/calibration/storage_tooltipcalib'
offline = False
tooltip_calib = TooltipCalibration(pattern_localizer, stereocalib_sensorhead_cams, left_serial, right_serial, exposure_time,
world_view_client, world_view_folder, move_group, torso_to_cam, storage_folder, torso_link_name,
flange_link_name, offline)
tooltip_calib.runTooltipCalib()
def main(loopCount: int):
# create move group instance targeting the right arm of the robot
move_group = example_utils.get_move_group()
# get the gripper attached at the right arm
wsg_gripper = example_utils.get_gripper(move_group)
# get a client to communicate with the robot
robot_chat = RobotChatClient()
# create a instance of WorldViewClient to get access to rosvita world view
world_view_client = WorldViewClient()
# get the example joint values from world view
jv_home = world_view_client.get_joint_values("Home",
"example_01_pick_place")
jv_prePick = world_view_client.get_joint_values("01_PrePick",
"example_01_pick_place")
jv_pick = world_view_client.get_joint_values("02_Pick",
"example_01_pick_place")
jv_prePlace = world_view_client.get_joint_values("03_PrePlace",
"example_01_pick_place")
jv_place = world_view_client.get_joint_values("04_Place",
"example_01_pick_place")
loop = asyncio.get_event_loop()
# register the loop handler to be shutdown appropriately
register_asyncio_shutdown_handler(loop)
async def move_supervised(joint_values: JointValues, velocity_scaling=1):
"""Opens a window in rosvita to let the user supervise the motion to joint values """
stepped_client = move_group.move_joints_collision_free_supervised(
joint_values, velocity_scaling=velocity_scaling)
robot_chat_stepped_motion = RobotChatSteppedMotion(
robot_chat, stepped_client, move_group.name)
await robot_chat_stepped_motion.handle_stepwise_motions()
async def prepare_gripper():
print("Home the gripper and move to joints")
# Allows parallel homing and supervised movement
t1 = wsg_gripper.homing()
t2 = move_supervised(jv_home)
await asyncio.gather(t1, t2)
async def go_to_prepick():
print("Go to prepick position and open gripper")
# Allows parallel adjustment of the gripper and movement of the end effector
t1 = move_group.move_joints(jv_prePick, velocity_scaling=0.5)
t2 = wsg_gripper.grasp(0.02, 1, 0.05)
await asyncio.gather(t1, t2)
async def pick_up():
"""Simple example of a "picking up" motion """
print("Pick up")
await move_group.move_joints(jv_pick, velocity_scaling=0.04)
await wsg_gripper.grasp(0.002, 0.1, 0.05)
await move_group.move_joints(jv_prePick)
async def go_to_preplace():
print("Go to preplace position")
await move_group.move_joints(jv_prePlace)
async def place():
"""Simple example of a "placing" motion """
print("Place")
await move_group.move_joints(jv_place, velocity_scaling=0.04)
await wsg_gripper.grasp(0.04, 1, 0.05)
await move_group.move_joints(jv_prePlace)
async def pick_and_place():
"""Actions to be repeated in loop"""
await go_to_prepick()
await pick_up()
await go_to_preplace()
await place()
async def return_home():
print("Return home")
await move_group.move_joints_collision_free(jv_home)
try:
# plan a trajectory to the begin pose
loop.run_until_complete(prepare_gripper())
for i in range(loopCount):
loop.run_until_complete(pick_and_place())
loop.run_until_complete(return_home())
finally:
loop.close()
def main(xSize: int, ySize: int, xStepSize: float , yStepSize: float):
"""
The parameter for this function describes the size of the grid.
To manipulate orientation and position, one can alter the Pose named "GridPose"
in the folder "example_02_palletizing" of the world view.
Parameters
----------
xSize : int
Number of elements of the grid in x-direction
ySize : int
Number of elements of the grid in y-direction
xStepSize : float
The distance between the poses in x-direction
yStepSize : float
The distance between the poses in y-direction
"""
world_view_folder = "example_02_palletizing"
# Create move group instance targeting the right arm of the robot
move_group = example_utils.get_move_group()
# Get the gripper attached at the right arm
wsg_gripper = example_utils.get_gripper(move_group)
# Create a instance of WorldViewClient to get access to rosvita world view
world_view_client = WorldViewClient()
# Generate the folders used by this example in world vew
generate_folders(world_view_client)
# Get the pose of the position which defines the location and rotation of the grid
pose = world_view_client.get_pose("GridPose", world_view_folder)
jv_home = world_view_client.get_joint_values("Home", world_view_folder)
# Get the target place poses
poses = example_02_1_generate_grid.main(pose, xSize, ySize, xStepSize, yStepSize)
rotation = pose.quaternion
# Calculate the orthogonal vector of the plane we want to span
# Since we use [1,0,0] and [0,1,0] vectors to span the grid relatively to the
# pose orientation, [0,0,1] is orthogonal to the grid
orthogonal = rotation.rotate(np.array([0,0,1]))
# For visualization and possible collisions, add some boxes below the positions
# we want to visit
getBoxPose = lambda pose : Pose(pose.translation + (orthogonal * (0.02)), pose.quaternion)
boxPoses = list(map(getBoxPose, poses))
boxes = example_02_2_create_collision_boxes.main(boxPoses, (xStepSize*0.9, yStepSize*0.9, 0.01))
world_view_client.add_collision_object("collision_matrix",
"/{}/generated/collision_objects".format(world_view_folder),
boxes)
# Now calculate the pre place poses, which hover over the desired place poses
# Since we want to access every pose in the grid "from above", we apply a
# translation orthogonal to the place poses for the pre place poses
#func = lambda pose : Pose(pose.translation + (orthogonal * (-0.1)), pose.quaternion)
func = lambda pose : Pose(pose.translation + (pose.quaternion.rotate(np.array([0,0,1])) * (-0.1)), pose.quaternion)
pre_place_poses = list(map( func , poses))
# Now that we have all the poses we want to visit, we should find the
# corresponding joint values
pre_place_jvs = calculate_pre_place_joint_values(pre_place_poses,
jv_home,
move_group,
world_view_client,
world_view_folder)
example_02_4_pick_place_poses.main(poses, pre_place_jvs, jv_home, move_group)
world_view_client.remove_element("generated", world_view_folder)
if not ik_results.succeeded:
raise Exception(
"The inverse kinematics operation could not be applied on all the positions."
)
# joint_path now contains the result of the ik-operation
return ik_results.path
if __name__ == '__main__':
# Called when running this script standalone
world_view_folder = "example_02_palletizing/example_create_jv_from_poses"
move_group = example_utils.get_move_group()
end_effector = example_utils.get_move_group().get_end_effector()
world_view_client = WorldViewClient()
seed = world_view_client.get_joint_values("Seed", world_view_folder)
# Read poses from world view
poses_dict = world_view_client.query_poses(world_view_folder)
# Read names too to associate the joint values to the poses
names = list(
map(lambda posix_path: posix_path.name, list(poses_dict.keys())))
# Create no names for joint values
new_names = list(map(lambda name: "joint_values_of_{}".format(name),
names))
poses = list(poses_dict.values())
joint_values = main(poses, seed, end_effector)
# Save the generated joint value in world view
try:
def main():
world_view_folder = "example_04_collision_objects"
world_view_client = WorldViewClient()
move_group = example_utils.get_move_group()
loop = asyncio.get_event_loop()
# register the loop handler to be shutdown appropriately
register_asyncio_shutdown_handler(loop)
# Read joint values for begin and end configuration
jv_start = world_view_client.get_joint_values("Start", world_view_folder)
jv_end = world_view_client.get_joint_values("End", world_view_folder)
# Read poses from world view
poses_dict = world_view_client.query_poses(
"{}/poses".format(world_view_folder))
# Read names too to associate the joint values to the poses
poses_names = list(
map(lambda posix_path: posix_path.name, list(poses_dict.keys())))
# Create no names for the collision objects
collision_names = list(
map(lambda name: "collision_obect_of_{}".format(name), poses_names))
# Get poses of dictionary
poses = list(poses_dict.values())
add_generated_folder(world_view_client, world_view_folder)
# Store collision box for every positionin collision_objects
collision_objects = {}
for i in range(len(poses)):
pose = poses[i]
name = collision_names[i]
box = CollisionPrimitive.create_box(0.15, 0.15, 0.15, pose)
coll_obj = CollisionObject([box])
collision_objects[name] = coll_obj
async def move_back_and_forth(jv_start: JointValues, jv_end: JointValues):
"""
Moves back and forth between jv_start and jv_end, evading collision objects
Parameters
----------
jv_start : JointValues
Start configuration
jv_end : JointValues
End configuration
"""
print("Moving to start position")
# First, move to start joint values
await move_group.move_joints_collision_free(jv_start,
velocity_scaling=0.5)
# Now activate one collision object and observe how the robot finds a way
# TODO: Update code when python api support activating/deactivating of collision objects
for name in collision_names:
print("Moving around obstacle \"{}\".".format(name))
# activate "name"
coll_obj = collision_objects[name]
# Add element to world view
world_view_client.add_collision_object(
name, "/{}/generated".format(world_view_folder), coll_obj)
test = world_view_client.get_collision_object(
name, "/{}/generated".format(world_view_folder))
# move to end location
await move_group.move_joints_collision_free(jv_end,
velocity_scaling=0.5)
# Remove element from world view
world_view_client.remove_element(
name, "/{}/generated".format(world_view_folder))
# switch start and end
jv_temp = jv_end
jv_end = jv_start
jv_start = jv_temp
try:
loop.run_until_complete(move_back_and_forth(jv_start, jv_end))
finally:
loop.close()
# clean up
world_view_client.remove_element("generated", world_view_folder)