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)
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]
joint_values = [joint_values_1, joint_values_2, joint_values_3]
main(poses, joint_values, home)
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(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)
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:
# delete folder if it already exists
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)