joint的拖拽旋转

课程目标

• 拖拽旋转joint

1-拖拽变换的原理

先回顾一下机器人模型的结构。

在做拖拽变换时，我们要知道以下规则：

• URDF 模型是树状结构，无闭环。
• 在URDF 树中，根节点是link，其子节点是按照一个joint、一个link的方式延伸的。
• joint 无实物，link 有实物。

由于joint是没有实物的，所以只能通过其子对象-link 监听鼠标事件，在鼠标拖拽link对象的时候，对其父对象的joint 进行变换。

2-选择link图形的方法

link图形可以解析为mesh 对象。

mesh 对象的选择是一个老生常谈的话题，有基础的同学可以略过。

2-1-核心原理

1. 计算鼠标的标准化设备坐标位：将鼠标点击的像素坐标（屏幕坐标）转换为 webgl的标准化设备坐标（NDC）（范围：-1 到 1）。
2. 计算鼠标的世界坐标位：将标准化设备坐标转入相机所处的坐标系，即用相机的视图投影矩阵的逆矩阵乘以标准化设备坐标。
3. 创建射线：以相机的世界坐标位为起点、鼠标的世界坐标位为方向，创建射线。
4. 射线检测：判断射线是否与mesh 对象中的任意一个三角面相交，若相交，则表示此mesh 对象被选中。

注：相机的视图矩阵本质上是相机的世界模型矩阵的逆矩阵。

视图投影矩阵 = 相机的投影矩阵 * 相机的视图矩阵
相机的视图矩阵 = 相机的世界模型矩阵的逆矩阵

2-2-three.js 中的代码示例

假设canvas 画布充满了整个窗口，我们要选择scene 场景中的所有物体。

// 创建场景
const scene = new THREE.Scene();

// 创建相机
const camera = new THREE.PerspectiveCamera(75, window.innerWidth / window.innerHeight, 0.1, 1000);
camera.position.z = 5;

// 定义射线检测器和鼠标坐标变量
const raycaster = new THREE.Raycaster();
const mouse = new THREE.Vector2();

// 鼠标点击事件处理
function onMouseClick(event) {
  // 1. 将鼠标像素坐标转换为标准化设备坐标
  // x: 从左到右 -1 到 1；y: 从上到下 -1 到 1（注意y轴反转）
  mouse.x = (event.clientX / window.innerWidth) * 2 - 1;
  mouse.y = - (event.clientY / window.innerHeight) * 2 + 1;

  // 2. 更新射线在世界坐标系里的起点和方向
  raycaster.setFromCamera(mouse, camera);

  // 3. 检测射线与场景中物体的相交情况
  // 参数1：要检测的物体数组
  // 参数2：是否检测后代（默认为true）
  const intersects = raycaster.intersectObjects(scene.children, true);

  // 4. 处理相交结果
  if (intersects.length > 0) {
    // 第一个相交的物体（最前面的物体）  
    const target = intersects[0].object; 
    //……
  } 
}

// 鼠标点击事件
window.addEventListener('click', onMouseClick);

3-根据选中的物体定位其所在的link和joint

在URDF中，一个link可能对应一个Mesh 对象，比如.stl 模型，也可能对应多个Mesh对象组成的Group，比如.dae 模型。

所以我们需要根据Mesh 对象的parent 属性追踪其父级，然后根据我们在解析URDF时已经封装好的URDFLink对象的isURDFLink 属性和URDFJoint对象的isURDFJoint 属性，来判断其父级是link 还是joint。

代码示例

// 根据子元素，向上层寻找可变换的URDFJoint
function findNearestJoint(child: Object3D): URDFJoint | undefined {
  return findParent(child,({userData:{isURDFJoint,type}})=>isURDFJoint&&type !='fixed') as URDFJoint
}

// 根据子元素，向上层寻找URDFLink
function findNearestLink(child: Object3D): URDFLink | undefined {
  return findParent(child,({userData:{isURDFLink}})=>isURDFLink) as URDFLink
}

