def render_joint(joint, option):
glPushAttrib(GL_LIGHTING)
if option.lighting:
glEnable(GL_LIGHTING)
else:
glDisable(GL_LIGHTING)
j_type = joint.type()
T0 = joint.child_bodynode.transform()
T1 = joint.transform_from_child_body_node()
T = T0.dot(T1)
R, p = conversions.T2Rp(T)
scale = option.scale
if option.render_pos:
gl_render.render_point(p,
color=option.color_pos,
scale=scale,
radius=0.1)
if option.render_ori:
if j_type == "WeldJoint":
pass
elif j_type == "RevoluteJoint":
axis1 = joint.axis_in_world_frame()
gl_render.render_line(p,
p + scale * axis1,
color=option.color_ori[0])
elif j_type == "UniversalJoint":
axis1 = joint.axis1_in_world_frame()
axis2 = joint.axis2_in_world_frame()
gl_render.render_line(p,
p + scale * axis1,
color=option.color_ori[0])
gl_render.render_line(p,
p + scale * axis2,
color=option.color_ori[1])
elif j_type == "EulerJoint":
gl_render.render_transform(T,
scale=scale,
color_pos=option.color_pos,
color_ori=option.color_ori)
elif j_type == "BallJoint":
gl_render.render_transform(T,
scale=scale,
color_pos=option.color_pos,
color_ori=option.color_ori)
elif j_type == "TranslationalJoint":
gl_render.render_transform(T,
scale=scale,
color_pos=option.color_pos,
color_ori=option.color_ori)
elif j_type == "FreeJoint":
gl_render.render_transform(T,
scale=scale,
color_pos=option.color_pos,
color_ori=option.color_ori)
else:
raise NotImplementedError("Not Implemented")
glPopAttrib(GL_LIGHTING)
def render_joints(pb_client, model):
for j in range(pb_client.getNumJoints(model)):
joint_info = pb_client.getJointInfo(model, j)
joint_local_p, joint_local_Q, link_idx = joint_info[14], joint_info[
15], joint_info[16]
T_joint_local = conversions.Qp2T(np.array(joint_local_Q),
np.array(joint_local_p))
if link_idx == -1:
link_world_p, link_world_Q = pb_client.getBasePositionAndOrientation(
model)
else:
link_info = pb_client.getLinkState(model, link_idx)
link_world_p, link_world_Q = link_info[0], link_info[1]
T_link_world = conversions.Qp2T(np.array(link_world_Q),
np.array(link_world_p))
T_joint_world = np.dot(T_link_world, T_joint_local)
# Render joint position
glPushMatrix()
gl_render.glTransform(T_joint_world)
gl_render.render_point(np.zeros(3), radius=0.02, color=[0, 0, 0, 1])
# Render joint axis depending on joint types
joint_type = joint_info[2]
if joint_type == pb_client.JOINT_FIXED:
pass
elif joint_type == pb_client.JOINT_REVOLUTE:
axis = joint_info[13]
gl_render.render_line(np.zeros(3),
axis,
color=[1, 0, 0, 1],
line_width=1.0)
elif joint_type == pb_client.JOINT_SPHERICAL:
gl_render.render_transform(constants.eye_T(), scale=0.2)
else:
raise NotImplementedError()
glPopMatrix()
def _render_pose(self, pose, body_model, color):
skel = pose.skel
for j in skel.joints:
T = pose.get_transform(j, local=False)
pos = conversions.T2p(T)
gl_render.render_point(pos, radius=0.03 * self.scale, color=color)
if j.parent_joint is not None:
# returns X that X dot vec1 = vec2
pos_parent = conversions.T2p(
pose.get_transform(j.parent_joint, local=False))
p = 0.5 * (pos_parent + pos)
l = np.linalg.norm(pos_parent - pos)
r = 0.05
R = math.R_from_vectors(np.array([0, 0, 1]), pos_parent - pos)
gl_render.render_capsule(
conversions.Rp2T(R, p),
l,
r * self.scale,
color=color,
slice=8,
)
def render_links(pb_client, model):
for j in range(pb_client.getNumJoints(model)):
link_state = pb_client.getLinkState(model, j)
p, Q = np.array(link_state[0]), np.array(link_state[1])
T = conversions.Qp2T(Q, p)
gl_render.render_point(p, radius=0.01, color=[0, 1, 0, 1])