多设备草稿

https://robotics.shanghaitech.edu.cn/static/videos/Shakey.mp4

Xxxhttp://ai.stanford.edu/~nilsson/OnlinePubs-Nils/shakey-the-robot.pdfXhttps://fast.lycorisrecoil.org/link/TlEb5MfZpX8zX4Uu?sub=1

[-1.24243437 -1.13685393  1.06958234  2.16102957  1.43905544  0.23775425]
[-1.42120683 -1.34875954  0.67697859  2.16054917  1.43905544  0.23775425]
[-1.49603092 -0.98652715  1.29149795  2.16056752  1.43905544  0.23775425]
[-1.34960318 -0.91121518  1.25618589  2.16056752  1.43905544  0.23775425]
[-1.29492402 -1.10603511  1.0407809   2.16056394  1.43905544  0.23775425]
[-1.46189618 -1.10507417  1.06474424  2.16056394  1.43905544  0.23775425]

https://mp.weixin.qq.com/s?__biz=MzU1NjEwMTY0Mw==&mid=2247555708&idx=2&sn=55b2fbc07a213d22897026f6fc484ab9&chksm=fbc86318ccbfea0e12e6f9a5e04b17cc9ad63b62965bc3ef8e99aa24ece0913e1450b39d8bcc&cur_album_id=2441283576110350340&scene=190#rd

#!/usr/bin/env python

#GitHub - Khrylx/PyTorch-RL: PyTorch implementation of Deep Reinforcement Learning: Policy Gradient methods (TRPO, PPO, A2C) and Generative Adversarial Imitation Learning (GAIL). Fast Fisher vector product TRPO.
# -*- coding: utf-8 -*-

import rospy, sys
import moveit_commander
from control_msgs.msg import GripperCommand

class MoveItFkDemo:
    def __init__(self):
        # 初始化move_group的API
        moveit_commander.roscpp_initialize(sys.argv)

        # 初始化ROS节点
        rospy.init_node('moveit_fk_demo', anonymous=True)
 
        # 初始化需要使用move group控制的机械臂中的arm group
        i3 = moveit_commander.MoveGroupCommander('manipulator_i3')
        
        # 初始化需要使用move group控制的机械臂中的gripper group
        #gripper = moveit_commander.MoveGroupCommander('hand_group')
        
        # 设置机械臂和夹爪的允许误差值
        i3.set_goal_joint_tolerance(0.001)
        #gripper.set_goal_joint_tolerance(0.001)
        
        # 控制机械臂先回到初始化位置
        #i3.set_named_target('home')
        #i3.go()
        #rospy.sleep(2)
         
      
        # 设置机械臂的目标位置，使用六轴的位置数据进行描述（单位：弧度）
        joint_positions = [0.8,0,0,0,0,0.8]
        i3.set_joint_value_target(joint_positions)
                 
        # 控制机械臂完成运动
        i3.go()
        rospy.sleep(0.1)

        joint_positions = [0.8,0.8,0,0,0,0.8]
        i3.set_joint_value_target(joint_positions)

        i3.go()
        rospy.sleep(0.1)

        
        # 设置夹爪的目标位置，并控制夹爪运动
        #gripper.set_joint_value_target([0.03, -0.03])
        #gripper.go()
        #rospy.sleep(1)
         
        # 关闭并退出moveit
        moveit_commander.roscpp_shutdown()
        moveit_commander.os._exit(0)

if __name__ == "__main__":
    try:
        MoveItFkDemo()
    except rospy.ROSInterruptException:
        pass