// 需找符合特定条件的父级
function findParent(child: Object3D,fn:(obj:Object3D)=>boolean){
  let obj:Object3D|null  = child ;
  while (obj) {
    if (fn(obj)) {
      return obj;
    }
    obj = obj.parent;
  }
}

当我们找到joint 后，就可以根据鼠标拖拽数据，对其进行变换。

4-joint 的拖拽旋转算法

在世界坐标系中，已知：

• 鼠标点击在模型上的点为点A
• 鼠标拖拽后的点为点B
• 模型的基点是点O
• 模型的旋转轴是单位向量u
• 相机的位置是点E

求：将模型从点A拖拽到点B时，模型的相对旋转量θ

解：

1.计算点A在旋转轴上的投影点P：

P=O+(OA·u)*u

2.根据点A 和旋转轴u 可以确定一个唯一的平面plane，此平面过点A，法线方向为u。

3.根据射线与平面的交点公式，计算射线EB与plane 的交点C。

4.计算PA和PC之间的夹角θ。此时算出的θ是无符号的，即|θ|。

PA·PC=|PA|*|PC|*cosθ
cosθ=(PA·PC)/(|PA|*|PC|) 
unsignθ=acos((PA·PC)/(|PA|*|PC|))

5.计算以PA、PC和u 为基向量的坐标系的行列式，此行列式的正负符号可以说明绕u轴旋转的方向，即θ的正负符号。

determinant=(PA^PC)·u //^是叉乘符号
s=determinant/|determinant|
θ=s*unsignθ

算出θ 值后，将相应joint 对象的旋转值加上θ 即可。

5-joint拖拽旋转的整体代码实现

为了方便管理，我们可以创建一个专门用于拖拽变换的URDFDragControls 类。

• src/robot/URDFDragControls.ts

import {
    Color,
    EventDispatcher,
    type Intersection,
    Material,
    Mesh,
    MeshStandardMaterial,
    Object3D,
    type Object3DEventMap,
    OrthographicCamera,
    PerspectiveCamera,
    Plane,
    Raycaster,
    Vector3,
} from 'three'
import { URDFJoint, URDFLink, URDFRobot } from './URDFClasses'
import {
    findLinkVisual,
    findNearestJoint,
    findNearestLink,
    screenToNDC,
} from './utils'
import { ResourceTracker } from './ResourceTracker'

type CameraType = OrthographicCamera | PerspectiveCamera
type HoverType = Intersection & {
    link: URDFLink
    joint?: URDFJoint
    // joint 变换基点
    origin?: Vector3
    // joint 变换轴
    pivot?: Vector3
    // joint 变换平面,旋转平面或推拉平面
    tfPlane?: Plane
}

/* URDF 模型拖拽类 */
class URDFDragControls extends EventDispatcher<any> {
    // 相机
    camera: CameraType
    // dom
    domElement: HTMLElement
    // 拖拽对象集合
    robots: URDFRobot[]
    // 鼠标划上的对象数据
    curHover: HoverType | undefined
    // 当前鼠标拖拽的关节的变换值
    curJointValue: number = 0
    // 鼠标每次移动时的起始位置和结束位置(世界坐标系)
    dragStart = new Vector3()
    dragEnd = new Vector3()
    // 是否正在拖拽
    dragging = false
    // 是否启用拖拽变换
    enabled = true
    // 资源清理器
    resourceTracker = new ResourceTracker()

    constructor(
        camera: CameraType,
        domElement: HTMLElement,
        robots: URDFRobot | URDFRobot[] = [],
    ) {
        super()
        this.camera = camera
        this.domElement = domElement
        this.robots = robots instanceof Array ? robots : [robots]
        // 鼠标事件
        this.pointerdown = this.pointerdown.bind(this)
        this.pointermove = this.pointermove.bind(this)
        this.pointerup = this.pointerup.bind(this)
        this.listen()
    }
    // 添加拖拽对象
    add(robot: URDFRobot) {
        this.robots.push(robot)
    }
    // 监听事件
    listen() {
        const { domElement } = this
        domElement.addEventListener('pointerdown', this.pointerdown)
        domElement.addEventListener('pointermove', this.pointermove)
        domElement.addEventListener('pointerup', this.pointerup)
    }

