joint_position_control.py

# Copyright (c) Facebook, Inc. and its affiliates.

# This source code is licensed under the MIT license found in the
# LICENSE file in the root directory of this source tree.

"""
Example for commanding robot with position control
"""

import time

from pyrobot import Robot


def main():
    target_joints = [
        [0.704, -0.455, -0.159, 1.395, -1.240, 1.069, 2.477],
        [-0.341, -0.384, -0.018, 1.533, -0.977, -1.492, -1.084]
    ]

    bot = Robot('sawyer',
                use_arm=True,
                use_base=False,
                use_camera=False,
                use_gripper=True)
    bot.arm.go_home()

    time.sleep(1)
    for joint in target_joints:
        bot.arm.set_joint_positions(joint, plan=False)
        time.sleep(1)

    bot.arm.go_home()


if __name__ == "__main__":
    main()