# 4C2_control_head

**Repository Path**: xu--classmate/4-c2_control_head

## Basic Information

- **Project Name**: 4C2_control_head
- **Description**: No description available
- **Primary Language**: Unknown
- **License**: Not specified
- **Default Branch**: master
- **Homepage**: None
- **GVP Project**: No

## Statistics

- **Stars**: 0
- **Forks**: 0
- **Created**: 2025-03-15
- **Last Updated**: 2025-06-29

## Categories & Tags

**Categories**: Uncategorized

**Tags**: None

## README

# 机械臂项目仓库(眼在头上)

## 1. Branch
- master   主分支
- grasp_dev 测试分支（忽略）
## 2. 项目说明


## 3.启动方式
0. 准备
```C
catkin_make
source ./devel/setup.sh
```

1. 启动rviz、gazebo联合仿真
```C
roslaunch rm_gazebo rm_bringup_moveit.launch
```
2. 新建一个窗口进入scipts文件夹下面
```C
source ./devel/setup.sh
roscd rm_control/scripts/
```

3. 运行py脚本

```c
python grasp.py
```

## 4.文件夹
1. gazebo-pkgs

    gazebo负责夹爪吸附插件

2. realsense_ros_gazebo

    官网d435功能包

3. rm_control

    控制部分的代码，scripts里面为py代码

4. rm_gazebo

    gazebo仿真环境

5. rm_ligting_robot

    urdf、xacro等描述文件

6. rm_models

    gazebo里面需要用到的模型文件

7. rm_moveit_confit

    moveit生成包

8. roboticsgroup_gazebo_plugins

    mini夹具插件

9. yolov5_ros

    这里yoloV5的代码

10. yolov5_ros_msgs

    yolov5通讯功能包

## 5.追加说明（10个入参均在bsp包下面封装）

data：2025/6/29

- bsp_agv.py为agv控制部分

```python
#测试demo
from bsp_agv import CmdVelPublisher

def main():
    pub = CmdVelPublisher(rate_hz=10)   # 10 Hz 循环发布
    pub.send(0.5,0.0, once=False)	#入参为线速度、角速度

if __name__ == "__main__":
    main()
```

- bsp_control.py这里为机械臂和夹爪控制部分共计8个控制量

```python
#测试demo
from bsp_control import AllController

def main():
    ctrl = AllController()                      # 节点 & 发布器只建一次
    #入参依次为升降臂，joint1~6和夹具角度，最后一个参数为控制时间用作频率使用
    ctrl.move([0.5, -1, 1, 0, 0, 0, 0, 0.35], 0.5)   # 随时调用
    
if __name__ == "__main__":
    main()
```

![image-20250629160147980](image/image-20250629160147980.png)

- bsp_get_target.py获取目标的xzy坐标

```python
from MPC import (
    MobileManipulatorController,
    TargetObject,
    TaskState,
    validate_system_parameters,
    test_forward_kinematics,
    create_test_scenario
)
import rospy
import numpy as np
from bsp_agv import CmdVelPublisher
from bsp_get_target import ObjectDetector
from bsp_control import AllController

def main():
    rospy.init_node('object_position_client')
    detector = ObjectDetector()
    while not rospy.is_shutdown():
        obj_class, position = detector.get_object_position(timeout=5.0)
        if position:
            rospy.loginfo(f"Detected {obj_class} at position: {position}")
            break
            # 在这里使用位置信息...
        else:
            rospy.loginfo("No object detected, waiting...")
            rospy.sleep(1)
    # 1. 验证系统参数
    validate_system_parameters()
    
    # 2. 测试正运动学
    test_forward_kinematics()
    
    # 3. 创建控制器并添加自定义目标
    controller = MobileManipulatorController()
    
    # 创建目标物体
    target= TargetObject(
        position=position,
        orientation=np.array([0.0, 0.0, 0.1]),
        confidence=0.95,
        size=np.array([0.07, 0.07, 0.15]),
        detected_from="fixed_camera"
    )
    
    controller.target_objects = target
    
    # 设置初始任务状态
    controller.mpc.task_state = TaskState.SEARCHING
    
    # 4. 运行控制循环
    controller.run_control_loop(max_iterations=400)
    
    # 5. 使用预定义的测试场景
    print("\n运行测试场景...")
    test_controller = create_test_scenario()
    test_controller.run_control_loop(max_iterations=200)

if __name__ == "__main__":
    main()
```

![image-20250629161702980](image/image-20250629161702980.png)

![image-20250629161759375](image/image-20250629161759375.png)