    // 鼠标移动时
    pointermove({ pageX, pageY }: PointerEvent) {
        const {
            enabled,
            curHover,
            dragEnd,
            dragStart,
            domElement: canvas,
            camera,
        } = this
        if (!enabled) {
            return
        }
        // 鼠标的屏幕坐标
        const { left, top } = canvas.getBoundingClientRect()
        const [cx, cy] = [pageX - left, pageY - top]
        // 鼠标的DNC 坐标
        const NDCPos = screenToNDC({ x: cx, y: cy }, canvas)
        // 从视点到鼠标点的射线
        const raycaster = new Raycaster()
        raycaster.setFromCamera(NDCPos, camera)
        // 瞬时变换量
        let delta: number = 0
        if (this.dragging) {
            // 若有关节处于拖拽状态
            if (!curHover) {
                return
            }
            const { origin, pivot, joint, tfPlane } = curHover
            if (!pivot || !joint || !tfPlane) {
                return
            }
            const {
                userData: { type },
            } = joint
            const oldValue = this.curJointValue
            // 计算从视点到鼠标点的射线在变换平面上的交点
            raycaster.ray.intersectPlane(tfPlane, dragEnd)
            if (!dragEnd) {
                return
            }
            if (type == 'prismatic') {
                // 推拉关节
                // 推拉终点到推拉起点的向量
                const startToEnd = dragEnd.clone().sub(dragStart)
                // startToEnd 在推拉轴上的正射影,即瞬时推拉量
                delta = startToEnd.dot(pivot)
            } else {
                // 旋转关节
                if (!origin) {
                    return
                }
                // 构建origin 基点到拖拽起点和终点的向量
                const originToStart = new Vector3().subVectors(dragStart, origin)
                const originToEnd = new Vector3().subVectors(dragEnd, origin)
                // 根据行列式获取绕轴旋转的正负方向
                const direction = Math.sign(
                    originToStart.clone().cross(originToEnd).dot(pivot),
                )
                // 瞬时旋转弧度
                delta = direction * originToEnd.angleTo(originToStart)
            }
            // 若瞬时变换量为0，则说明没有变换
            if (!delta) {
                return
            }
            // 累积瞬时变换量
            this.curJointValue += delta
            // 变换关节
            joint.setValue(this.curJointValue)
            // 触发changeValue事件
            this.dispatchEvent({
                type: 'changeValue',
                joint,
                value: this.curJointValue,
                oldValue,
            })
            // 更新拖拽起点
            dragStart.copy(dragEnd)
        } else {
            // 若没有关节处于拖拽状态，选择模型
            let intersections = this.intersectRobots(raycaster)
            // 若选中模型
            if (intersections.length) {
                // 离视点最近的模型数据
                const hoverData = intersections[0]
        if(hoverData==undefined){return}
                // 根据当前模型寻找其父级link
                const link = findNearestLink(hoverData.object)
                if (!link) {
                    return
                }
                //离link最近的可变换关节
                const joint = findNearestJoint(link)
                if (!this.curHover) {
                    // 若之前没有被物体鼠标划上，高亮link
                    this.highlightMaterial(link)
                    // 触发鼠标划入机器人事件
                    this.dispatchEvent({
                        type: 'pointerOverRobot',
                        link,
                        joint,
                        x: cx,
                        y: cy,
                    })
                } else if (this.curHover.object != hoverData.object) {
                    // 否则，若鼠标之前划上的物体与当前鼠标划上的物体不是同一个
                    // 重置关节材质
                    this.restoreCurHoverMat()
                    // 高亮link
                    this.highlightMaterial(link)
                }
                // 存储鼠标划上的对象数据
                this.curHover = { ...hoverData, link: link, joint }
                // 触发鼠标在机器人上移动的事件
                this.dispatchEvent({
                    type: 'pointerMoveOnRobot',
                    link,
                    joint,
                    x: cx,
                    y: cy,
                })
                // 更新鼠标样式
                canvas.style.cursor = 'pointer'
            } else if (curHover) {
                this.dispatchEvent({ type: 'pointerOutRobot' })
                // 若鼠标没有选中物体，且之前有物体被选中
                // 重置此物体所在link的材质
                this.restoreCurHoverMat()
                // 清理curHover数据
                this.curHover = undefined
                // 更新鼠标样式
                canvas.style.cursor = 'default'
            }
        }
    }

