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])