def main():
ioloop = asyncio.get_event_loop()
register_asyncio_shutdown_handler(ioloop)
# create instance of movegroup to provide motion services
move_group = MoveGroup()
try:
print('---------- wsg gripper -------------')
# create instance of wsg gripper by name
properties = WeissWsgGripperProperties('wsg50')
wsg_gripper = WeissWsgGripper(properties, move_group.motion_service)
print('homeing')
ioloop.run_until_complete(wsg_gripper.homing())
print('move gripper')
ioloop.run_until_complete(wsg_gripper.move(0.1, 0.05, 0.05, True))
print('perform grasp')
result = ioloop.run_until_complete(wsg_gripper.grasp(0.02, 0.1, 0.05))
print('---------- common gripper -------------')
# create instance of common gripper by name and driver rosnode name
properties1 = CommonGripperProperties('wsg50', 'xamla/wsg_driver')
gripper = CommonGripper(properties1, move_group.motion_service)
print('move')
result1 = ioloop.run_until_complete(gripper.move(0.1, 0.005))
finally:
ioloop.close()
def main(poses: List[Pose], pre_place_jvs: List[JointValues],
home: JointValues):
"""
This function does a pick and place motion to every pose
Parameters
----------
poses : List[Pose]
Defines the place poses to be addressed
pre_place_jvs : List[JointValues]
Defines the joint values of the pre place positions
home : JointValues
The joint values, to which the robot returns after place
"""
loop = asyncio.get_event_loop()
# Register the loop handler to be shutdown appropriately
register_asyncio_shutdown_handler(loop)
place_jvs = calculate_place_joint_values(poses, pre_place_jvs, move_group,
world_view_client)
async def place(jv_pre_place: JointValues, jv_place: JointValues) -> None:
"""
This asynchronous function lets the robot move from home to every place position and
back in a "pick and place" manner
Parameters
----------
jv_pre_place : JointValues
The pre place configuration
jv_place : JointValues
The place configuration
"""
# Creates a joint path over the joint values to the target pose
joint_path = JointPath(home.joint_set, [home, jv_pre_place, jv_place])
# Move over the joints to target pose
await move_group.move_joints_collision_free(joint_path)
# do something, e.g. place an object
# Creates a joint path over the joint values to the home pose
joint_path_back = JointPath(home.joint_set,
[jv_place, jv_pre_place, home])
# Move back over the joint path
await move_group.move_joints_collision_free(joint_path_back)
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()
def main():
# create move group instance
move_group = MoveGroup()
# get default endeffector of the movegroup
end_effector = move_group.get_end_effector()
# create cartesian path and equivalent joint path
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)
pose_1 = Pose(t1, q1)
pose_2 = Pose(t2, q2)
pose_3 = Pose(t3, q3)
cartesian_path = CartesianPath([pose_1, pose_2, pose_3])
plan = end_effector.move_cartesian(cartesian_path,
collision_check=True).plan()
stepped_client = plan.execute_supervised()
robot_chat = RobotChatClient()
robot_chat_stepped_motion = RobotChatSteppedMotion(robot_chat,
stepped_client,
move_group.name)
loop = asyncio.get_event_loop()
register_asyncio_shutdown_handler(loop)
try:
loop.run_until_complete(
robot_chat_stepped_motion.handle_stepwise_motions())
finally:
loop.close()
"Move Robot to viewpoint %d, and press Enter to continue..." % i)
poses.append(end_effector.get_current_pose())
joint_values.append(move_group.get_current_joint_positions())
path = {
'poses': CartesianPath(poses),
'joint_values': JointPath(joint_values[0].joint_set, joint_values)
}
print("successfully created the new path")
return path
if __name__ == '__main__':
# create move group instance
move_group = MoveGroup()
# get default endeffector of the movegroup
end_effector = move_group.get_end_effector()
view_num = input("Enter the number of view points for the new path:")
path = new_path(move_group, end_effector, view_num)
loop = asyncio.get_event_loop()
register_asyncio_shutdown_handler(loop)
try:
loop.run_until_complete(test_run(move_group, path, view_num))
finally:
loop.close()
def main(poses: List[Pose], pre_place_jvs: List[JointValues], home: JointValues, move_group : MoveGroup) :
"""
This function does a pick and place motion to every pose
Parameters
----------
poses : List[Pose]
Defines the place poses to be addressed
pre_place_jvs : List[JointValues]
Defines the joint values of the pre place positions
home : JointValues
The joint values, to which the robot returns after place
move_group : MoveGroup
"""
loop = asyncio.get_event_loop()
# Register the loop handler to be shutdown appropriately
register_asyncio_shutdown_handler(loop)
async def place(jv_pre_place: JointValues, place: Pose) -> None:
"""
This asynchronous function lets the robot move from home to every place position and
back in a "pick and place" manner
The movement from pre place JointValues to place Pose and back is done by moving the end
effector in a linear fashion in cartesian space.
The movement is planned an executed on the fly.
Parameters
----------
jv_pre_place : JointValues
The pre place configuration
jv_place : JointValues
The place configuration
"""
end_effector = move_group.get_end_effector()
# Creates a joint path over the joint values to the target pose
joint_path = JointPath(home.joint_set, [home, jv_pre_place ])
# Move over the joints to target pose
await move_group.move_joints_collision_free(joint_path)
pre_place_pose = end_effector.compute_pose(jv_pre_place)
await end_effector.move_poses_linear(CartesianPath([pre_place_pose, place]),
velocity_scaling = 0.05)
# do something, e.g. place an object
await end_effector.move_poses_linear(CartesianPath([place, pre_place_pose]),
velocity_scaling = 0.05)
# Creates a joint path over the joint values to the home pose
joint_path_back = JointPath(home.joint_set, [jv_pre_place, home ])
# Move back over the joint path
await move_group.move_joints_collision_free(joint_path_back)
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()
def main():
# create move group instance
move_group = MoveGroup()
# get default endeffector of the movegroup
end_effector = move_group.get_end_effector()
# create cartesian path and equivalent joint path
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)
pose_1 = Pose(t1, q1)
pose_2 = Pose(t2, q2)
pose_3 = Pose(t3, q3)
cartesian_path = CartesianPath([pose_1, pose_2, pose_3])
joint_path = end_effector.inverse_kinematics_many(cartesian_path,
False).path
loop = asyncio.get_event_loop()
register_asyncio_shutdown_handler(loop)
async def example_moves():
print('test MoveGroup class')
print('---------------- move joints -------------------')
move_joints = move_group.move_joints(joint_path)
move_joints = move_joints.with_velocity_scaling(0.1)
move_joints_plan = move_joints.plan()
await move_joints_plan.execute_async()
print('---------------- move joints supervised -------------------')
move_joints = move_joints.with_velocity_scaling(0.5)
stepped_motion_client = move_joints.plan().execute_supervised()
await run_supervised(stepped_motion_client)
print('---------------- move joints collision free -------------------')
move_joints_cf = MoveJointsCollisionFreeOperation(
move_joints.to_args())
await move_joints_cf.plan().execute_async()
print('test EndEffector class')
print('---------------- move cartesian -------------------')
move_cartesian = end_effector.move_cartesian(cartesian_path)
move_cartesian = move_cartesian.with_velocity_scaling(0.4)
move_cartesian_plan = move_cartesian.plan()
await move_cartesian_plan.execute_async()
print('---------------- move cartesian supervised -------------------')
move_cartesian = move_cartesian.with_velocity_scaling(0.8)
stepped_motion_client = move_cartesian.plan().execute_supervised()
await run_supervised(stepped_motion_client)
print('---------------- move cartesian collision free -------------------')
move_cartesian_cf = MoveCartesianCollisionFreeOperation(
move_cartesian.to_args()
)
await move_cartesian_cf.plan().execute_async()
try:
loop.run_until_complete(example_moves())
finally:
loop.close()
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():
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)