    // 射线选中的的Mesh
    intersectRobots(raycaster: Raycaster) {
        const { robots } = this
        let temp: Intersection<Object3D<Object3DEventMap>>[] = []
        for (let robot of robots) {
            const {
                userData: { linkMap },
            } = robot
            if (!linkMap) {
                continue
            }
            linkMap.forEach((link) => {
                findLinkVisual(link, (obj) => {
                    const intersections = raycaster.intersectObject(obj, false)
          if(intersections[0]){
            if(temp[0]){
              if(intersections[0].distance<temp[0].distance){
                temp = intersections
              }
            }else{
              temp = intersections
            }
          }
                })
            })
        }
        return temp
    }

    // 鼠标按下时
    pointerdown({ button }: PointerEvent) {
        // 只适配左击
        if (button !== 0) {
            return
        }
        const { curHover, enabled, camera } = this
        if (!enabled || !curHover || !curHover.joint) {
            return
        }
        // 确保相机的视图投影矩阵为最新的
        camera.updateMatrixWorld()
        camera.updateProjectionMatrix()

        const { point, joint } = curHover
        const {
            matrixWorld,
            matrixWorld: { elements },
            userData: { type, value, axis },
        } = joint
        // 记录当前关节的变换量
        this.curJointValue = value
        // 拖拽起始位置，即鼠标射线与变换平面的交点
        this.dragStart.copy(point)
        // 变换轴在世界坐标系里的方向
        const pivot = axis.clone().transformDirection(matrixWorld)
        curHover.pivot = pivot
        // 拖拽状态
        this.dragging = true
        // 关节的世界坐标位
        const jointWorldPos = new Vector3(elements[12], elements[13], elements[14])
        // 交点point在pivot上的正交投影(世界坐标系)，即joint的旋转的基点
        const origin = point
            .clone()
            .sub(jointWorldPos)
            .projectOnVector(pivot)
            .add(jointWorldPos)
        // 存储变换基点
        curHover.origin = origin

        // 变换平面，用于旋转和推拉点位的计算
        const tfPlane = new Plane()
        if (type == 'prismatic') {
            // 构建推拉平面
            const v1 = new Vector3()
                .subVectors(camera.position, point)
                .normalize()
                .cross(pivot)
            const v2 = pivot.clone().cross(v1)
            tfPlane.setFromNormalAndCoplanarPoint(v2, point)
        } else {
            // 构建旋转平面
            tfPlane.setFromNormalAndCoplanarPoint(pivot, point)
        }
        // 存储变换平面
        curHover.tfPlane = tfPlane
        // 触发开始拖拽事件
        this.dispatchEvent({ type: 'beginDrag' })
    }
    // 鼠标抬起时
    pointerup({ button }: PointerEvent) {
        // 只适配左击
        if (button !== 0) {
            return
        }
        const { dragging, enabled } = this
        if (!dragging || !enabled) {
            return
        }
        this.dragging = false
        // 触发结束拖拽事件
        this.dispatchEvent({ type: 'finishDrag' })
    }
    // 高亮关节
    highlightMaterial(link: URDFLink) {
        // 遍历关节中的Mesh对象
        this.traverseMeshInLink(link, (mesh) => {
            const material = mesh.material as Material
            // 暂存当前材质
            mesh.userData.material = material
            // 基于当前材质高亮
            const matClone = material.clone() as MeshStandardMaterial
            matClone.emissive = new Color(0x00acec)
            matClone.emissiveIntensity = 0.3
            mesh.material = matClone
        })
    }
    // 重置当前材质
    restoreCurHoverMat() {
        const { curHover } = this
        if (!curHover || !curHover.link) {
            return
        }
        this.traverseMeshInLink(curHover.link, (mesh) => {
            const { material } = mesh
            'dispose' in material && material.dispose()
            mesh.material = mesh.userData.material
            mesh.userData.material = undefined
        })
    }

