def to_dict(self):
'''
to_dict() returns the shapes information in dictionary form
:returns: All information about the shape
:rtype: dict
'''
if self.stype == 'cylinder':
fk = self.wT * SE3.Rx(np.pi/2)
else:
fk = self.wT
shape = {
'stype': self.stype,
'scale': self.scale.tolist(),
'filename': self.filename,
'radius': self.radius,
'length': self.length,
't': fk.t.tolist(),
'q': r2q(fk.R).tolist(),
'v': self.v.tolist(),
'color': list(self.color)
}
return shape
def to_dict(self):
ob = {
'links': [],
'name': self.name,
'n': self.n,
'M': self.M,
'q_idx': self.q_idx
}
self.allfkine()
for link in self.ets:
li = {
'axis': [],
'eta': [],
'q_idx': link.q_idx,
'geometry': [],
't': link._fk.t.tolist(),
'q': r2q(link._fk.R).tolist()
}
for et in link.ets:
li['axis'].append(et.axis)
li['eta'].append(et.eta)
for gi in link.geometry:
g_fk = link._fk * gi.base
if gi.scale is not None:
scale = gi.scale.tolist()
else:
scale = [1, 1, 1]
li['geometry'].append({
'filename': gi.filename,
'scale': scale,
't': g_fk.t.tolist(),
'q': r2q(g_fk.R).tolist()
})
ob['links'].append(li)
return ob
def fk_dict(self):
ob = {'links': []}
self.allfkine()
# print(Tall)
for link in self.ets:
li = {'t': link._fk.t.tolist(), 'q': r2q(link._fk.R).tolist()}
ob['links'].append(li)
return ob
def fk_dict(self):
'''
fk_dict() outputs shapes pose in dictionary form
:returns: The shape pose in translation and quternion form
:rtype: dict
'''
if self.stype == 'cylinder':
fk = self.wT * SE3.Rx(np.pi / 2)
else:
fk = self.wT
shape = {'t': fk.t.tolist(), 'q': r2q(fk.R).tolist()}
return shape
def _update_pyb(self):
if _pyb and self.co is not None:
q = r2q(self.wT.R)
rot = [q[1], q[2], q[3], q[0]]
p.resetBasePositionAndOrientation(self.co, self.wT.t, rot)