joint的拖拽推拉

课程链接：www.bilibili.com/cheese/play…

代码链接：github.com/buglas/robo…

课程目标

• 拖拽推拉joint

1-joint 类型回顾

我们之前在说URDF的解析的时候，说过joint 的类型，我们再回顾一下：

• fixed：固定关节，不可旋转或移动
• continuous：连续关节，可无限旋转
• revolute：旋转关节，可在一定范围内旋转
• prismatic：推拉关节，可在一定范围内移动
• planar：平面关节，可在平面内移动
• floating：浮动关节，可在三维空间内自由移动和旋转

可以旋转的关节类型是continuous和revolute。

可以推拉的关节类型是prismatic，这就是我们这一章要推拉的关节。

2-joint 的拖拽推拉算法

在世界坐标系中，已知：

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

求：鼠标在推拉轴上的推拉量AD。

解：

1.计算向量AE和推拉轴u 的垂直向量i

i=normalize(AE)^u 

• normalize(AE) 是AE的单位向量
• ^ 是叉乘符号

2.计算u 和i 的垂直向量j

j=u^i

3.根据点A和向量j 确定一个过点A、法线方向为j 的平面 plane

4.以点E为起点，向点B的方向做射线EB

5.用射线与平面的求交算法，计算射线EB 与平面 plane 的交点C

6.向量AC与u轴的点积，就是鼠标在推拉轴上的推拉量AD

AD=AC·u

注：射线与平面求交的算法。

根据上图写一个射线的方程：

r(t)=E+td 
t=(EA·j)/(d·j)

• E：射线原点
• t：射线上的时间或距离标量，0≤t＜∞
• d：射线的方向

3-拖拽推拉的代码实现

在URDFDragControls 类的鼠标事件中增加对prismatic 类型的关节的判断。

在pointerdown 方法中构建拖拽平面：

if(type=='prismatic'){
  // 构建推拉平面
  const v1=new Vector3().subVectors(camera.position,point).normalize().cross(pivot)
  const v2=pivot.clone().cross(v1)
  tfPlane.setFromNormalAndCoplanarPoint(v2, point);
}

在pointermove 中计算瞬时推拉量：

if(type=='prismatic'){
    // 推拉关节
    // 推拉终点到推拉起点的向量
    const startToEnd=dragEnd.clone().sub(dragStart)
    // startToEnd 在推拉轴上的正射影,即瞬时推拉量
    delta=startToEnd.dot(pivot)
}

整体代码如下：

• src/robot/URDFDragControls.ts

import {
  Color,
  EventDispatcher,
  Intersection,
  Material,
  Mesh,
  MeshStandardMaterial,
  Object3D,
  Object3DEventMap,
  OrthographicCamera,
  PerspectiveCamera,
  Plane,
  Raycaster,
  Vector3,
} from "three";
import { URDFJoint, URDFLink, URDFRobot } from "./URDFClasses";
import { findMesh, 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];
        // 根据当前模型寻找其父级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=>{
        findMesh(link,(obj)=>{
          const intersections = raycaster.intersectObject(obj, false);
          const intLen = intersections.length;
          if (
            intLen &&
            (!temp.length || intersections[0].distance < temp[0].distance)
          ) {
            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 };

4-拖拽推拉测试

我们需要引入一个带有推拉关节的URDF模型-PR2.urdf，此模型可以直接在我的项目中找到。

PR2.urdf 中的推拉关节如下：

<joint name="gripper_extension" type="prismatic">
    <parent link="base_link"/>
    <child link="gripper_pole"/>
    <limit effort="1000.0" lower="-0.38" upper="0" velocity="0.5"/>
    <origin rpy="0 0 0" xyz="0.19 0 0.2"/>
</joint>

在App.vue 中引入PR2.urdf。

/* 机器人可视化 */
const hdrURL = "/texture/venice_sunset_1k.hdr";
// const urdfURL = "./models/panda/urdf/panda.urdf";
const urdfURL = "./models/PR2/urdf/PR2.urdf";
const robotVisual = new RobotVisual(hdrURL);
const { urdfDragControls } = robotVisual;

// 辅助控制
const helperControl = new URDFHelperControl();
const { helperMaps, currentHelperKey, currentHelperEles } = helperControl;

// 机器人
let robot: URDFRobot;
// 加载URDF模型
const urdfLoader= robotVisual.loadURDF(urdfURL,(model:URDFRobot)=>{
  robot = model;
  helperControl.setRobot(model)
});

// 设置子路径解析方法
/* urdfLoader.resolveSubPath=(filename: string)=>{
  return filename.replace(
    "package://example-robot-data/robots/panda_description", 
    './models/panda'
  );
} */
urdfLoader.resolveSubPath=(filename: string)=>{
  return filename.replace(
    "package://urdf_tutorial", 
    './models/PR2'
  );
}

效果如下：

总结

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

下一章我们会说一下joint拖拽变换的辅助路径的绘制。