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():
# 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():
world_view_client = WorldViewClient()
input_var = input("Please type in the torch filename (with path) of the sensorhead stereo camera calibration (e.g. \"calibration/torso_cameras/stereo_cams_<serial1>_<serial2>.t7\" ): ")
cam_calib_fn = str(input_var)
stereocalib_sensorhead_cams = torchfile.load(cam_calib_fn)
end_of_right_serial = cam_calib_fn.rfind(".")
start_of_right_serial = cam_calib_fn.rfind("_")
right_serial = cam_calib_fn[start_of_right_serial+1:end_of_right_serial]
end_of_left_serial = start_of_right_serial
start_of_left_serial = cam_calib_fn.rfind("_", 0, end_of_left_serial)
left_serial = cam_calib_fn[start_of_left_serial+1:end_of_left_serial]
print("right_serial:")
print(right_serial)
print("left_serial:")
print(left_serial)
print("Please type in the exposure time of the cameras [in microseconds],")
input_var = input("or press \'enter\' to use 120000ms:")
if input_var == "" :
exposure_time = 120000
else :
exposure_time = int(input_var)
print("exposure_time:")
print(exposure_time)
print(type(exposure_time))
move_group_names = MotionService.query_available_move_groups()
move_group_name = move_group_names[2].name # left_arm_torso
flange_link_name = move_group_names[2].end_effector_link_names[0] # arm_left_link_tool0
print("For which arm will the tooltip be calibrated? Please type l or r, then press \'Enter\'.")
print("l: left arm")
print("r: right arm")
which = sys.stdin.read(1)
if which == "r" :
move_group_name = move_group_names[3].name # right_arm_torso
flange_link_name = move_group_names[3].end_effector_link_names[0] # arm_right_link_tool0
print("move_group_name:")
print(move_group_name)
move_group = MoveGroup(move_group_name)
print("flange_link_name")
print(flange_link_name)
torso_link_name = "torso_link_b1"
print("torso_link_name:")
print(torso_link_name)
last_slash = cam_calib_fn.rfind("/")
torso_to_cam_fn = cam_calib_fn[:last_slash] + "/LeftCam_torso_joint_b1.t7"
print("Please type in the name of the torchfile (with path) containing the transformation between torso joint and torso cameras,")
input_var = input("or press \'enter\' to use " + torso_to_cam_fn)
if input_var != "" :
torso_to_cam_fn = str(input_var)
print("torso_to_cam_fn:")
print(torso_to_cam_fn)
torso_to_cam_t7 = torchfile.load(torso_to_cam_fn)
torso_to_cam = Pose.from_transformation_matrix(matrix=torso_to_cam_t7, frame_id=torso_link_name, normalize_rotation=False)
pattern_localizer = patternlocalisation.PatternLocalisation()
pattern_localizer.circleFinderParams.minArea = 50
pattern_localizer.circleFinderParams.maxArea = 1000
pattern_localizer.setPatternData(8, 11, 0.003)
pattern_localizer.setStereoCalibration(stereocalib_sensorhead_cams)
#pattern_localizer.setFindCirclesGridFlag(cv.CALIB_CB_ASYMMETRIC_GRID) # Don't use "cv.CALIB_CB_CLUSTERING", because it's much more sensitive to background clutter.
pattern_localizer.setFindCirclesGridFlag(cv.CALIB_CB_ASYMMETRIC_GRID + cv.CALIB_CB_CLUSTERING)
world_view_folder = '/Calibration'
storage_folder = '/tmp/calibration/storage_tooltipcalib'
offline = False
tooltip_calib = TooltipCalibration(pattern_localizer, stereocalib_sensorhead_cams, left_serial, right_serial, exposure_time,
world_view_client, world_view_folder, move_group, torso_to_cam, storage_folder, torso_link_name,
flange_link_name, offline)
tooltip_calib.runTooltipCalib()
def get_move_group() -> MoveGroup:
return MoveGroup(get_move_group_name())
def get_full_body_move_group() -> MoveGroup:
return MoveGroup("/sda10f")
def get_left_move_group() -> MoveGroup:
return MoveGroup("/sda10f/sda10f_r1_controller")
def get_right_move_group() -> MoveGroup:
return MoveGroup("/sda10f/sda10f_r2_controller")
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 -------------------')
await move_group.move_joints(joint_path)
print('---------------- move joints collision free -------------------')
await move_group.move_joints_collision_free(joint_path)
print('---------------- move joints supervised -------------------')
stepped_motion_client = move_group.move_joints_supervised(
joint_path, 0.5)
await run_supervised(stepped_motion_client)
print('----------------move joints collision free supervised -------------------')
stepped_motion_client = move_group.move_joints_collision_free_supervised(
joint_path, 0.5)
await run_supervised(stepped_motion_client)
print('test EndEffector class')
print('---------------- move poses -------------------')
await end_effector.move_poses(cartesian_path)
print('---------------- move poses collision free -------------------')
await end_effector.move_poses_collision_free(cartesian_path)
print('---------------- move poses supervised -------------------')
stepped_motion_client = end_effector.move_poses_supervised(
cartesian_path, None, 0.5)
await run_supervised(stepped_motion_client)
print('---------------- move poses collision free supervised -------------------')
stepped_motion_client = end_effector.move_poses_collision_free_supervised(
cartesian_path, None, 0.5)
await run_supervised(stepped_motion_client)
try:
loop.run_until_complete(example_moves())
finally:
loop.close()