def __getonebasedrotmats(self):
self.onebasedrotmats = []
self.onebasedrotRPY = []
self.onebasedrotRPYdistance = []
self.placementRotMat33 = []
for rotmat in self.pRotMat:
rotmat = p3du.mat4ToNp(rotmat)
rotmat33 = rotmat[0:3, 0:3]
self.placementRotMat33.append(rotmat33)
for i, rotmat in enumerate(self.placementRotMat33):
rotmat_ni = np.linalg.inv(rotmat)
temonebasedrotmat = []
temonebasedrotRPY = []
temonebasedrotRPYdistance = []
for j in range(0, len(self.placementRotMat33)):
if j != i and self.collisionlist[
j] == False: #add the False adjustment if no considering about collision
temonebasedrotmat.append(
np.dot(rotmat_ni, self.placementRotMat33[j]))
transRottoRPY = rm.rotmat_to_euler(
np.dot(rotmat_ni, self.placementRotMat33[j]))
# temonebasedrotRPY.append(transRottoRPY)
if abs(transRottoRPY[0] + transRottoRPY[1] +
transRottoRPY[2]) > 0.00001:
temonebasedrotRPY.append(transRottoRPY)
temonebasedrotRPYdistance.append(
humath.distance(transRottoRPY, [0, 0, 0]))
self.onebasedrotmats.append(temonebasedrotmat)
self.onebasedrotRPY.append(temonebasedrotRPY)
self.onebasedrotRPYdistance.append(min(temonebasedrotRPYdistance))
print("self.onebasedrotmats", self.onebasedrotmats)
print("self.onebasedrotRPY", self.onebasedrotRPY)
print("self.onebasedrotRPYdistance", self.onebasedrotRPYdistance)
def get_rpy(self):
"""
get the pose of the object using rpy
:return: [r, p, y] in radian
author: weiwei
date: 20190513
"""
npmat3 = self.get_rotmat()
rpy = rm.rotmat_to_euler(npmat3, axes="sxyz")
return np.array([rpy[0], rpy[1], rpy[2]])
def gen_data(rbt_s,
component_name='arm',
granularity=math.pi / 8,
save_name='cobotta_ik.csv'):
n_jnts = rbt_s.manipulator_dict[component_name].ndof
all_ranges = []
for jnt_id in range(1, n_jnts + 1):
r0, r1 = rbt_s.manipulator_dict[component_name].jnts[jnt_id][
'motion_rng']
all_ranges.append(np.arange(r0, r1, granularity))
# print(granularity, all_ranges[-1])
all_data = itertools.product(*all_ranges)
n_data = 1
for rngs in all_ranges:
n_data = n_data * len(rngs)
data_set = []
in_data_npy = np.empty((0, 6))
for i, data in enumerate(all_data):
print(i, n_data)
rbt_s.fk(component_name=component_name, jnt_values=np.array(data))
xyz, rotmat = rbt_s.get_gl_tcp(manipulator_name=component_name)
rpy = rm.rotmat_to_euler(rotmat)
in_data = (xyz[0], xyz[1], xyz[2], rpy[0], rpy[1], rpy[2])
# diff = np.sum(np.abs(np.array(in_data) - in_data_npy), 1)
# if np.any(diff < 1e-4):
# print(diff)
# input("Press Enter to continue...")
in_data_npy = np.vstack((in_data_npy, np.array(in_data)))
out_data = data
data_set.append([in_data, out_data])
# df = pd.DataFrame(data_set, columns=['xyzrpy', 'jnt_values'])
# df.to_csv(save_name)
np.save(save_name + "_min_max",
np.array([np.min(in_data_npy, 0),
np.max(in_data_npy, 0)]))
np.save(save_name, np.array(data_set))
def __onebasedrotmattoRPY(self):
self.rotRPY = []
for rotmat in self.pRotMat:
rotmat = p3du.mat4ToNp(rotmat)
rotmat33 = rotmat[0:3, 0:3]
self.rotRPY.append(rm.rotmat_to_euler(rotmat33))
import robot_sim.robots.cobotta.cobotta as cbt_s
if __name__ == '__main__':
base = world.World(cam_pos=np.array([1.5, 1, .7]))
gm.gen_frame().attach_to(base)
rbt_s = cbt_s.Cobotta()
rbt_s.fk(jnt_values=np.zeros(6))
rbt_s.gen_meshmodel(toggle_tcpcs=True, rgba=[.5, .5, .5,
.3]).attach_to(base)
rbt_s.gen_stickmodel(toggle_tcpcs=True).attach_to(base)
tgt_pos = np.array([.25, .2, .15])
tgt_rotmat = rm.rotmat_from_axangle([0, 1, 0], math.pi * 2 / 3)
gm.gen_frame(pos=tgt_pos, rotmat=tgt_rotmat).attach_to(base)
# numerical ik
jnt_values = rbt_s.ik(tgt_pos=tgt_pos, tgt_rotmat=tgt_rotmat)
rbt_s.fk(jnt_values=jnt_values)
rbt_s.gen_meshmodel(toggle_tcpcs=True, rgba=[.5, .5, .5,
.3]).attach_to(base)
# neural ik
model = cbf.Net(n_hidden=100, n_jnts=6)
model.load_state_dict(torch.load("cobotta_model.pth"))
tgt_rpy = rm.rotmat_to_euler(tgt_rotmat)
xyzrpy = torch.from_numpy(np.hstack((tgt_pos, tgt_rpy)))
jnt_values = model(xyzrpy.float()).to('cpu').detach().numpy()
rbt_s.fk(jnt_values=jnt_values)
rbt_s.gen_meshmodel(toggle_tcpcs=True).attach_to(base)
base.run()