    // 在link内遍历mesh
    traverseMeshInLink(obj: Object3D, callback: (mesh: Mesh) => void) {
        const {
            userData: { isURDFJoint, isURDFHelper },
        } = obj
        if (isURDFJoint || isURDFHelper) {
            return
        }
        if (obj instanceof Mesh) {
            callback(obj)
        }
        obj.children.forEach((child) => {
            this.traverseMeshInLink(child, callback)
        })
    }

    // 清理内存
    dispose() {
        const { domElement } = this
        this.resourceTracker.dispose()
        domElement.removeEventListener('pointerdown', this.pointerdown)
        domElement.removeEventListener('pointermove', this.pointermove)
        domElement.removeEventListener('pointerup', this.pointerup)
    }
}

export { URDFDragControls }

我们可以根据鼠标事件理解其整体的代码逻辑。

6-在RobotVisual类中实例化URDFDragControls

在RobotVisual类中，需要将其camera、domElement和robot 传给URDFDragControls类的实例，使URDFDragControls 实例通过监听domElement 的鼠标事件，拖拽变换robot 中的关节。

代码如下

• src/robot/RobotVisual.ts

import {
    Color,
    DirectionalLight,
    EquirectangularReflectionMapping,
    EventDispatcher,
    Fog,
    GridHelper,
    Mesh,
    PerspectiveCamera,
    PlaneGeometry,
    Scene,
    WebGLRenderer,
    OrthographicCamera,
    MeshBasicMaterial,
    ShadowMaterial,
    LoadingManager,
    Box3,
} from 'three'
import { HDRLoader } from 'three/examples/jsm/loaders/HDRLoader.js'
import { OrbitControls } from 'three/examples/jsm/controls/OrbitControls.js'
import { ResourceTracker } from './ResourceTracker'
import { URDFRobot } from './URDFClasses'
import { URDFLoader } from './URDFLoader'
import { halfPI } from './utils'
import { URDFDragControls } from './URDFDragControls'

/* 机器可视化类 */
class RobotVisual extends EventDispatcher<any> {
    renderer = new WebGLRenderer({
        antialias: true,
        logarithmicDepthBuffer: true,
    })
    scene = new Scene()
    camera: OrthographicCamera | PerspectiveCamera = new PerspectiveCamera(
        45,
        1,
        0.01,
        200,
    )
    orbitControls = new OrbitControls(this.camera, this.renderer.domElement)
    continuousFrame = 0
    resourceTracker = new ResourceTracker()
    urdfDragControls = new URDFDragControls(this.camera, this.renderer.domElement)

