在这里发布一个ISAAC GYM可以使用的箭头绘制类。

gymutil默认有WireframeBoxGeometry，WireframeBBoxGeometry， WireframeSphereGeometry三个线段集生成函数，可以绘制盒子和球体。绘制函数分别有draw_lines和draw_line。

同理，使用以下方法绘制3D箭头：

# -*- coding: utf-8 -*-
'''------------------------------------------------------------------@File Name:        WireframeGeometry@Created:          2024 2024/11/22 16:01@Software:         PyCharm@Author:           Jiayu ZENG@Email:            jiayuzeng@asagi.waseda.jp@Description:      ------------------------------------------------------------------
'''from __future__ import print_function, division, absolute_importfrom abc import ABC, abstractmethod
import math
import numpy as np
import argparse
from bisect import bisectfrom isaacgym import gymtorch, gymapi, gymutil
import numpy as np
import mathclass WireframeArrowGeometry(gymutil.LineGeometry):def __init__(self, start_point, direction, length=1.0, arrow_head_length=0.2, arrow_head_width=0.1, shaft_radius=0.05, shaft_segments=8, pose=None, color=None, color2=None):if color is None:color = (1, 0, 0)if color2 is None:color2 = color# Normalize directiondirection = np.array(direction, dtype=float)# Calculate main arrow shaft endpointshaft_end_point = start_point + length * directionarrow_tip = start_point + (length+arrow_head_length) * direction# Arrow shaftverts = []colors = []# Generate perpendicular vectors to direction for shaftperp1 = np.cross(direction, np.array([1, 0, 0]))if np.linalg.norm(perp1) < 1e-6:perp1 = np.cross(direction, np.array([0, 1, 0]))perp1 = perp1 / np.linalg.norm(perp1) * shaft_radiusperp2 = np.cross(direction, perp1)perp2 = perp2 / np.linalg.norm(perp2) * shaft_radius# Generate shaft lines in a circular patternangle_step = 2 * math.pi / shaft_segmentsshaft_base_points = []arrow_base_points = []for i in range(shaft_segments):angle = i * angle_stepnext_angle = (i + 1) * angle_stepoffset1 = math.cos(angle) * perp1 + math.sin(angle) * perp2offset2 = math.cos(next_angle) * perp1 + math.sin(next_angle) * perp2start_circle = start_point + offset1end_circle = shaft_end_point + offset1shaft_base_points.append(end_circle)verts.append((start_circle, end_circle))colors.append(color)verts.append((start_circle, start_point + offset2))colors.append(color)verts.append((end_circle, shaft_end_point + offset2))colors.append(color)# Arrow head base pointarrow_base = shaft_end_point# Generate perpendicular vectors to direction for arrow headperp1_head = perp1 / shaft_radius * arrow_head_widthperp2_head = perp2 / shaft_radius * arrow_head_width# Generate arrow head lines to represent a conefor i in range(shaft_segments):angle = i * angle_stepnext_angle = (i + 1) * angle_stepoffset1 = math.cos(angle) * perp1_head + math.sin(angle) * perp2_headoffset2 = math.cos(next_angle) * perp1_head + math.sin(next_angle) * perp2_headbase_point1 = arrow_base + offset1base_point2 = arrow_base + offset2arrow_base_points.append(base_point1)# Lines from tip to base circleverts.append((arrow_tip, base_point1))colors.append(color2)# Lines around the base circleverts.append((base_point1, base_point2))colors.append(color2)# Connect corresponding points on the shaft end and arrow basefor shaft_point, arrow_point in zip(shaft_base_points, arrow_base_points):verts.append((shaft_point, arrow_point))colors.append(color2)# Convert verts and colors to numpy arraysnum_lines = len(verts)verts_np = np.empty((num_lines, 2), gymapi.Vec3.dtype)colors_np = np.empty(num_lines, gymapi.Vec3.dtype)for idx, (v_start, v_end) in enumerate(verts):verts_np[idx][0] = (v_start[0], v_start[1], v_start[2])verts_np[idx][1] = (v_end[0], v_end[1], v_end[2])colors_np[idx] = colors[idx]# Apply pose transformation if providedif pose is None:self.verts = verts_npelse:self.verts = pose.transform_points(verts_np)self._colors = colors_npdef vertices(self):return self.vertsdef colors(self):return self._colors

同样调用gymutil.draw_lines绘制。以下代码仅作参考。

start_point = np.array([0.0, 0.0, 0.0])direction[2] = start_point[2]color = (0, 1, 0)color2 = (1, 0, 0)length *= 0.5shaft_radius = 0.02shaft_segments = 16arrow_head_length = 0.2arrow_head_width = shaft_radius * 3arrow = WireframeArrowGeometry(start_point, direction, length, arrow_head_length, arrow_head_width,shaft_radius, shaft_segments, color=color, color2=color2)sphere_pose = gymapi.Transform(gymapi.Vec3(pos[0],pos[1],pos[2] + 0.2), r=None)gymutil.draw_lines(arrow, self.gym, self.viewer, env, sphere_pose)

绘制效果：