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 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
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', '')
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
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(joint_values)):
world_view_client.add_joint_values(
new_names[i], "{}/generated".format(world_view_folder),
joint_values[i])
input("Press enter to clean up")
world_view_client.remove_element("generated", world_view_folder)