def main( joint_values_list: List[JointValues], diff_pose: Pose) -> List[JointValues]:
"""
This function takes a translation vector and a list of joint values and tries
to find corresponding joint values shifted by "diff_pose" in Cartesian Space
Parameters
----------
joint_values_list: List[JointValues]
A list of joint values to be shifted
diff_pose: Pose
The pose which defines translation an rotation
Returns
----------
List[JointValues]
A list shifted JointValues for every joint value in joint_values_list
that could be shifted
"""
move_group = example_utils.get_move_group()
print("Shift joint values.")
shifted_jvs_list = []
for joint_values in joint_values_list:
try:
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
async def main():
move_group = example_utils.get_move_group()
end_effector = move_group.get_end_effector()
pose = end_effector.get_current_pose()
# Create a cartesian path of the same poses
path = []
for i in range(100):
path.append(pose)
cartesian_path = CartesianPath(path)
# This applies a sinus trajectory an amplitude of 5 mm with 10 periods
# over the path on the y axis
target = add_sin_to_trajectory(cartesian_path=cartesian_path,
amplitude=0.005,
frequency=1 / 10,
axis=1)
# This applies a sinus trajectory an amplitude of 2 mm with 100 periods
# over the path on the z axis
target = add_sin_to_trajectory(target,
amplitude=0.002,
frequency=1 / 100,
axis=2)
# Move the end effector according to the now sinus shaped path
await end_effector.move_poses_linear(target, max_deviation=0.05)
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 get_one_to_many_trajectory_cache() -> TaskTrajectoryCache:
world_view_client = WorldViewClient()
move_group = example_utils.get_move_group()
end_effector = move_group.get_end_effector()
world_view_folder = "example_08_trajectory_cache"
# Get start and end from world view
start_jv = world_view_client.get_joint_values("start_jv",
world_view_folder)
start_pose = end_effector.compute_pose(start_jv)
end_pose = world_view_client.get_pose("end_pose", world_view_folder)
# Get a SampleBox, which contains a Volume defined by translations and rotations
end_box = get_sample_box(end_pose)
# Use a cash to store the calculated trajectories
cache = Cache("trajectory_cache", "example_08")
cache.load()
key = "one_to_many"
# First, show a one_to_one
# Check if the key exists already in the cache
trajectory_cache = cache.get(key)
if trajectory_cache is None:
print(
"Create the trajectory cache, since it has not been serialized yet"
)
trajectory_cache = create_trajectory_cache(
set_robot_service_name="/sda10f/xamlaSda10fController/set_state",
end_effector=end_effector,
seed=start_jv,
start=start_pose,
target=end_box)
print("Serialize the calculated trajectories")
cache.add(key, trajectory_cache)
cache.dump()
else:
print("Data already serialized.")
return trajectory_cache
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)
try:
# Move to home position
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)
try:
# Move to home position
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], poses[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)
poses = [pose_1, pose_2, pose_3]
joint_values = [joint_values_1, joint_values_2, joint_values_3]
main(poses, joint_values, home, move_group)
def test_cached_trajectories(tc_go_to: TaskTrajectoryCache,
tc_come_back: TaskTrajectoryCache):
world_view_client = WorldViewClient()
move_group = example_utils.get_move_group()
end_effector = move_group.get_end_effector()
world_view_folder = "example_08_trajectory_cache"
add_generated_folder(world_view_client, world_view_folder)
# Get start and end from world view
start_jv = world_view_client.get_joint_values("start_jv",
world_view_folder)
start_pose = end_effector.compute_pose(start_jv)
end_pose = world_view_client.get_pose("end_pose", world_view_folder)
# Get a SampleBox, which contains a Volume defined by translations and rotations
end_box = get_sample_box(end_pose)
# Show the grid in world view
sample_positions = end_box.sample_positions
for i in range(len(sample_positions[0])):
translation = [
sample_positions[0][i], sample_positions[1][i],
sample_positions[2][i]
]
pose = Pose.from_transformation_matrix(np.eye(4))
pose = pose.translate(translation)
pose = pose.rotate(end_box.quaternions[0])
# poses contains a list of poses with same translation, different rotation
world_view_client.add_pose(element_name="pose_{}".format(i),
folder_path=world_view_folder +
"/generated",
pose=pose)
# Move to start_jv
loop.run_until_complete(move_group.move_joints_collision_free(start_jv))
if tc_go_to is not None and tc_come_back is not None:
world_view_client.add_pose(element_name="target_pose",
folder_path=world_view_folder +
"/generated",
pose=end_pose)
input(
"Please move 'target_pose' in world view in folder 'example_08_trajectory_cache/generated', then press enter."
)
target_pose = world_view_client.get_pose(
element_name="target_pose",
folder_path=world_view_folder + "/generated")
try:
# This movement uses the trajectory which ends nearest to target_pose
print("Move to nearest pose")
execution = move_with_trajectory_cache(
cache=tc_go_to,
end_effector=end_effector,
start_joint_values=start_jv,
target_pose=target_pose,
max_position_diff_radius=0.05)
loop.run_until_complete(execution)
# This movement uses the trajectory which begins nearest to target_pose
print("Return home")
execution = move_with_trajectory_cache(
cache=tc_come_back,
end_effector=end_effector,
start_joint_values=None,
target_pose=start_pose,
max_position_diff_radius=0.05)
loop.run_until_complete(execution)
except RuntimeError as e:
print(e)
print(
"The chosen pose must lie in the radius of one of the poses in the used SampleVolume "
)
else:
print("Could not open serialized data. ")
input("Press enter to clean up")
world_view_client.remove_element("generated", world_view_folder)
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)
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)