I was trying to get the vertex positions of the meshes after performing forward kinematics on the robot (say, when the joint positions are set, the vertices of the meshes had have to be updated with respect to the base frame). I used the UR robot which is a part of this repository as an example and here is my implementation.
from urdfpy import URDF
class Kinematics:
def __init__(self, urdf):
self.urdf = urdf
self.robot = URDF.load(self.urdf)
self.fk = self.robot.collision_trimesh_fk()
def setjntpos(self, jntpos):
assert(len(jntpos)==len(self.robot.actuated_joints))
config = {}
for index in range(len(self.robot.actuated_joints)):
config[self.robot.actuated_joints[index].name] = jntpos[index]
self.fk = self.robot.collision_trimesh_fk(cfg=config)
return config
def getrobotvertices(self, config):
for i in range(len(list(self.fk.keys()))):
print(list(self.fk.keys())[i].vertices)
kinematics = Kinematics('robots/ur5/ur5.urdf')
cfg = kinematics.setjntpos([0.0, -2.0, 2.0, 0.0, 1.0, 0.0])
kinematics.getrobotvertices(cfg)
The positions of the vertices are not being updated. Is this the right way to use the collision_trimesh_fk API?
I was trying to get the vertex positions of the meshes after performing forward kinematics on the robot (say, when the joint positions are set, the vertices of the meshes had have to be updated with respect to the base frame). I used the UR robot which is a part of this repository as an example and here is my implementation. from urdfpy import URDF
The positions of the vertices are not being updated. Is this the right way to use the collision_trimesh_fk API?