def setposrpy(Mat4):
RPY = rm.euler_from_matrix(
pg.npToMat3(
np.array([[Mat4[0][0], Mat4[0][1], Mat4[0][2]],
[Mat4[1][0], Mat4[1][1], Mat4[1][2]],
[Mat4[2][0], Mat4[2][1], Mat4[2][2]]])))
Pos = np.array([Mat4[3][0], Mat4[3][1], Mat4[3][2]])
return Pos, RPY
def setPosRPY(objcm, Mat4):
RPY = rm.euler_from_matrix(pg.npToMat3(np.array([[Mat4[0][0], Mat4[0][1], Mat4[0][2]],
[Mat4[1][0], Mat4[1][1], Mat4[1][2]],
[Mat4[2][0], Mat4[2][1], Mat4[2][2]]])))
Pos = np.array([Mat4[3][0], Mat4[3][1], Mat4[3][2]])
objcm.setPos(Pos[0], Pos[1], Pos[2])
objcm.setRPY(RPY[0], RPY[1], RPY[2])
return Pos, RPY
def genmnp(self, robot, jawwidthrgt = 0, jawwidthlft = 0,
toggleendcoord = True, togglejntscoord = False, name = 'robotmesh'):
"""
generate the mesh model of ur5
mnp means mesh nodepath
:param robotjoints: the joint positions of ur5
:param toggleendcoord: whether to plot end coordinate systems
:param togglejntscoord: whether to plot joints coordinate systems
:return: a nodepath which is ready to be plotted using plotmesh
author: weiwei
date: 20180130
"""
robotmesh_nodepath = NodePath(name)
# robotmesh_nodepath.setMat(pg.npToMat4(robot.base['rotmat'], robot.base['linkpos']))
robotwaist_rotmat = pg.npToMat4(robot.base['rotmat'], npvec3=robot.base['linkpos'])
self.__robotwaist_nodepath.setMat(robotwaist_rotmat)
# rgt
# base
ur3rgtbase_rotmat = pg.npToMat4(robot.rgtarm[1]['rotmat'], robot.rgtarm[1]['linkpos'])
self.__ur3rgtbase_nodepath.setMat(ur3rgtbase_rotmat)
self.__ur3rgtbase_nodepath.setColor(.5,.5,.5,1)
# shoulder
ur3rgtshoulder_rotmat = pg.npToMat4(robot.rgtarm[1]['rotmat'], robot.rgtarm[1]['linkpos'])
self.__ur3rgtshoulder_nodepath.setMat(ur3rgtshoulder_rotmat)
self.__ur3rgtshoulder_nodepath.setColor(.1,.3,.5,1)
# upperarm
ur3rgtupperarm_rotmat = pg.npToMat4(robot.rgtarm[2]['rotmat'], robot.rgtarm[2]['linkpos'])
self.__ur3rgtupperarm_nodepath.setMat(ur3rgtupperarm_rotmat)
self.__ur3rgtupperarm_nodepath.setColor(.7,.7,.7,1)
# forearm
ur3rgtforearm_rotmat = pg.npToMat4(robot.rgtarm[3]['rotmat'], robot.rgtarm[3]['linkpos'])
self.__ur3rgtforearm_nodepath.setMat(ur3rgtforearm_rotmat)
self.__ur3rgtforearm_nodepath.setColor(.35,.35,.35,1)
# wrist1
ur3rgtwrist1_rotmat = pg.npToMat4(robot.rgtarm[4]['rotmat'], robot.rgtarm[4]['linkpos'])
self.__ur3rgtwrist1_nodepath.setMat(ur3rgtwrist1_rotmat)
self.__ur3rgtwrist1_nodepath.setColor(.7,.7,.7,1)
# wrist2
ur3rgtwrist2_rotmat = pg.npToMat4(robot.rgtarm[5]['rotmat'], robot.rgtarm[5]['linkpos'])
self.__ur3rgtwrist2_nodepath.setMat(ur3rgtwrist2_rotmat)
self.__ur3rgtwrist2_nodepath.setColor(.1,.3,.5,1)
# wrist3
ur3rgtwrist3_rotmat = pg.npToMat4(robot.rgtarm[6]['rotmat'], robot.rgtarm[6]['linkpos'])
self.__ur3rgtwrist3_nodepath.setMat(ur3rgtwrist3_rotmat)
self.__ur3rgtwrist3_nodepath.setColor(.5,.5,.5,1)
self.__robotwaist_nodepath.reparentTo(robotmesh_nodepath)
self.__ur3rgtbase_nodepath.reparentTo(robotmesh_nodepath)
self.__ur3rgtshoulder_nodepath.reparentTo(robotmesh_nodepath)
self.__ur3rgtupperarm_nodepath.reparentTo(robotmesh_nodepath)
self.__ur3rgtforearm_nodepath.reparentTo(robotmesh_nodepath)
self.__ur3rgtwrist1_nodepath.reparentTo(robotmesh_nodepath)
self.__ur3rgtwrist2_nodepath.reparentTo(robotmesh_nodepath)
self.__ur3rgtwrist3_nodepath.reparentTo(robotmesh_nodepath)
# endcoord
if toggleendcoord:
self.pggen.plotAxis(robotmesh_nodepath,
spos = robot.rgtarm[-1]['linkend'],
pandamat3 = pg.npToMat3(robot.rgtarm[-1]['rotmat']))
# toggle all coord
if togglejntscoord:
for i in range(1, 7):
self.pggen.plotAxis(robotmesh_nodepath, spos=robot.rgtarm[i]['linkpos'],
pandamat3=pg.npToMat3(robot.rgtarm[i]['refcs']))
# hand
if self.rgthnd is not None:
self.rgthnd.setMat(pg.npToMat4(robot.rgtarm[6]['rotmat'], robot.rgtarm[6]['linkpos']))
self.rgthnd.setJawwidth(jawwidthrgt)
self.rgthnd.reparentTo(robotmesh_nodepath)
#lft
# base
ur3lftbase_rotmat = pg.npToMat4(robot.lftarm[1]['rotmat'], robot.lftarm[1]['linkpos'])
self.__ur3lftbase_nodepath.setMat(ur3lftbase_rotmat)
self.__ur3lftbase_nodepath.setColor(.5,.5,.5,1)
# shoulder
ur3lftshoulder_rotmat = pg.npToMat4(robot.lftarm[1]['rotmat'], robot.lftarm[1]['linkpos'])
self.__ur3lftshoulder_nodepath.setMat(ur3lftshoulder_rotmat)
self.__ur3lftshoulder_nodepath.setColor(.1,.3,.5,1)
# upperarm
ur3lftupperarm_rotmat = pg.npToMat4(robot.lftarm[2]['rotmat'], robot.lftarm[2]['linkpos'])
self.__ur3lftupperarm_nodepath.setMat(ur3lftupperarm_rotmat)
self.__ur3lftupperarm_nodepath.setColor(.7,.7,.7,1)
# forearm
ur3lftforearm_rotmat = pg.npToMat4(robot.lftarm[3]['rotmat'], robot.lftarm[3]['linkpos'])
self.__ur3lftforearm_nodepath.setMat(ur3lftforearm_rotmat)
self.__ur3lftforearm_nodepath.setColor(.35,.35,.35,1)
# wrist1
ur3lftwrist1_rotmat = pg.npToMat4(robot.lftarm[4]['rotmat'], robot.lftarm[4]['linkpos'])
self.__ur3lftwrist1_nodepath.setMat(ur3lftwrist1_rotmat)
self.__ur3lftwrist1_nodepath.setColor(.7,.7,.7,1)
# wrist2
ur3lftwrist2_rotmat = pg.npToMat4(robot.lftarm[5]['rotmat'], robot.lftarm[5]['linkpos'])
self.__ur3lftwrist2_nodepath.setMat(ur3lftwrist2_rotmat)
self.__ur3lftwrist2_nodepath.setColor(.1,.3,.5,1)
# wrist3
ur3lftwrist3_rotmat = pg.npToMat4(robot.lftarm[6]['rotmat'], robot.lftarm[6]['linkpos'])
self.__ur3lftwrist3_nodepath.setMat(ur3lftwrist3_rotmat)
self.__ur3lftwrist3_nodepath.setColor(.5,.5,.5,1)
self.__ur3lftbase_nodepath.reparentTo(robotmesh_nodepath)
self.__ur3lftshoulder_nodepath.reparentTo(robotmesh_nodepath)
self.__ur3lftupperarm_nodepath.reparentTo(robotmesh_nodepath)
self.__ur3lftforearm_nodepath.reparentTo(robotmesh_nodepath)
self.__ur3lftwrist1_nodepath.reparentTo(robotmesh_nodepath)
self.__ur3lftwrist2_nodepath.reparentTo(robotmesh_nodepath)
self.__ur3lftwrist3_nodepath.reparentTo(robotmesh_nodepath)
# endcoord
if toggleendcoord:
self.pggen.plotAxis(robotmesh_nodepath,
spos = robot.lftarm[-1]['linkend'],
pandamat3 = pg.npToMat3(robot.lftarm[-1]['rotmat']))
# toggle all coord
if togglejntscoord:
for i in range(1, 7):
self.pggen.plotAxis(robotmesh_nodepath, spos = robot.lftarm[i]['linkpos'],
pandamat3 = pg.npToMat3(robot.lftarm[i]['refcs']))
# hand
if self.lfthnd is not None:
self.lfthnd.setMat(pg.npToMat4(robot.lftarm[6]['rotmat'], robot.lftarm[6]['linkpos']))
self.lfthnd.setJawwidth(jawwidthlft)
self.lfthnd.reparentTo(robotmesh_nodepath)
return copy.deepcopy(robotmesh_nodepath)
pos_joint = np.array(
[px_resample[i], py_resample[i], pz_resample[i]])
joint_pose_to_base = rm.homobuild(pos_joint, rot_joint)
rot_obj0 = np.array([[0, 1, 0], [0, 0, 1], [1, 0, 0]])
pos_obj = np.array([0, 21, 54 + 15 + 130 + 15])
obj_pose_to_joint0 = rm.homobuild(pos_obj, rot_obj0)
obj_pose_to_joint = np.dot(
rm.homobuild(np.array([0, 0, 0]),
rm.euler_matrix(0, 0, 225 + 22.5)),
obj_pose_to_joint0)
obj_pose_to_base = np.dot(joint_pose_to_base, obj_pose_to_joint)
p_obj = obj_pose_to_base[:3, 3]
# print(p_obj)
pobj.append(p_obj)
rpy_obj = rm.euler_from_matrix(
pg.npToMat3(np.transpose(obj_pose_to_base[:3, :3])))
# print(rpy_obj)
robj.append(rpy_obj)
# with open(os.path.join(this_dir, "resampledata", file), "w", newline='') as resamplefile:
# writer = csv.writer(resamplefile)
# writer.writerow(['px_robot[mm]', 'py_robot[mm]', 'pz_robot[mm]', 'prx_robot[deg]', 'pry_robot[deg]', 'prz_robot[deg]',
# 'fx_robot[N]', 'fy_robot[N]', 'fz_robot[N]', 'mx_robot[Nm]', 'my_robot[Nm]', 'mz_robot[Nm]',
# 'px_obj[mm]', 'py_obj[mm]', 'pz_obj[mm]', 'prx_obj[deg]', 'pry_obj[deg]', 'prz_obj[deg]'])
# for i in range(10, resample_num+del_num-10):
# writer.writerow([round(px_resample[i], 3), round(py_resample[i], 3), round(pz_resample[i], 3),
# round(prx_resample[i], 3), round(pry_resample[i], 3), round(prz_resample[i], 3),
# round(fx_resample[i], 3), round(fy_resample[i], 3), round(fz_resample[i], 3),
# round(frx_resample[i], 3), round(fry_resample[i], 3), round(frz_resample[i], 3),
# round(pobj[i][0], 3), round(pobj[i][1], 3), round(pobj[i][2], 3),
# round(robj[i][0], 3), round(robj[i][1], 3), round(robj[i][2], 3)])
lookatp=[0, 0, 0],
up=[0, 0, 1],
fov=40,
w=1920,
h=1080)
env = bldgfsettingnear.Env()
# self.env.reparentTo(base.render)
objname = "new_LSHAPE.stl"
objcm = env.loadobj(objname)
groove = env.loadobj("new_GROOVE.stl")
posG = base.pg.npToMat4(np.array([[1, 0, 0], [0, 1, 0], [0, 0, 1]]),
np.array([0, 0, 0]))
virtualgoalpos = np.array([posG[3][0], posG[3][1], posG[3][2]])
virtualgoalrot = rm.euler_from_matrix(
pg.npToMat3(
np.array([[posG[0][0], posG[0][1], posG[0][2]],
[posG[1][0], posG[1][1], posG[1][2]],
[posG[2][0], posG[2][1], posG[2][2]]])))
groove.setPos(virtualgoalpos[0], virtualgoalpos[1], virtualgoalpos[2])
groove.setRPY(virtualgoalrot[0], virtualgoalrot[1], virtualgoalrot[2])
groove.setColor(0, 0, 1, 0.4)
groove.reparentTo(base.render)
# relpos = np.array([-60.14147593, -0.43612779, 40.08426847])
# relrot = np.array([[0.9672951, 0.003912, 0.25362363],
# [-0.00265115, 0.99998247, -0.00531303],
# [-0.25363992, 0.00446691, 0.9672884]])
relpos = np.array([-56.28811304, -3.55672667, 40.3790682])
relrot = np.array([[0.96447225, -0.01227223, 0.2638989],
[0.02384718, 0.99888664, -0.04070254],
[-0.26310561, 0.04554969, 0.96369113]])
relmat = base.pg.npToMat4(relrot, relpos)
def genmnp(self,
robot,
jawwidthrgt=0,
jawwidthlft=0,
toggleendcoord=False,
togglejntscoord=False,
name='robotmesh'):
"""
generate a panda3d nodepath for the robot
mnp indicates this function generates a mesh nodepath
:param robot: the robot object, see robot.py
:return: a nodepath which is ready to be plotted using plotmesh
author: weiwei
date: 20180109
"""
robotmesh_nodepath = NodePath(name)
# body
nxtleg_rotmat = pg.npToMat4(robot.base['refcs'],
npvec3=robot.base['linkpos'])
self.__nxtleg_nodepath.setMat(nxtleg_rotmat)
nxtwaist_rotmat = pg.npToMat4(robot.base['rotmat'],
npvec3=robot.base['linkpos'] +
np.array([0.0, 0.0, 973.0]))
self.__nxtwaist_nodepath.setMat(base.render, nxtwaist_rotmat)
# rgtarm
nxtrgtarmlj0_rotmat = pg.npToMat4(robot.rgtarm[1]['rotmat'],
robot.rgtarm[1]['linkpos'])
self.__nxtrgtarmlj0_nodepath.setMat(nxtrgtarmlj0_rotmat)
nxtrgtarmlj1_rotmat = pg.npToMat4(robot.rgtarm[2]['rotmat'],
robot.rgtarm[2]['linkpos'])
self.__nxtrgtarmlj1_nodepath.setMat(nxtrgtarmlj1_rotmat)
nxtrgtarmlj2_rotmat = pg.npToMat4(robot.rgtarm[3]['rotmat'],
robot.rgtarm[3]['linkpos'])
self.__nxtrgtarmlj2_nodepath.setMat(nxtrgtarmlj2_rotmat)
nxtrgtarmlj3_rotmat = pg.npToMat4(robot.rgtarm[4]['rotmat'],
robot.rgtarm[4]['linkpos'])
self.__nxtrgtarmlj3_nodepath.setMat(nxtrgtarmlj3_rotmat)
nxtrgtarmlj4_rotmat = pg.npToMat4(robot.rgtarm[5]['rotmat'],
robot.rgtarm[5]['linkpos'])
self.__nxtrgtarmlj4_nodepath.setMat(nxtrgtarmlj4_rotmat)
# lftarm
nxtlftarmlj0_rotmat = pg.npToMat4(robot.lftarm[1]['rotmat'],
robot.lftarm[1]['linkpos'])
self.__nxtlftarmlj0_nodepath.setMat(nxtlftarmlj0_rotmat)
nxtlftarmlj1_rotmat = pg.npToMat4(robot.lftarm[2]['rotmat'],
robot.lftarm[2]['linkpos'])
self.__nxtlftarmlj1_nodepath.setMat(nxtlftarmlj1_rotmat)
nxtlftarmlj2_rotmat = pg.npToMat4(robot.lftarm[3]['rotmat'],
robot.lftarm[3]['linkpos'])
self.__nxtlftarmlj2_nodepath.setMat(nxtlftarmlj2_rotmat)
nxtlftarmlj3_rotmat = pg.npToMat4(robot.lftarm[4]['rotmat'],
robot.lftarm[4]['linkpos'])
self.__nxtlftarmlj3_nodepath.setMat(nxtlftarmlj3_rotmat)
nxtlftarmlj4_rotmat = pg.npToMat4(robot.lftarm[5]['rotmat'],
robot.lftarm[5]['linkpos'])
self.__nxtlftarmlj4_nodepath.setMat(nxtlftarmlj4_rotmat)
self.__nxtleg_nodepath.reparentTo(robotmesh_nodepath)
self.__nxtrgtarmlj0_nodepath.reparentTo(robotmesh_nodepath)
self.__nxtrgtarmlj1_nodepath.reparentTo(robotmesh_nodepath)
self.__nxtrgtarmlj2_nodepath.reparentTo(robotmesh_nodepath)
self.__nxtrgtarmlj3_nodepath.reparentTo(robotmesh_nodepath)
self.__nxtrgtarmlj4_nodepath.reparentTo(robotmesh_nodepath)
self.__nxtlftarmlj0_nodepath.reparentTo(robotmesh_nodepath)
self.__nxtlftarmlj1_nodepath.reparentTo(robotmesh_nodepath)
self.__nxtlftarmlj2_nodepath.reparentTo(robotmesh_nodepath)
self.__nxtlftarmlj3_nodepath.reparentTo(robotmesh_nodepath)
self.__nxtlftarmlj4_nodepath.reparentTo(robotmesh_nodepath)
## right
# endcoord
if toggleendcoord:
self.pggen.plotAxis(robotmesh_nodepath,
spos=robot.rgtarm[-1]['linkend'],
pandamat3=pg.npToMat3(
robot.rgtarm[-1]['rotmat']))
# toggle all coord
if togglejntscoord:
for i in range(1, 7):
self.pggen.plotAxis(robotmesh_nodepath,
spos=robot.rgtarm[i]['linkpos'],
pandamat3=pg.npToMat3(
robot.rgtarm[i]['refcs']))
# hand
if self.rgthnd is not None:
self.rgthnd.setMat(
pg.npToMat4(robot.rgtarm[-1]['rotmat'],
robot.rgtarm[-1]['linkpos']))
self.rgthnd.setJawwidth(jawwidthrgt)
self.rgthnd.reparentTo(robotmesh_nodepath)
# left
# endcoord
if toggleendcoord:
self.pggen.plotAxis(robotmesh_nodepath,
spos=robot.lftarm[-1]['linkend'],
pandamat3=pg.npToMat3(
robot.lftarm[-1]['rotmat']))
# toggle all coord
if togglejntscoord:
for i in range(1, 7):
self.pggen.plotAxis(robotmesh_nodepath,
spos=robot.lftarm[i]['linkpos'],
pandamat3=pg.npToMat3(
robot.lftarm[i]['refcs']))
# hand
if self.lfthnd is not None:
self.lfthnd.setMat(
pg.npToMat4(robot.lftarm[-1]['rotmat'],
robot.lftarm[-1]['linkpos']))
self.lfthnd.setJawwidth(jawwidthlft)
self.lfthnd.reparentTo(robotmesh_nodepath)
return copy.deepcopy(robotmesh_nodepath)
# print(rot_obj)
pos_obj = np.array([0, 21, 54 + 15 + 130 + 15])
obj_pose_to_joint0 = rm.homobuild(pos_obj, rot_obj0)
obj_pose_to_joint = np.dot(
rm.homobuild(np.array([0, 0, 0]),
rm.euler_matrix(0, 0, 225 + 22.5)),
obj_pose_to_joint0)
# print(obj_pose_to_joint)
obj_pose_to_base = np.dot(joint_pose_to_base, obj_pose_to_joint)
# print("obj pose to base is", obj_pose_to_base)
objpos0 = np.array([0, 0, 0])
rot0 = rm.euler_matrix(0, 0, 0)
base.pggen.plotAxis(nodepath=base.render,
spos=objpos0,
pandamat3=pg.npToMat3(rot0))
base.pggen.plotAxis(nodepath=base.render,
spos=joint_pose_to_base[:3, 3],
pandamat3=pg.npToMat3(joint_pose_to_base[:3, :3]),
length=200)
rpy_jnt = rm.euler_from_matrix(
(pg.npToMat3(np.transpose(joint_pose_to_base[:3, :3]))))
print("joint euler angle to base=", rpy_jnt)
base.pggen.plotAxis(nodepath=base.render,
spos=obj_pose_to_base[:3, 3],
pandamat3=pg.npToMat3(obj_pose_to_base[:3, :3]),
length=225)
rpy_obj = rm.euler_from_matrix(
pg.npToMat3(np.transpose(obj_pose_to_base[:3, :3])))
print("obj euler angle to base=", rpy_obj)
