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 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 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 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
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)
reference_pose = world_view_client.get_pose("Reference",
"/{}/poses".format(world_view_folder))
shift_pose = world_view_client.get_pose("Shift",
"/{}/poses".format(world_view_folder))
# Create a pose defining the shift from the reference_poses view
# First undo translation and rotation of reference pose
# then translate and rotate by the shift pose
diff_pose = Pose( -reference_pose.translation, reference_pose.quaternion.inverse)\
.translate(shift_pose.translation).rotate(shift_pose.quaternion)
shifted_joint_values_list = main(joint_values_list, diff_pose)
# Save the shifted joint values to world view
print("Save the shifted joint values to 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)
for i in range(len(shifted_joint_values_list)):
joint_values = shifted_joint_values_list[i]
# Test if not "None"
if joint_values:
name = new_names[i]
world_view_client.add_joint_values(name,
"/{}/generated".format(world_view_folder),
joint_values)
input("Press enter to clean up")
world_view_client.remove_element("generated", world_view_folder)
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(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)
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)