forked from facebookresearch/pyrobot
-
Notifications
You must be signed in to change notification settings - Fork 7
/
ee_pose_control.py
46 lines (36 loc) · 1.09 KB
/
ee_pose_control.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
# 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 to specific gripper pose
"""
import time
import numpy as np
from pyrobot import Robot
def main():
# Example poses
# orientation can be either rotation matrix or quaternion
target_poses = [
{
"position": np.array([0.279, 0.176, 0.217]),
"orientation": np.array(
[
[0.5380200, -0.6650449, 0.5179283],
[0.4758410, 0.7467951, 0.4646209],
[-0.6957800, -0.0035238, 0.7182463],
]
),
},
{
"position": np.array([0.339, 0.0116, 0.255]),
"orientation": np.array([0.245, 0.613, -0.202, 0.723]),
},
]
bot = Robot("locobot", use_base=False)
bot.arm.go_home()
for pose in target_poses:
bot.arm.set_ee_pose(**pose)
time.sleep(1)
bot.arm.go_home()
if __name__ == "__main__":
main()