def render_joint_geoms(pb_client,
model,
radius=0.025,
color=[0.5, 0.5, 0.5, 1]):
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.render_sphere(T_joint_world,
radius,
color=color,
slice1=16,
slice2=16)
gl_render.render_sphere_info(T_joint_world,
radius,
line_width=4.0,
slice=32)
glPopMatrix()
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_model(
pb_client,
model,
draw_link=True,
draw_link_info=True,
draw_joint=False,
draw_joint_geom=True,
ee_indices=None,
color=None,
link_info_scale=1.0,
link_info_color=[0, 0, 0, 1],
link_info_line_width=1.0,
lighting=True,
):
if draw_link or draw_link_info:
data_visual = pb_client.getVisualShapeData(model)
for d in data_visual:
if lighting:
glEnable(GL_LIGHTING)
else:
glDisable(GL_LIGHTING)
link_id = d[1]
if link_id == -1:
p, Q = pb_client.getBasePositionAndOrientation(model)
else:
link_state = pb_client.getLinkState(model, link_id)
p, Q = link_state[4], link_state[5]
p, Q = np.array(p), np.array(Q)
R = conversions.Q2R(Q)
T_joint = conversions.Rp2T(R, p)
T_visual_from_joint = conversions.Qp2T(np.array(d[6]),
np.array(d[5]))
glPushMatrix()
gl_render.glTransform(T_joint)
gl_render.glTransform(T_visual_from_joint)
# if color is None: color = d[7]
# alpha = 0.5 if draw_joint else color[3]
if color is None:
color = [d[7][0], d[7][1], d[7][2], d[7][3]]
if draw_link:
render_geom(geom_type=d[2], geom_size=d[3], color=color)
if draw_link_info:
render_geom_info(geom_type=d[2],
geom_size=d[3],
color=link_info_color,
scale=link_info_scale,
line_width=link_info_line_width)
# if ee_indices is not None and link_id in ee_indices:
# render_geom_bounding_box(geom_type=d[2], geom_size=d[3], color=[0, 0, 0, 1])
glPopMatrix()
if draw_joint_geom:
render_joint_geoms(pb_client, model)
glDisable(GL_DEPTH_TEST)
if draw_joint:
render_joints(pb_client, model)
render_links(pb_client, model)
glEnable(GL_DEPTH_TEST)
def get_joint_position(j, p_root, Q_root, p_link, Q_link):
if j == self._char_info.ROOT or self._joint_parent_link[
j] == self._char_info.ROOT:
p, Q = p_root, Q_root
else:
p, Q = p_link[self._joint_parent_link[j]], Q_link[
self._joint_parent_link[j]]
T_link_world = conversions.Qp2T(Q, p)
T_joint_local = constants.eye_T(
) if j == self._char_info.ROOT else self._joint_xform_from_parent_link[
j]
T_joint_world = np.dot(T_link_world, T_joint_local)
return conversions.T2p(T_joint_world)
def set_transform(self, key, T, local, do_ortho_norm=True):
if local:
T1 = T
else:
T0 = self.skel.get_joint(key).xform_global
T1 = np.dot(math.invertT(T0), T)
if do_ortho_norm:
"""
This insures that the rotation part of
the given transformation is valid
"""
Q, p = conversions.T2Qp(T1)
Q = quaternion.Q_op(Q, op=["normalize"])
T1 = conversions.Qp2T(Q, p)
self.data[self.skel.get_index_joint(key)] = T1
def render(self):
for obs in self.obstacles:
decay_start = 0.5
alpha = min(1.0, 1.0 / decay_start * obs.duration)
c = obs.color
T = conversions.Qp2T(obs.Q, obs.p)
rm.gl.glPushMatrix()
rm.gl_render.glTransform(T)
if obs.shape == Shape.BOX:
geom_type = self.pb_client.GEOM_BOX
elif obs.shape == Shape.SPHERE:
geom_type = self.pb_client.GEOM_SPHERE
else:
raise NotImplementedError
rm.bullet_render.render_geom(geom_type=geom_type,
geom_size=obs.size,
color=[c[0], c[1], c[2], alpha])
rm.bullet_render.render_geom_info(geom_type=geom_type,
geom_size=obs.size)
rm.gl.glPopMatrix()
def get_pose(self, skel):
p, Q = self._pb_client.getBasePositionAndOrientation(self._body_id)
states = self._pb_client.getJointStatesMultiDof(
self._body_id, self._joint_indices)
pose_data = []
for i in range(skel.num_joint()):
joint = skel.joints[i]
if joint == skel.root_joint:
pose_data.append(conversions.Qp2T(Q, p))
else:
j = self._char_info.bvh_map_inv[joint.name]
if j is None:
pose_data.append(constants.eye_T())
else:
joint_type = self._joint_type[j]
if joint_type == self._pb_client.JOINT_FIXED:
pose_data.append(constants.eye_T())
elif joint_type == self._pb_client.JOINT_SPHERICAL:
pose_data.append(conversions.Q2T(states[j][0]))
else:
raise NotImplementedError()
return motion.Pose(skel, pose_data)
def __init__(
self,
pybullet_client,
model_file,
char_info,
scale=1.0, # This affects loadURDF
ref_scale=1.0, # This will be used when reference motions are appllied to this agent
verbose=False,
kinematic_only=False,
self_collision=True,
name="agent",
actuation="spd",
):
self._name = name
self._actuation = SimAgent.Actuation.from_string(actuation)
self._pb_client = pybullet_client
self._char_info = char_info
# Load self._body_id file
char_create_flags = self._pb_client.URDF_MAINTAIN_LINK_ORDER
if self_collision:
char_create_flags = char_create_flags|\
self._pb_client.URDF_USE_SELF_COLLISION|\
self._pb_client.URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS
self._body_id = self._pb_client.loadURDF(model_file, [0, 0, 0],
globalScaling=scale,
useFixedBase=False,
flags=char_create_flags)
for pair in self._char_info.collison_ignore_pairs:
self._pb_client.setCollisionFilterPair(self._body_id,
self._body_id,
pair[0],
pair[1],
enableCollision=False)
# TODO: should ref_scale be removed?
self._ref_scale = ref_scale
self._num_joint = self._pb_client.getNumJoints(self._body_id)
self._joint_indices = range(self._num_joint)
self._link_indices = range(-1, self._num_joint)
self._joint_indices_movable = []
if kinematic_only:
self.setup_kinematics()
else:
self.setup_dynamics()
# Pre-compute informations about the agent
self._joint_type = []
self._joint_axis = []
self._joint_dofs = []
for j in self._joint_indices:
joint_info = self._pb_client.getJointInfo(self._body_id, j)
self._joint_type.append(joint_info[2])
self._joint_axis.append(np.array(joint_info[13]))
# if verbose:
# print('-----------------------')
# print(joint_info[1])
# print('joint_type', joint_info[2])
# print('joint_damping', joint_info[6])
# print('joint_friction', joint_info[7])
# print('joint_upper_limit', joint_info[8])
# print('joint_lower_limit', joint_info[9])
# print('joint_max_force', joint_info[10])
# print('joint_max_vel', joint_info[11])
for j in self._joint_indices:
if self._joint_type[j] == self._pb_client.JOINT_SPHERICAL:
self._joint_dofs.append(3)
self._joint_indices_movable.append(j)
elif self._joint_type[j] == self._pb_client.JOINT_REVOLUTE:
self._joint_dofs.append(1)
self._joint_indices_movable.append(j)
elif self._joint_type[j] == self._pb_client.JOINT_FIXED:
self._joint_dofs.append(0)
else:
raise NotImplementedError()
self._num_dofs = np.sum(self._joint_dofs)
self._joint_pose_init, self._joint_vel_init = self.get_joint_states()
self._joint_parent_link = []
self._joint_xform_from_parent_link = []
for j in self._joint_indices:
joint_info = self._pb_client.getJointInfo(self._body_id, j)
joint_local_p = np.array(joint_info[14])
joint_local_Q = np.array(joint_info[15])
link_idx = joint_info[16]
self._joint_parent_link.append(link_idx)
self._joint_xform_from_parent_link.append(
conversions.Qp2T(joint_local_Q, joint_local_p))
self._link_masses = []
self._link_total_mass = 0.0
for i in self._link_indices:
di = self._pb_client.getDynamicsInfo(self._body_id, i)
mass = di[0]
self._link_total_mass += mass
self._link_masses.append(mass)
if verbose:
print('[SimAgent] Creating an agent...', model_file)
print('num_joint <%d>, num_dofs <%d>, total_mass<%f>'\
%(self._num_joint, self._num_dofs, self._link_total_mass))
def get_root_transform(self):
p, Q, _, _ = bu.get_base_pQvw(self._pb_client, self._body_id)
return conversions.Qp2T(Q, p)
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])