    constructor(hdrURL?: string) {
        super()
        const { renderer, scene, resourceTracker, orbitControls, camera } = this
        // 适应屏幕分辨率
        renderer.setPixelRatio(window.devicePixelRatio)
        // 投影
        renderer.shadowMap.enabled = true
        // 场景背景色
        scene.background = new Color(0xf6f6f8)
        // 场景雾效
        scene.fog = new Fog(0xf6f6f8, 20, 50)

        // 环境光
        hdrURL &&
            new HDRLoader().loadAsync(hdrURL).then((texture) => {
                texture.mapping = EquirectangularReflectionMapping
                scene.environment = texture
                resourceTracker.track(texture)
            })
        // 灯光
        const light = new DirectionalLight(0xffffff, 1)
        light.position.set(0, 10, 5)
        light.castShadow = true
        scene.add(light)
        resourceTracker.track(light)

        // 地面网格
        const floorGrid = new GridHelper(100, 100, 0x9c9aa5, 0xbcbac7)
        scene.add(floorGrid)
        resourceTracker.track(floorGrid)
        // 地面Geometry
        const floorGeometry = new PlaneGeometry(100, 100)
        // 地面阴影
        const floorShadowMaterial = new ShadowMaterial({
            transparent: true,
            opacity: 0.1,
        })
        const floorShadowMesh = new Mesh(floorGeometry, floorShadowMaterial)
        floorShadowMesh.rotateX(-Math.PI / 2)
        floorShadowMesh.receiveShadow = true
        scene.add(floorShadowMesh)
        resourceTracker.track(floorShadowMesh)
        // 地面
        const floorMaterial = new MeshBasicMaterial({
            color: 0xffffff,
            transparent: true,
            opacity: 0.5,
        })
        const floorMesh = new Mesh(floorGeometry, floorMaterial)
        floorMesh.rotateX(-Math.PI / 2)
        floorMesh.position.y = -0.005
        scene.add(floorMesh)
        resourceTracker.track(floorMesh)

        // 设置镜头
        camera.position.set(0, 1.2, 3.6)
        orbitControls.target.set(0, 0.8, 0)
        orbitControls.update()
    }
    // 加载URDF模型
    loadURDF(url: string, onLoad?: (robot: URDFRobot) => void) {
        const manager = new LoadingManager()
        const loader = new URDFLoader(manager)
        let robot: URDFRobot
        loader.load(url, (res: any) => {
            robot = res
        })
        manager.onLoad = () => {
            // 初始化模型
            this.initModel(robot)
            // 监听事件
            this.listen()
            onLoad && onLoad(robot)
        }
        manager.onError = () => {
            console.error('URDFLoader: Error loading model.')
        }
        return loader
    }
    // 初始化模型
    initModel(robot: URDFRobot) {
        const { scene, resourceTracker, urdfDragControls } = this
        // 使robot面朝z轴
        robot.rotation.set(-halfPI, 0, -halfPI)
        scene.add(robot)
        // 拖拽变换机器人
        urdfDragControls.add(robot)
        // 添加缓存清理机制
        resourceTracker.track(robot)
        // 模型落地
        this.toGround(robot)
    }
    // 使机器人的底部落地
    toGround(robot: URDFRobot) {
        const bb = new Box3()
        bb.setFromObject(robot)
        robot.position.y -= bb.min.y
    }
    // 监听事件
    listen() {
        const { urdfDragControls } = this

        /* 
          协调orbitControls和 urdfDragControls 的有效性
        */
        urdfDragControls.addEventListener('beginDrag', () => {
            this.orbitControls.enabled = false
        })
        urdfDragControls.addEventListener('finishDrag', () => {
            this.orbitControls.enabled = true
        })
        this.orbitControls.addEventListener('start', () => {
            !urdfDragControls.curHover && (urdfDragControls.enabled = false)
        })
        this.orbitControls.addEventListener('end', () => {
            urdfDragControls.enabled = true
        })
    }

    // 响应式布局
    resize(width: number, height: number) {
        const { renderer, camera } = this
        if (camera instanceof OrthographicCamera) {
            const halfWidth = (camera.top * width) / height
            camera.left = -halfWidth
            camera.right = halfWidth
        } else {
            camera.aspect = width / height
        }
        camera.updateProjectionMatrix()
        renderer.setSize(width, height, true)
    }

    // 渲染
    render() {
        const { renderer, scene, camera } = this
        renderer.render(scene, camera)
    }

    // 连续渲染
    continuousRender() {
        this.render()
        this.continuousFrame = requestAnimationFrame(
            this.continuousRender.bind(this),
        )
    }

    // 清理数据
    dispose() {
        this.resourceTracker.dispose()
        this.renderer.dispose()
        this.orbitControls.dispose()
        this.urdfDragControls.dispose()
        this.renderer.domElement.remove()
        cancelAnimationFrame(this.continuousFrame)
    }
}
export { RobotVisual }

7-拖拽旋转测试

我们可以运行项目测试拖拽旋转。

不过，我需要在旋转关节的同时，同步更新左侧的关节面板信息。

• src/App.vue

const robotVisual = new RobotVisual(hdrURL);
const { urdfDragControls } = robotVisual;
//……
/* 拖拽变换关节对象时，更新关节dom 的值 */
urdfDragControls.addEventListener("changeValue", ({joint, value}) => {
  formControl.setJointValue(value,joint.name)
});

总结

这一章我们说了joint 拖拽旋转的核心原理和代码实现，其中拖拽旋转算法是重点，其它的都是比简单的业务逻辑。

下一章我们会说一下joint 的拖拽推拉。