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 setup_class(cls):
cls.client = WorldViewClient()
joint_set = JointSet('1,2,3')
cls.joint_values_1 = JointValues(joint_set, 0.0)
cls.joint_values_2 = JointValues(joint_set, [1, 2, 3])
cls.pose_1 = Pose.identity()
cls.pose_2 = Pose.identity().translate([0.1, 0.2, 1.8])
cls.pose_3 = Pose.identity().translate([0.1, -0.2, 1.8])
cls.cartesian_path_1 = CartesianPath([cls.pose_1, cls.pose_2])
cls.cartesian_path_2 = CartesianPath([cls.pose_2, cls.pose_1])
collision_box_1 = CollisionPrimitive.create_box(
0.05,
0.05,
0.05,
cls.pose_1
)
collision_box_2 = CollisionPrimitive.create_box(
0.05,
0.05,
0.05,
cls.pose_2
)
collision_box_3 = CollisionPrimitive.create_box(
0.05,
0.05,
0.05,
cls.pose_3
)
cls.collision_object_1 = CollisionObject([collision_box_1])
cls.collision_object_2 = CollisionObject(
[collision_box_2, collision_box_3])
cls.folder_path = 'test/my_test'
cls.joint_values_path = 'test_joint_values'
cls.pose_path = 'test_pose'
cls.cartesian_path = 'test_cartesian_path'
cls.collision_object_path = 'test_collision_object'
async def imitate_move(end_effector, move_group):
input(
"Press enter to set the current position as reference start point, and start to listen to poses from client"
)
start = end_effector.get_current_pose()
with socket.socket(socket.AF_INET, socket.SOCK_STREAM) as s:
s.bind((HOST, PORT))
s.listen()
while True:
conn = None
try:
conn, addr = s.accept()
with conn:
while True:
data = conn.recv(1024)
if not data:
break
pose_list = eval(data.decode("utf-8"))
# print(pose_list)
path = []
for pose in pose_list:
coordinate = pose[:3]
# The following line should be changed according to current pose and the frame of robot base
coordinate[0], coordinate[1], coordinate[
2] = coordinate[2], coordinate[0], coordinate[
1]
coordinate = np.array(
coordinate) + start.translation
quat_element = pose[3:7]
# The following line should be changed to match the previous line of coordinate
quat = Quaternion(quat_element[0], quat_element[3],
quat_element[1], quat_element[2])
quat = quat * start.quaternion
pose = Pose(coordinate, quat)
path.append(pose)
cartesian_path = CartesianPath(path)
joint_path = end_effector.inverse_kinematics_many(
cartesian_path, False).path
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()
except KeyboardInterrupt:
if conn:
conn.close()
break
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)
def new_path(move_group, end_effector, view_num):
joint_values = []
poses = []
for i in range(int(view_num)):
inst = input(
"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
def main(poses: List[Pose],
seed: JointValues,
end_effector: EndEffector) \
-> List[JointValues]:
"""
Calculates joint values from poses and a seed
Calling inverse_kinematics_many with "const_seed = True" lets us
exclusively using the "seed" joint values as seed.
Parameters
----------
poses : List[Pose]
A list of poses for which the joint values should be calculated
seed : JointValues
The seed to be used
end_effector: EndEffector
Returns
------
List[JointValues]
A list of joint values for every pose
Exceptions
------
Exception
Raised when there was one or more poses for which the inverse kinematics
operation failed
"""
cartesian_path = CartesianPath(poses)
ik_results = end_effector.inverse_kinematics_many(cartesian_path,
collision_check=True,
seed=seed,
const_seed=True)
# Check if a configuration has been found for every pose
if not ik_results.succeeded:
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
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()
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([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/test
world_view_client.add_folder('test/joint_values')
world_view_client.add_folder('test/poses')
world_view_client.add_folder('test/cartesian_paths')
world_view_client.add_folder('test/collision_objects')
print('---------------- add joint values --------------')
world_view_client.add_joint_values(
'test/joint_values/joint_values1',
joint_values1
)
world_view_client.add_joint_values(
'test/joint_values/test',
joint_values2
)
print('---------------- update joint values --------------')
world_view_client.add_joint_values(
'test/joint_values/joint_values1',
joint_values2
)
print('---------------- get joint values --------------')
get_value = world_view_client.get_joint_values(
'test/joint_values/joint_values1'
)
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'
)
for v in queried_values:
print(str(v))
print('---------------- add poses --------------')
world_view_client.add_pose(
'test/poses/pose1',
pose1
)
world_view_client.add_pose(
'test/poses/test',
pose3
)
print('---------------- update pose --------------')
world_view_client.add_pose(
'test/poses/pose1',
pose2
)
print('---------------- get pose --------------')
get_value = world_view_client.get_pose(
'test/poses/pose1'
)
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')
for v in queried_values:
print(str(v))
print('---------------- add cartesian path --------------')
world_view_client.add_cartesian_path(
'test/cartesian_paths/cartesian_path1',
cartesian_path1
)
world_view_client.add_cartesian_path(
'test/cartesian_paths/test',
cartesian_path1
)
print('---------------- update cartesian path --------------')
world_view_client.add_cartesian_path(
'test/cartesian_paths/cartesian_path1',
cartesian_path1
)
print('---------------- get cartesian path --------------')
get_value = world_view_client.get_cartesian_path(
'test/cartesian_paths/cartesian_path1'
)
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(
'test/collision_objects/collision_object1',
collision_object1
)
world_view_client.add_collision_object(
'test/collision_objects/test',
collision_object1
)
print('---------------- update collision object --------------')
world_view_client.add_collision_object(
'test/collision_objects/collision_object1',
collision_object2
)
print('---------------- get collision object --------------')
get_value = world_view_client.get_collision_object(
'test/collision_objects/collision_object1'
)
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', '')
async def test_run(move_group, path, view_num):
new_path = path
while 1:
command = input("Enter command:")
if command == "e":
print("--- moving to the start point ---")
move_joints_cf = move_group.move_joints_collision_free(
new_path['joint_values'][0], velocity_scaling=0.2)
await move_joints_cf.plan().execute_async()
print('start test run')
temp_path = new_path['joint_values']
move_joints_cf = move_group.move_joints_collision_free(temp_path)
move_joints_cf = move_joints_cf.with_velocity_scaling(0.2)
await move_joints_cf.plan().execute_async()
print('test run finished')
elif command.isdigit():
num = int(command)
if num < int(view_num):
view_points = new_path['joint_values']
print('--- Going to Viewpoint %d---' % num)
move_joints_cf = move_group.move_joints_collision_free(
view_points[num], velocity_scaling=0.2)
await move_joints_cf.plan().execute_async()
else:
print('There are %s viewpoints, please enter a valid number' %
view_num)
elif command == 's':
name = input("Enter the name of the path: ")
if not name:
name = "default"
file_path = open('paths/%s.obj' % name, 'wb')
pickle.dump(new_path, file_path)
print('saved as %s.obj' % name)
break
elif command == 'l':
name = input("Enter the name of the path you want to load: ")
if not name:
name = "default"
file_path = open('paths/%s.obj' % name, 'r+b')
move_group = MoveGroup()
end_effector = move_group.get_end_effector()
path = pickle.load(file_path)
joint_values = []
poses = []
for i in range(len(path['poses'])):
poses.append(path['poses'][i])
joint_values.append(path['joint_values'][i])
pos = input(
"Enter the name of the position you want to overwrite: ")
input(
"Move Robot to the new position, and press Enter to continue.")
poses[int(pos)] = end_effector.get_current_pose()
joint_values[int(pos)] = move_group.get_current_joint_positions()
print(path)
path2 = {
'poses': CartesianPath(poses),
'joint_values': JointPath(joint_values[0].joint_set,
joint_values)
}
print(path2)
print("successfully created the new path")
pickle.dump(path2, file_path)
print('saved as %s.obj' % name)
break
elif command == 'd':
break
else:
print("""usage: - e: execute a test run of path
- number: go to specified viewpoint
- s: save the path to local directory
- d: discard the path""")
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 add_sin_to_trajectory(cartesian_path: CartesianPath, amplitude: float,
frequency: float, axis: int) -> CartesianPath:
"""
Applies a translation to every pose in the cartesian path in form of a sinus shape
Parameters
----------
cartesian_path : CartesianPath
The CartesianPath object to be altered
amplitude : float
The amplitude of the sinus function in mm
(the maximal deviationfrom the pose)
frequency : float
The frequency of the sinus function
(how long a period takes relatively to the path length)
axis: int
The axis on which the translation should be applied
Returns
------
CartesianPath
The cartesian_path altered by a sinus shaped translation
"""
if axis < 0 or axis > 2:
msg = 'axis can be x=0, y=1, z=2 but has value: {}'.format(axis)
raise ValueError(msg)
def sin_generator(number_of_samples: int, amplitude: float,
frequency: float):
"""
This generator calculates values for an index according to a sinus
function defined by the parameters.
It yields the current index and the corresponding sinus value
Parameters
----------
number_of_samples : int
The number of samples
amplitude : float
The amplitude of the sinus function in mm
frequency : float
The frequency of the sinus function
(how long a period takes relatively to the path length)
axis: int
The axis on which the translation should be applied
"""
for sample_index in range(number_of_samples):
# Calculate the angle at index sample_index
angle = 1 / frequency * (sample_index * 360.0) / number_of_samples
# Yield a tuple of index and the sinus value of the angle, multiplied by the amplitude
yield sample_index, amplitude * math.sin(math.radians(angle))
new_path = []
new_translation = np.asarray([0.0, 0.0, 0.0])
number_of_samples = len(cartesian_path)
# For every pose, get the translation value of the sinus shaped movement
for i, s in sin_generator(number_of_samples, amplitude, frequency):
# Apply it only to one axis
new_translation[axis] = s
# Apply the translation to corresponding pose
new_pose = cartesian_path.points[i].translate(new_translation)
new_path.append(new_pose)
return CartesianPath(new_path)