def get_current_variable_values_for_arena(self):
"""
:return: (dict) returns all the exposed variables and their values
in the environment's stage except for objects.
"""
if self._pybullet_client_w_o_goal_id is not None:
client = self._pybullet_client_w_o_goal_id is not None
else:
client = self._pybullet_client_full_id
variable_params = dict()
variable_params["floor_color"] = \
pybullet.getVisualShapeData(WorldConstants.FLOOR_ID,
physicsClientId=client)[0][7][:3]
variable_params["floor_friction"] = \
pybullet.getDynamicsInfo(WorldConstants.FLOOR_ID, -1,
physicsClientId=client)[1]
variable_params["stage_color"] = \
pybullet.getVisualShapeData(WorldConstants.STAGE_ID,
physicsClientId=client)[0][7][:3]
variable_params["stage_friction"] = \
pybullet.getDynamicsInfo(WorldConstants.STAGE_ID, -1,
physicsClientId=client)[1]
variable_params["gravity"] = \
self._current_gravity
return variable_params
def _RecordMassInfoFromURDF(self):
self._base_mass_urdf = p.getDynamicsInfo(self.minitaur,
self.BASE_LINK_ID)[0]
self._leg_masses_urdf = []
self._leg_masses_urdf.append(
p.getDynamicsInfo(self.minitaur, self.LEG_LINK_ID[0])[0])
self._leg_masses_urdf.append(
p.getDynamicsInfo(self.minitaur, self.MOTOR_LINK_ID[0])[0])
def changeFriction(self, lateralFriction=1.0, spinningFriction=1.0):
pybullet.changeDynamics(bodyUniqueId=self.id,
linkIndex=-1,
lateralFriction=lateralFriction,
spinningFriction=spinningFriction)
print("Floor friction updated!")
print("lateralFriction:", pybullet.getDynamicsInfo(self.id, -1)[1])
print("spinningFriction:", pybullet.getDynamicsInfo(self.id, -1)[7])
def _RecordMassInfoFromURDF(self):
self._base_mass_urdf = pybullet.getDynamicsInfo(
self.quadruped, BASE_LINK_ID)[0]
self._leg_masses_urdf = []
self._leg_masses_urdf.append(
pybullet.getDynamicsInfo(self.quadruped, LEG_LINK_ID[0])[0])
self._leg_masses_urdf.append(
pybullet.getDynamicsInfo(self.quadruped, MOTOR_LINK_ID[0])[0])
def initializeFromBulletBody(self, bodyUid, physicsClientId):
self.initialize()
#always create a base link
baseLink = UrdfLink()
baseLinkIndex = -1
self.convertLinkFromMultiBody(bodyUid, baseLinkIndex, baseLink, physicsClientId)
baseLink.link_name = p.getBodyInfo(bodyUid, physicsClientId=physicsClientId)[0].decode("utf-8")
self.robotName = p.getBodyInfo(bodyUid, physicsClientId=physicsClientId)[1].decode("utf-8")
self.linkNameToIndex[baseLink.link_name] = len(self.urdfLinks)
self.urdfLinks.append(baseLink)
#optionally create child links and joints
for j in range(p.getNumJoints(bodyUid, physicsClientId=physicsClientId)):
jointInfo = p.getJointInfo(bodyUid, j, physicsClientId=physicsClientId)
urdfLink = UrdfLink()
self.convertLinkFromMultiBody(bodyUid, j, urdfLink, physicsClientId)
urdfLink.link_name = jointInfo[12].decode("utf-8")
self.linkNameToIndex[urdfLink.link_name] = len(self.urdfLinks)
self.urdfLinks.append(urdfLink)
urdfJoint = UrdfJoint()
urdfJoint.link = urdfLink
urdfJoint.joint_name = jointInfo[1].decode("utf-8")
urdfJoint.joint_type = jointInfo[2]
urdfJoint.joint_lower_limit = jointInfo[8]
urdfJoint.joint_upper_limit = jointInfo[9]
urdfJoint.joint_axis_xyz = jointInfo[13]
orgParentIndex = jointInfo[16]
if (orgParentIndex < 0):
urdfJoint.parent_name = baseLink.link_name
else:
parentJointInfo = p.getJointInfo(bodyUid, orgParentIndex, physicsClientId=physicsClientId)
urdfJoint.parent_name = parentJointInfo[12].decode("utf-8")
urdfJoint.child_name = urdfLink.link_name
#todo, compensate for inertia/link frame offset
dynChild = p.getDynamicsInfo(bodyUid, j, physicsClientId=physicsClientId)
childInertiaPos = dynChild[3]
childInertiaOrn = dynChild[4]
parentCom2JointPos = jointInfo[14]
parentCom2JointOrn = jointInfo[15]
tmpPos, tmpOrn = p.multiplyTransforms(childInertiaPos, childInertiaOrn, parentCom2JointPos,
parentCom2JointOrn)
tmpPosInv, tmpOrnInv = p.invertTransform(tmpPos, tmpOrn)
dynParent = p.getDynamicsInfo(bodyUid, orgParentIndex, physicsClientId=physicsClientId)
parentInertiaPos = dynParent[3]
parentInertiaOrn = dynParent[4]
pos, orn = p.multiplyTransforms(parentInertiaPos, parentInertiaOrn, tmpPosInv, tmpOrnInv)
pos, orn_unused = p.multiplyTransforms(parentInertiaPos, parentInertiaOrn,
parentCom2JointPos, [0, 0, 0, 1])
urdfJoint.joint_origin_xyz = pos
urdfJoint.joint_origin_rpy = p.getEulerFromQuaternion(orn)
self.urdfJoints.append(urdfJoint)
def preset(self):
pb.resetSimulation(physicsClientId = self.pybullet_client_ID)
pb.setGravity(0,0,-9.8, physicsClientId = self.pybullet_client_ID)
self.tau = 1.0/60
pb.setPhysicsEngineParameter(fixedTimeStep=self.tau, numSolverIterations=5, numSubSteps=2, physicsClientId = self.pybullet_client_ID)
# Load objects
pb.loadURDF("plane.urdf", physicsClientId = self.pybullet_client_ID)
self.robotId = pb.loadURDF("humanoid.xml",flags = pb.URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS, physicsClientId = self.pybullet_client_ID)
# Initialize robot
# TODO ; Choose init_frame randomly
self.init_frame = 0
wxyz = self.data[self.init_frame]['qwxyz_root']
pb.resetBasePositionAndOrientation(self.robotId, self.data[self.init_frame]['root_pos'], wxyz[[1,2,3,0]], physicsClientId = self.pybullet_client_ID)
# Extract model information
self.motor_names = []
self.motor_power = []
self.motor_limit = []
self.revolute_joints = 0
self.end_effectors = ['link_right_ankle', 'link_left_ankle', 'link_right_wrist', 'link_left_wrist']
self.reward_links = ['link_chest',
'link_left_ankle',
'link_right_ankle',
'link_right_knee',
'link_left_knee',
'link_neck',
'link_right_elbow',
'link_left_elbow',
'link_left_hip',
'link_right_hip',
'link_left_shoulder',
'link_right_shoulder']
self.label2index = dict()
for joint_index in range(pb.getNumJoints(self.robotId, physicsClientId = self.pybullet_client_ID)):
joint_data = pb.getJointInfo(self.robotId, joint_index, physicsClientId = self.pybullet_client_ID)
link_name = joint_data[12].decode("ascii")
self.label2index[link_name] = joint_index
if joint_data[2] != pb.JOINT_REVOLUTE:
continue
self.revolute_joints += 1
self.motor_names.append(link_name)
lower_lim, upper_lim = (joint_data[8], joint_data[9])
self.motor_limit.append((lower_lim, upper_lim))
self.motor_power.append(joint_data[10])
self.link_masses = {'root':pb.getDynamicsInfo(self.robotId, -1)[0]}
for link_label in self.reward_links:
self.link_masses[link_label] = pb.getDynamicsInfo(self.robotId, self.label2index[link_label])[0]
self.nframes = len(self.data)
self.motors = [self.label2index[label] for label in self.motor_names]
def initializeFromBulletBody(self, bodyUid, physicsClientId):
self.initialize()
#always create a base link
baseLink = UrdfLink()
baseLinkIndex = -1
self.convertLinkFromMultiBody(bodyUid, baseLinkIndex, baseLink, physicsClientId)
baseLink.link_name = p.getBodyInfo(bodyUid, physicsClientId=physicsClientId)[0].decode("utf-8")
self.linkNameToIndex[baseLink.link_name]=len(self.urdfLinks)
self.urdfLinks.append(baseLink)
#print(visualShapes)
#optionally create child links and joints
for j in range(p.getNumJoints(bodyUid,physicsClientId=physicsClientId)):
jointInfo = p.getJointInfo(bodyUid,j,physicsClientId=physicsClientId)
urdfLink = UrdfLink()
self.convertLinkFromMultiBody(bodyUid, j, urdfLink,physicsClientId)
urdfLink.link_name = jointInfo[12].decode("utf-8")
self.linkNameToIndex[urdfLink.link_name]=len(self.urdfLinks)
self.urdfLinks.append(urdfLink)
urdfJoint = UrdfJoint()
urdfJoint.link = urdfLink
urdfJoint.joint_name = jointInfo[1].decode("utf-8")
urdfJoint.joint_type = jointInfo[2]
urdfJoint.joint_axis_xyz = jointInfo[13]
orgParentIndex = jointInfo[16]
if (orgParentIndex<0):
urdfJoint.parent_name = baseLink.link_name
else:
parentJointInfo = p.getJointInfo(bodyUid,orgParentIndex,physicsClientId=physicsClientId)
urdfJoint.parent_name = parentJointInfo[12].decode("utf-8")
urdfJoint.child_name = urdfLink.link_name
#todo, compensate for inertia/link frame offset
dynChild = p.getDynamicsInfo(bodyUid,j,physicsClientId=physicsClientId)
childInertiaPos = dynChild[3]
childInertiaOrn = dynChild[4]
parentCom2JointPos=jointInfo[14]
parentCom2JointOrn=jointInfo[15]
tmpPos,tmpOrn = p.multiplyTransforms(childInertiaPos,childInertiaOrn,parentCom2JointPos,parentCom2JointOrn)
tmpPosInv,tmpOrnInv = p.invertTransform(tmpPos,tmpOrn)
dynParent = p.getDynamicsInfo(bodyUid,orgParentIndex,physicsClientId=physicsClientId)
parentInertiaPos = dynParent[3]
parentInertiaOrn = dynParent[4]
pos,orn = p.multiplyTransforms(parentInertiaPos,parentInertiaOrn, tmpPosInv, tmpOrnInv)
pos,orn_unused=p.multiplyTransforms(parentInertiaPos,parentInertiaOrn, parentCom2JointPos,[0,0,0,1])
urdfJoint.joint_origin_xyz = pos
urdfJoint.joint_origin_rpy = p.getEulerFromQuaternion(orn)
self.urdfJoints.append(urdfJoint)
def changeFriction(self,
lateralFriction=1.0,
spinningFriction=1.0,
rollingFriction=0.0):
print("Current table dynamic: ", pybullet.getDynamicsInfo(self.id, -1))
pybullet.changeDynamics(bodyUniqueId=self.id,
linkIndex=-1,
lateralFriction=lateralFriction,
spinningFriction=spinningFriction,
rollingFriction=rollingFriction)
print("Updated table dynamic: ", pybullet.getDynamicsInfo(self.id, -1))
def ResetSimulation(self):
pybullet.resetSimulation()
pybullet.setGravity(0,0,-9.81)
pybullet.setTimeStep(self.timestep)
# Read in configuration file
config = yaml.load(open(self.config))
if config["with_ground"] == True:
self.ground_id = load_pybullet_from_urdf_or_sdf(os.environ["SPARTAN_SOURCE_DIR"] + "/build/bullet3/data/plane.urdf")
else:
self.ground_id = None
# Load in the Kuka
if config["robot"]:
q0 = config["robot"]["base_pose"]
position = q0[0:3]
quaternion = pybullet.getQuaternionFromEuler(q0[3:6])
self.kuka_id = load_pybullet_from_urdf_or_sdf(kIiwaUrdf, position, quaternion, True, self.packageMap)
for obj_id in range(-1, pybullet.getNumJoints(self.kuka_id)):
print "Dynamic info for body %d, %d:" % (self.kuka_id, obj_id)
print pybullet.getDynamicsInfo(self.kuka_id, obj_id)
pybullet.changeDynamics(self.kuka_id, obj_id, lateralFriction=0.9, spinningFriction=0.9, frictionAnchor=1)
self.BuildJointNameInfo()
self.BuildMotorIdList()
# Models entry is a dictionary of model URDF strings
model_dict = config["models"]
self.object_ids = []
# Add each model as requested
self.rbts = []
for instance in config["instances"]:
q0 = instance["q0"]
position = q0[0:3]
quaternion = pybullet.getQuaternionFromEuler(q0[3:8])
fixed = instance["fixed"]
self.object_ids.append(load_pybullet_from_urdf_or_sdf(model_dict[instance["model"]], position, quaternion, fixed))
# Report all friction properties
for obj_id in range(-1, pybullet.getNumJoints(self.object_ids[-1])):
print "Dynamic info for body %d, %d:" % (self.object_ids[-1], obj_id)
print pybullet.getDynamicsInfo(self.object_ids[-1], obj_id)
pybullet.changeDynamics(self.object_ids[-1], obj_id, lateralFriction=0.9, spinningFriction=0.9, frictionAnchor=0)
self.rbts.append(load_drake_from_urdf_or_sdf(model_dict[instance["model"]]))
# Set up camera info (which relies on having models loaded
# so we know where to mount the camera)
self.rgbd_info = RgbdCameraMetaInfo(self.camera_serial_number, self.link_to_joint_id_map)
def __init__(self):
self.pcId = p.connect(p.GUI)
p.resetDebugVisualizerCamera(1, 0, 0, [0, 0, 0.3])
p.configureDebugVisualizer(p.COV_ENABLE_SHADOWS, 0)
#p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0)
urdf_path = ""
self.groundId = p.loadURDF(urdf_path + "/plane_001/plane.urdf",
[0, 0, -1],
physicsClientId=self.pcId)
#self.robotId = p.loadURDF(urdf_path + "/robot_001/robot.urdf", [0,0,1], physicsClientId=self.pcId)
self.robotId = p.loadURDF(urdf_path + "/robot_002_1/robot.urdf",
[0, 0, 0],
flags=p.URDF_MERGE_FIXED_LINKS,
physicsClientId=self.pcId)
#self.robotId = p.loadURDF(urdf_path + "/robot_002_1/robot.urdf", [0,0,0], physicsClientId=self.pcId)
#self.urdf_joint_indexes = [0,1,2,3,4,5,6,7,8,9,10,11]
self.urdf_joint_indexes = [2, 4, 8, 13, 15, 19, 24, 26, 30, 35, 37, 41]
self.timeStep = 1 / 240
p.setPhysicsEngineParameter(fixedTimeStep=self.timeStep,
physicsClientId=self.pcId)
p.resetBasePositionAndOrientation(self.robotId, [0, 0, 0],
[0, 0, 0, 1],
physicsClientId=self.pcId)
base_pos, base_rot = p.getBasePositionAndOrientation(
self.robotId, physicsClientId=self.pcId)
p.resetDebugVisualizerCamera(1,
30,
-15,
base_pos,
physicsClientId=self.pcId)
print(p.getNumJoints(self.robotId, physicsClientId=self.pcId))
zero_pos = -np.asarray(
p.getLinkState(self.robotId, 0, physicsClientId=self.pcId)[0])
print(
"body_mass :",
p.getDynamicsInfo(self.robotId, -1, physicsClientId=self.pcId)[0])
leg_mass = 0
# p.getNumJoints(self.robotId, physicsClientId=self.pcId)
for i in range(3):
leg_mass += p.getDynamicsInfo(self.robotId,
i,
physicsClientId=self.pcId)[0]
print("leg_mass :", leg_mass)
def get_current_variable_values(self):
"""
:return: (dict) returns all the exposed variables in the environment along with their
corresponding values.
"""
# TODO: not a complete list yet of what we want to expose
variable_params = dict()
variable_params['joint_positions'] = self._latest_full_state[
'positions']
variable_params['control_index'] = self._control_index
variable_params['joint_velocities'] = self._latest_full_state[
'velocities']
if self._pybullet_client_w_o_goal_id is not None:
client = self._pybullet_client_w_o_goal_id
else:
client = self._pybullet_client_full_id
position, _ = pybullet. \
getBasePositionAndOrientation(WorldConstants.ROBOT_ID,
physicsClientId=
client)
variable_params[
'robot_height'] = position[-1] + WorldConstants.ROBOT_HEIGHT
for robot_finger_link in WorldConstants.LINK_IDS:
variable_params[robot_finger_link] = dict()
variable_params[robot_finger_link]['color'] = \
pybullet.getVisualShapeData(WorldConstants.ROBOT_ID,
physicsClientId=client)\
[WorldConstants.VISUAL_SHAPE_IDS[robot_finger_link]][7][:3]
variable_params[robot_finger_link]['mass'] = \
pybullet.getDynamicsInfo(WorldConstants.ROBOT_ID,
WorldConstants.LINK_IDS[robot_finger_link],
physicsClientId=client)[0]
return variable_params
def step(self, action=None, vis_frames=False):
# Apply every action.
if action and action.__class__.__name__ == 'PushAction':
hand_pos = action.step()
for world in self.worlds:
world.set_hand_pos(hand_pos, self.pybullet_server.client)
# forward step the sim
self.pybullet_server.step()
# update all world object poses
for world in self.worlds:
for obj in world.objects:
pos, orn = self.pybullet_server.get_pose(obj.pb_id)
dynamics = p.getDynamicsInfo(
bodyUniqueId=obj.pb_id,
linkIndex=-1,
physicsClientId=self.pybullet_server.client)
(point, quat) = p.multiplyTransforms(
pos, orn, *p.invertTransform(dynamics[3], dynamics[4]))
# pyBullet returns the pose of the COM, not COG
obj.set_pose(Pose(Position(*point), Quaternion(*quat)))
if vis_frames:
#pos, quat = obj.get_pose()
self.pybullet_server.vis_frame(pos, orn)
# sleep (for visualization purposes)
if self.vis_sim:
time.sleep(0.005)
def add_body_friction_noise(uid, link, noise_level):
dynamics = p.getDynamicsInfo(uid, link)
noise_range = np.fabs(dynamics[1]) * noise_level
friction_noise = np.random.normal(0, noise_range)
p.changeDynamics(uid,
link,
lateralFriction=dynamics[1] + friction_noise)
def drawInertiaBox(parentUid, parentLinkIndex): mass,frictionCoeff, inertia =p.getDynamicsInfo(bodyUniqueId=parentUid,linkIndex=parentLinkIndex, flags = p.DYNAMICS_INFO_REPORT_INERTIA) Ixx = inertia[0] Iyy = inertia[1] Izz = inertia[2] boxScaleX = 0.5*math.sqrt(6*(Izz + Iyy - Ixx) / mass); boxScaleY = 0.5*math.sqrt(6*(Izz + Ixx - Iyy) / mass); boxScaleZ = 0.5*math.sqrt(6*(Ixx + Iyy - Izz) / mass); halfExtents = [boxScaleX,boxScaleY,boxScaleZ] pts = [[halfExtents[0],halfExtents[1],halfExtents[2]], [-halfExtents[0],halfExtents[1],halfExtents[2]], [halfExtents[0],-halfExtents[1],halfExtents[2]], [-halfExtents[0],-halfExtents[1],halfExtents[2]], [halfExtents[0],halfExtents[1],-halfExtents[2]], [-halfExtents[0],halfExtents[1],-halfExtents[2]], [halfExtents[0],-halfExtents[1],-halfExtents[2]], [-halfExtents[0],-halfExtents[1],-halfExtents[2]]] color=[1,0,0] p.addUserDebugLine(pts[0],pts[1],color,1, parentObjectUniqueId=parentUid, parentLinkIndex = parentLinkIndex) p.addUserDebugLine(pts[1],pts[3],color,1, parentObjectUniqueId=parentUid, parentLinkIndex = parentLinkIndex) p.addUserDebugLine(pts[3],pts[2],color,1, parentObjectUniqueId=parentUid, parentLinkIndex = parentLinkIndex) p.addUserDebugLine(pts[2],pts[0],color,1, parentObjectUniqueId=parentUid, parentLinkIndex = parentLinkIndex) p.addUserDebugLine(pts[0],pts[4],color,1, parentObjectUniqueId=parentUid, parentLinkIndex = parentLinkIndex) p.addUserDebugLine(pts[1],pts[5],color,1, parentObjectUniqueId=parentUid, parentLinkIndex = parentLinkIndex) p.addUserDebugLine(pts[2],pts[6],color,1, parentObjectUniqueId=parentUid, parentLinkIndex = parentLinkIndex) p.addUserDebugLine(pts[3],pts[7],color,1, parentObjectUniqueId=parentUid, parentLinkIndex = parentLinkIndex) p.addUserDebugLine(pts[4+0],pts[4+1],color,1, parentObjectUniqueId=parentUid, parentLinkIndex = parentLinkIndex) p.addUserDebugLine(pts[4+1],pts[4+3],color,1, parentObjectUniqueId=parentUid, parentLinkIndex = parentLinkIndex) p.addUserDebugLine(pts[4+3],pts[4+2],color,1, parentObjectUniqueId=parentUid, parentLinkIndex = parentLinkIndex) p.addUserDebugLine(pts[4+2],pts[4+0],color,1, parentObjectUniqueId=parentUid, parentLinkIndex = parentLinkIndex)
def _load(self):
body_id = p.loadURDF(self.filename,
globalScaling=self.scale,
flags=p.URDF_USE_MATERIAL_COLORS_FROM_MTL)
self.mass = p.getDynamicsInfo(body_id, -1)[0]
return body_id
def _get_pose_com_(self, frame):
com_numerator = np.array([0.0, 0.0, 0.0])
for link_index in range(p.getNumJoints(self.id)):
link_com = p.getLinkState(self.id, link_index)[0]
link_mass = p.getDynamicsInfo(self.id, link_index)[0]
com_numerator = np.add(com_numerator,
np.multiply(link_mass, link_com))
p_com_world = np.divide(com_numerator, self._mass)
p_base_world, q_base_world = p.getBasePositionAndOrientation(self.id)
q_com_world = q_base_world
if frame == 'world':
return p_com_world, q_com_world
elif frame == 'tip':
p_tip_world = self._get_p_tip_world()
p_com_tip = util.transformation(p_com_world,
p_tip_world,
q_base_world,
inverse=True)
q_com_tip = np.array([0., 0., 0., 1.])
return p_com_tip, q_com_tip
elif frame == 'base':
p_com_base = util.transformation(p_com_world,
p_base_world,
q_base_world,
inverse=True)
q_com_base = np.array([0.0, 0.0, 0.0, 1.0])
return p_com_base, q_com_base
def reset(self):
"""
Reset the object to its original pose and joint configuration
"""
for idx in range(len(self.body_ids)):
body_id = self.body_ids[idx]
transformation = self.poses[idx]
pos = transformation[0:3, 3]
orn = np.array(quatXYZWFromRotMat(transformation[0:3, 0:3]))
logging.info("Resetting URDF to (pos,ori): " +
np.array_str(pos) + ", " + np.array_str(orn))
dynamics_info = p.getDynamicsInfo(body_id, -1)
inertial_pos, inertial_orn = dynamics_info[3], dynamics_info[4]
pos, orn = p.multiplyTransforms(
pos, orn, inertial_pos, inertial_orn)
p.resetBasePositionAndOrientation(body_id, pos, orn)
# reset joint position to 0.0
for j in range(p.getNumJoints(body_id)):
info = p.getJointInfo(body_id, j)
jointType = info[2]
if jointType in [p.JOINT_REVOLUTE, p.JOINT_PRISMATIC]:
p.resetJointState(
body_id, j, targetValue=0.0, targetVelocity=0.0)
p.setJointMotorControl2(
body_id, j, p.VELOCITY_CONTROL,
targetVelocity=0.0, force=self.joint_friction)
def get_world_link_pose(body_unique_id: int, link_index: int):
"""Pose of link frame with respect to world frame expressed in world frame.
Args:
body_unique_id (int): The body unique id, as returned by loadURDF, etc.
link_index (int): Link index or -1 for the base.
Returns:
pos_orn (tuple): See description.
"""
if link_index == -1:
world_inertial_pose = get_world_inertial_pose(body_unique_id, -1)
dynamics_info = p.getDynamicsInfo(body_unique_id, -1)
local_inertial_pose = (dynamics_info[3], dynamics_info[4])
local_inertial_pose_inv = p.invertTransform(local_inertial_pose[0],
local_inertial_pose[1])
pos_orn = p.multiplyTransforms(world_inertial_pose[0],
world_inertial_pose[1],
local_inertial_pose_inv[0],
local_inertial_pose_inv[1])
else:
state = p.getLinkState(body_unique_id, link_index)
pos_orn = (state[4], state[5])
return pos_orn
def _load(self):
"""
Load the object into pybullet and set it to the correct pose
"""
for idx in range(len(self.urdf_paths)):
logging.info("Loading " + self.urdf_paths[idx])
body_id = p.loadURDF(self.urdf_paths[idx])
# flags=p.URDF_USE_MATERIAL_COLORS_FROM_MTL)
transformation = self.poses[idx]
pos = transformation[0:3, 3]
orn = np.array(quatXYZWFromRotMat(transformation[0:3, 0:3]))
logging.info("Moving URDF to (pos,ori): " +
np.array_str(pos) + ", " + np.array_str(orn))
dynamics_info = p.getDynamicsInfo(body_id, -1)
inertial_pos, inertial_orn = dynamics_info[3], dynamics_info[4]
pos, orn = p.multiplyTransforms(
pos, orn, inertial_pos, inertial_orn)
p.resetBasePositionAndOrientation(body_id, pos, orn)
p.changeDynamics(
body_id, -1,
activationState=p.ACTIVATION_STATE_ENABLE_SLEEPING)
for j in range(p.getNumJoints(body_id)):
info = p.getJointInfo(body_id, j)
jointType = info[2]
if jointType in [p.JOINT_REVOLUTE, p.JOINT_PRISMATIC]:
p.setJointMotorControl2(
body_id, j, p.VELOCITY_CONTROL,
targetVelocity=0.0, force=self.joint_friction)
self.body_ids.append(body_id)
return self.body_ids
def getCenterOfMassPosition(self):
"""Returns center of mass of the robot
Returns:
pos -- (x, y, z) robot center of mass
"""
k = -1
mass = 0
com = np.array([0., 0., 0.])
while True:
if k == -1:
pos, _ = p.getBasePositionAndOrientation(self.robot)
else:
res = p.getLinkState(self.robot, k)
if res is None:
break
pos = res[0]
d = p.getDynamicsInfo(self.robot, k)
m = d[0]
com += np.array(pos) * m
mass += m
k += 1
return com / mass
def __init__(self, mech, k=[2000.0, 20.0], d=[0.45, 0.45]):
"""
This class defines the actions a gripper can take such as grasping a handle
and executing PD control
:param mech: the gen.generator_busybox.Mechanism being actuated
:param k: a vector of length 2 where the first entry is the linear position
(stiffness) gain and the second entry is the angular position gain
:param d: a vector of length 2 where the first entry is the linear derivative
(damping) gain and the second entry is the angular derivative gain
"""
self.use_gripper = False
if self.use_gripper:
self.id = p.loadSDF("models/gripper/gripper_high_fric.sdf")[0]
self._left_finger_tip_id = 2
self._right_finger_tip_id = 5
self._left_finger_base_joint_id = 0
self._right_finger_base_joint_id = 3
self._finger_force = 20
self.pose_tip_world_reset = util.Pose([0.0, 0.0, 0.2], \
[0.50019904, 0.50019904, -0.49980088, 0.49980088])
# get mass of gripper
mass = 0
for link in range(p.getNumJoints(self.id)):
mass += p.getDynamicsInfo(self.id, link)[0]
self._mass = mass
self.errors = []
self.forces = []
# control parameters
self.k = k
self.d = d
def get_mass(self, linkID=None):
# TOOD: get full mass
if linkID is None:
linkID = self.base_link
#return self.get_dynamics_info(linkID).mass
#TODO hack for now:
return p.getDynamicsInfo(self.id, linkID)[0]
def print_physics_params(self):
# Query all the joints.
num_joints = pybullet.getNumJoints(self.robotId)
for ji in range(num_joints):
(
mass,
lateral_friction,
local_inertia_diag,
local_inertia_pos,
local_inertia_ori,
resitution,
rolling_friction,
spinning_friction,
contact_damping,
contact_stiffness,
) = pybullet.getDynamicsInfo(bodyUniqueId=self.robotId,
linkIndex=ji)
# for el in dynamics_info:
# print(el)
print("link ", ji)
print(" - mass : ", mass)
print(" - lateral_friction : ", lateral_friction)
print(" - local_inertia_diag : ", local_inertia_diag)
print(" - local_inertia_pos : ", local_inertia_pos)
print(" - local_inertia_ori : ", local_inertia_ori)
print(" - resitution : ", resitution)
print(" - rolling_friction : ", rolling_friction)
print(" - spinning_friction : ", spinning_friction)
print(" - contact_damping : ", contact_damping)
print(" - contact_stiffness : ", contact_stiffness)
def convertLinkFromMultiBody(self, bodyUid, linkIndex, urdfLink, physicsClientId):
dyn = p.getDynamicsInfo(bodyUid, linkIndex, physicsClientId=physicsClientId)
urdfLink.urdf_inertial.mass = dyn[0]
urdfLink.urdf_inertial.inertia_xxyyzz = dyn[2]
# todo
urdfLink.urdf_inertial.origin_xyz = dyn[3]
urdfLink.urdf_inertial.origin_rpy = p.getEulerFromQuaternion(dyn[4])
visualShapes = p.getVisualShapeData(bodyUid, physicsClientId=physicsClientId)
matIndex = 0
for v in visualShapes:
if (v[1] == linkIndex):
urdfVisual = UrdfVisual()
urdfVisual.geom_type = v[2]
if (v[2] == p.GEOM_BOX):
urdfVisual.geom_extents = v[3]
if (v[2] == p.GEOM_SPHERE):
urdfVisual.geom_radius = v[3][0]
if (v[2] == p.GEOM_MESH):
urdfVisual.geom_meshfilename = v[4].decode("utf-8")
if urdfVisual.geom_meshfilename == UNKNOWN_FILE:
continue
urdfVisual.geom_meshscale = v[3]
if (v[2] == p.GEOM_CYLINDER):
urdfVisual.geom_length = v[3][0]
urdfVisual.geom_radius = v[3][1]
if (v[2] == p.GEOM_CAPSULE):
urdfVisual.geom_length = v[3][0]
urdfVisual.geom_radius = v[3][1]
urdfVisual.origin_xyz = v[5]
urdfVisual.origin_rpy = p.getEulerFromQuaternion(v[6])
urdfVisual.material_rgba = v[7]
name = 'mat_{}_{}'.format(linkIndex, matIndex)
urdfVisual.material_name = name
urdfLink.urdf_visual_shapes.append(urdfVisual)
matIndex = matIndex + 1
collisionShapes = p.getCollisionShapeData(bodyUid, linkIndex, physicsClientId=physicsClientId)
for v in collisionShapes:
urdfCollision = UrdfCollision()
urdfCollision.geom_type = v[2]
if (v[2] == p.GEOM_BOX):
urdfCollision.geom_extents = v[3]
if (v[2] == p.GEOM_SPHERE):
urdfCollision.geom_radius = v[3][0]
if (v[2] == p.GEOM_MESH):
urdfCollision.geom_meshfilename = v[4].decode("utf-8")
if urdfCollision.geom_meshfilename == UNKNOWN_FILE:
continue
urdfCollision.geom_meshscale = v[3]
if (v[2] == p.GEOM_CYLINDER):
urdfCollision.geom_length = v[3][0]
urdfCollision.geom_radius = v[3][1]
if (v[2] == p.GEOM_CAPSULE):
urdfCollision.geom_length = v[3][0]
urdfCollision.geom_radius = v[3][1]
pos, orn = p.multiplyTransforms(dyn[3], dyn[4], v[5], v[6])
urdfCollision.origin_xyz = pos
urdfCollision.origin_rpy = p.getEulerFromQuaternion(orn)
urdfLink.urdf_collision_shapes.append(urdfCollision)
def calcCOM():
#CALCULATE COM
masstimesxpossum = 0.0
masstimesypossum = 0.0
masstimeszpossum = 0.0
masssum = 0.0
for i in range(0, p.getNumJoints(robotId) - 1):
# if(i >= 0):
# print(p.getJointInfo(robotId, i)[0:13])
wheight = p.getDynamicsInfo(robotId, i)[0]
xpos = p.getLinkState(robotId, i)[0][0]
ypos = p.getLinkState(robotId, i)[0][1]
zpos = p.getLinkState(robotId, i)[0][2]
masstimesxpossum += (wheight * xpos)
masstimesypossum += (wheight * ypos)
masstimeszpossum += (wheight * zpos)
masssum += wheight
# print(wheight)
# print(xpos)
# print(ypos)
# print(zpos)
# print("\n")
p.stepSimulation()
com = (masstimesxpossum / masssum, masstimesypossum / masssum,
masstimeszpossum / masssum)
#print("mass: " + str(masssum))
#print("center of mass: " + str(com))
#print("\n")
return com
def loosenModel(self):
nJoints = p.getNumJoints(self.cliffordID,
physicsClientId=self.physicsClientId)
tireIndices = [
self.linkNameToID[name]
for name in ['frtire', 'fltire', 'brtire', 'bltire']
]
for i in range(nJoints):
if len(p.getJointStateMultiDof(bodyUniqueId=self.cliffordID,jointIndex=i,physicsClientId=self.physicsClientId)[0]) == 4 \
and not self.params["fixedSuspension"]:
p.setJointMotorControlMultiDof(
bodyUniqueId=self.cliffordID,
jointIndex=i,
controlMode=p.POSITION_CONTROL,
targetPosition=[0, 0, 0, 1],
positionGain=0,
velocityGain=0,
force=[0, 0, 0],
physicsClientId=self.physicsClientId)
dynamicsData = p.getDynamicsInfo(
self.cliffordID, i, physicsClientId=self.physicsClientId)
massScale = self.params["massScale"]
if i in tireIndices:
massScale = massScale * self.params['tireMassScale']
newMass = dynamicsData[0] * massScale
newInertia = [
dynamicsData[2][j] * massScale
for j in range(len(dynamicsData[2]))
]
p.changeDynamics(self.cliffordID,
i,
mass=newMass,
localInertiaDiagonal=newInertia,
linearDamping=0.2,
angularDamping=0.2,
restitution=0,
physicsClientId=self.physicsClientId)
springJointNames = [
'brslower2upper', 'blslower2upper', 'frslower2upper',
'flslower2upper'
]
springConstant = 0
springDamping = 0
maxSpringForce = 0
for name in springJointNames:
#JointInfo = p.getJointInfo(self.cliffordID,self.jointNameToID[name],physicsClientId=self.physicsClientId)
#p.setJointMotorControlArray(bodyUniqueId=self.cliffordID,
# jointIndices=[self.jointNameToID[name]],
# controlMode=p.POSITION_CONTROL,
# targetPositions=[0],
# positionGains=[springConstant],
# velocityGains=[springDamping],
# forces=[maxSpringForce],physicsClientId=self.physicsClientId)
if not self.params["fixedSuspension"]:
p.changeDynamics(self.cliffordID,
self.jointNameToID[name],
jointLowerLimit=self.params["susLowerLimit"],
jointUpperLimit=self.params["susUpperLimit"],
physicsClientId=self.physicsClientId)
def _reset(self):
# print("-----------reset simulation---------------")
p.resetSimulation()
# see https://github.com/bulletphysics/bullet3/issues/1934 to load multiple colors
if self.render:
visualShapeId = p.createVisualShape(shapeType=p.GEOM_SPHERE,
radius=0.1,
rgbaColor=[1, 0, 0, 1],
specularColor=[0.4, .4, 0])
self.desired_pos_sphere = p.createMultiBody(
baseMass=0,
baseInertialFramePosition=[0, 0, 0],
baseVisualShapeIndex=visualShapeId)
rand_ori = self.np_random.uniform(
low=-1, high=1, size=(4, )) + np.asarray([0, 0, 0, 2])
rand_ori = rand_ori / np.linalg.norm(rand_ori)
rand_pos = self.np_random.uniform(low=-0.5, high=0.5, size=(3, ))
#TODO Start with random vel
self.quad = p.loadURDF(os.path.join(currentdir, "quad.urdf"),
[0, 0, 0.5] + rand_pos,
rand_ori,
flags=p.URDF_USE_INERTIA_FROM_FILE)
filename = os.path.join(pybullet_data.getDataPath(),
"plane_stadium.sdf")
self.ground_plane_mjcf = p.loadSDF(filename)
# filename = os.path.join(pybullet_data.getDataPath(),"stadium_no_collision.sdf")
# self.ground_plane_mjcf = self._p.loadSDF(filename)
#
for i in self.ground_plane_mjcf:
p.changeDynamics(i, -1, lateralFriction=0.8, restitution=0.5)
p.changeVisualShape(i, -1, rgbaColor=[1, 1, 1, 0.8])
self.timeStep = 0.01
self.gravity = -9.8
p.setGravity(0, 0, self.gravity)
# Default 0.04 for linear and angular damping.
# p.changeDynamics(self.quad, -1, linearDamping=1)
# p.changeDynamics(self.quad, -1, angularDamping=1)
p.setTimeStep(self.timeStep)
p.setRealTimeSimulation(0)
info = p.getDynamicsInfo(self.quad, -1)
self.mass = info[0]
self.zero_thrust = -self.mass * self.gravity
initialCartPos = self.np_random.uniform(low=-2, high=2, size=(1, ))
initialAngle = self.np_random.uniform(low=-0.5, high=0.5, size=(1, ))
# p.resetJointState(self.quad, 1, initialAngle)
# p.resetJointState(self.quad, 0, initialCartPos)
world_pos, world_ori = p.getBasePositionAndOrientation(self.quad)
world_vel, world_rot_vel = p.getBaseVelocity(self.quad)
self.state = world_pos + world_ori + world_vel + world_rot_vel
return np.array(self.state)
def _load(self):
body_id = p.loadURDF(self.filename,
globalScaling=self.scale,
useFixedBase=1,
flags=p.URDF_USE_SELF_COLLISION)
self.mass = p.getDynamicsInfo(body_id, -1)[0]
return body_id
def step(self, action):
p.configureDebugVisualizer(p.COV_ENABLE_SINGLE_STEP_RENDERING)
agent1_pose = np.concatenate(
p.getBasePositionAndOrientation(self.agent1Uid))
agent2_pose = np.concatenate(
p.getBasePositionAndOrientation(self.agent2Uid))
agent3_pose = np.concatenate(
p.getBasePositionAndOrientation(self.agent3Uid))
self.observation = np.concatenate(
[agent1_pose, agent2_pose, agent3_pose])
box_position = p.getBasePositionAndOrientation(self.objectUid)[:3]
done = False
reward = 0
info = {'box_position': box_position}
#gravity compensation
action[:3] = action[:3] + np.array([0, 0, 10]) * p.getDynamicsInfo(
self.agent1Uid, -1)[0]
action[3:6] = action[3:6] + np.array([0, 0, 10]) * p.getDynamicsInfo(
self.agent2Uid, -1)[0]
action[6:9] = action[6:9] + np.array([0, 0, 10]) * p.getDynamicsInfo(
self.agent3Uid, -1)[0]
#damping, only consider linear vel
damping = 15.0
agent1_vel, _ = p.getBaseVelocity(self.agent1Uid)
agent2_vel, _ = p.getBaseVelocity(self.agent2Uid)
agent3_vel, _ = p.getBaseVelocity(self.agent3Uid)
action[:3] = action[:3] - np.array(agent1_vel) * damping
action[3:6] = action[3:6] - np.array(agent2_vel) * damping
action[6:9] = action[6:9] - np.array(agent3_vel) * damping
#apply force to each agent
p.applyExternalForce(self.agent1Uid, -1, action[:3], agent1_pose[:3],
p.WORLD_FRAME)
p.applyExternalForce(self.agent2Uid, -1, action[3:6], agent2_pose[:3],
p.WORLD_FRAME)
p.applyExternalForce(self.agent3Uid, -1, action[6:9], agent3_pose[:3],
p.WORLD_FRAME)
p.stepSimulation()
return np.array(self.observation).astype(
np.float32), reward, done, info
def convertLinkFromMultiBody(self, bodyUid, linkIndex, urdfLink, physicsClientId):
dyn = p.getDynamicsInfo(bodyUid,linkIndex,physicsClientId=physicsClientId)
urdfLink.urdf_inertial.mass = dyn[0]
urdfLink.urdf_inertial.inertia_xxyyzz = dyn[2]
#todo
urdfLink.urdf_inertial.origin_xyz = dyn[3]
rpy = p.getEulerFromQuaternion(dyn[4])
urdfLink.urdf_inertial.origin_rpy = rpy
visualShapes = p.getVisualShapeData(bodyUid,physicsClientId=physicsClientId)
matIndex = 0
for v in visualShapes:
if (v[1]==linkIndex):
urdfVisual = UrdfVisual()
urdfVisual.geom_type = v[2]
if (v[2]==p.GEOM_BOX):
urdfVisual.geom_extents = v[3]
if (v[2]==p.GEOM_SPHERE):
urdfVisual.geom_radius = v[3][0]
if (v[2]==p.GEOM_MESH):
urdfVisual.geom_meshfilename = v[4].decode("utf-8")
urdfVisual.geom_meshscale = v[3]
if (v[2]==p.GEOM_CYLINDER):
urdfVisual.geom_length=v[3][0]
urdfVisual.geom_radius=v[3][1]
if (v[2]==p.GEOM_CAPSULE):
urdfVisual.geom_length=v[3][0]
urdfVisual.geom_radius=v[3][1]
urdfVisual.origin_xyz = v[5]
urdfVisual.origin_rpy = p.getEulerFromQuaternion(v[6])
urdfVisual.material_rgba = v[7]
name = 'mat_{}_{}'.format(linkIndex,matIndex)
urdfVisual.material_name = name
urdfLink.urdf_visual_shapes.append(urdfVisual)
matIndex=matIndex+1
collisionShapes = p.getCollisionShapeData(bodyUid, linkIndex,physicsClientId=physicsClientId)
for v in collisionShapes:
urdfCollision = UrdfCollision()
urdfCollision.geom_type = v[2]
if (v[2]==p.GEOM_BOX):
urdfCollision.geom_extents = v[3]
if (v[2]==p.GEOM_SPHERE):
urdfCollision.geom_radius = v[3][0]
if (v[2]==p.GEOM_MESH):
urdfCollision.geom_meshfilename = v[4].decode("utf-8")
urdfCollision.geom_meshscale = v[3]
if (v[2]==p.GEOM_CYLINDER):
urdfCollision.geom_length=v[3][0]
urdfCollision.geom_radius=v[3][1]
if (v[2]==p.GEOM_CAPSULE):
urdfCollision.geom_length=v[3][0]
urdfCollision.geom_radius=v[3][1]
pos,orn = p.multiplyTransforms(dyn[3],dyn[4],\
v[5], v[6])
urdfCollision.origin_xyz = pos
urdfCollision.origin_rpy = p.getEulerFromQuaternion(orn)
urdfLink.urdf_collision_shapes.append(urdfCollision)
def convertLinkFromMultiBody(self, bodyUid, linkIndex, urdfLink):
dyn = p.getDynamicsInfo(bodyUid, linkIndex)
urdfLink.urdf_inertial.mass = dyn[0]
urdfLink.urdf_inertial.inertia_xxyyzz = dyn[2]
#todo
urdfLink.urdf_inertial.origin_xyz = dyn[3]
rpy = p.getEulerFromQuaternion(dyn[4])
urdfLink.urdf_inertial.origin_rpy = rpy
visualShapes = p.getVisualShapeData(bodyUid)
matIndex = 0
for v in visualShapes:
if (v[1] == linkIndex):
print("visualShape base:", v)
urdfVisual = UrdfVisual()
urdfVisual.geom_type = v[2]
if (v[2] == p.GEOM_BOX):
urdfVisual.geom_extents = v[3]
if (v[2] == p.GEOM_SPHERE):
urdfVisual.geom_radius = v[3][0]
if (v[2] == p.GEOM_MESH):
urdfVisual.geom_meshfile = v[4].decode("utf-8")
if (v[2] == p.GEOM_CYLINDER):
urdfVisual.geom_radius = v[3][1]
urdfVisual.geom_length = v[3][0]
urdfVisual.origin_xyz = v[5]
urdfVisual.origin_rpy = p.getEulerFromQuaternion(v[6])
urdfVisual.material_rgba = v[7]
name = 'mat_{}_{}'.format(linkIndex, matIndex)
urdfVisual.material_name = name
urdfLink.urdf_visual_shapes.append(urdfVisual)
matIndex = matIndex + 1
collisionShapes = p.getCollisionShapeData(bodyUid, linkIndex)
for v in collisionShapes:
print("collisionShape base:", v)
urdfCollision = UrdfCollision()
print("geom type=", v[0])
urdfCollision.geom_type = v[2]
if (v[2] == p.GEOM_BOX):
urdfCollision.geom_extents = v[3]
if (v[2] == p.GEOM_SPHERE):
urdfCollision.geom_radius = v[3][0]
if (v[2] == p.GEOM_MESH):
urdfCollision.geom_meshfile = v[4].decode("utf-8")
#localInertiaFrame*childTrans
if (v[2] == p.GEOM_CYLINDER):
urdfCollision.geom_radius = v[3][1]
urdfCollision.geom_length = v[3][0]
pos,orn = p.multiplyTransforms(dyn[3],dyn[4],\
v[5], v[6])
urdfCollision.origin_xyz = pos
urdfCollision.origin_rpy = p.getEulerFromQuaternion(orn)
urdfLink.urdf_collision_shapes.append(urdfCollision)
def print_friction_coefficient(objectId, linkIndex):
# print lateral, rolling and spinning coefficient
dynamics_info = p.getDynamicsInfo(objectId, linkIndex)
lateral_friction_coeff = dynamics_info[1]
rolling_friction_coeff = dynamics_info[6]
spinning_friction_coeff = dynamics_info[7]
print("lateral_friction_coeff:", lateral_friction_coeff)
print("rolling_friction_coeff:", rolling_friction_coeff)
print("spinning_friction_coeff:", spinning_friction_coeff)
def gravity_compensation_force(self, object_id, link_id=-1):
""" Calculates the forces required to cancel out gravity for a particular object.
:param object_id: ID number of the particular object.
:param link_id: ID number of the link to use for calculation
:return: 3D force to cancel gravity as numpy array.
"""
mass = p.getDynamicsInfo(object_id, link_id)[0]
return -mass * self.gravity_vector
def drawInertiaBox(parentUid, parentLinkIndex, color): dyn = p.getDynamicsInfo(parentUid, parentLinkIndex) mass=dyn[0] frictionCoeff=dyn[1] inertia = dyn[2] if (mass>0): Ixx = inertia[0] Iyy = inertia[1] Izz = inertia[2] boxScaleX = 0.5*math.sqrt(6*(Izz + Iyy - Ixx) / mass); boxScaleY = 0.5*math.sqrt(6*(Izz + Ixx - Iyy) / mass); boxScaleZ = 0.5*math.sqrt(6*(Ixx + Iyy - Izz) / mass); halfExtents = [boxScaleX,boxScaleY,boxScaleZ] pts = [[halfExtents[0],halfExtents[1],halfExtents[2]], [-halfExtents[0],halfExtents[1],halfExtents[2]], [halfExtents[0],-halfExtents[1],halfExtents[2]], [-halfExtents[0],-halfExtents[1],halfExtents[2]], [halfExtents[0],halfExtents[1],-halfExtents[2]], [-halfExtents[0],halfExtents[1],-halfExtents[2]], [halfExtents[0],-halfExtents[1],-halfExtents[2]], [-halfExtents[0],-halfExtents[1],-halfExtents[2]]] p.addUserDebugLine(pts[0],pts[1],color,1, parentObjectUniqueId=parentUid, parentLinkIndex = parentLinkIndex) p.addUserDebugLine(pts[1],pts[3],color,1, parentObjectUniqueId=parentUid, parentLinkIndex = parentLinkIndex) p.addUserDebugLine(pts[3],pts[2],color,1, parentObjectUniqueId=parentUid, parentLinkIndex = parentLinkIndex) p.addUserDebugLine(pts[2],pts[0],color,1, parentObjectUniqueId=parentUid, parentLinkIndex = parentLinkIndex) p.addUserDebugLine(pts[0],pts[4],color,1, parentObjectUniqueId=parentUid, parentLinkIndex = parentLinkIndex) p.addUserDebugLine(pts[1],pts[5],color,1, parentObjectUniqueId=parentUid, parentLinkIndex = parentLinkIndex) p.addUserDebugLine(pts[2],pts[6],color,1, parentObjectUniqueId=parentUid, parentLinkIndex = parentLinkIndex) p.addUserDebugLine(pts[3],pts[7],color,1, parentObjectUniqueId=parentUid, parentLinkIndex = parentLinkIndex) p.addUserDebugLine(pts[4+0],pts[4+1],color,1, parentObjectUniqueId=parentUid, parentLinkIndex = parentLinkIndex) p.addUserDebugLine(pts[4+1],pts[4+3],color,1, parentObjectUniqueId=parentUid, parentLinkIndex = parentLinkIndex) p.addUserDebugLine(pts[4+3],pts[4+2],color,1, parentObjectUniqueId=parentUid, parentLinkIndex = parentLinkIndex) p.addUserDebugLine(pts[4+2],pts[4+0],color,1, parentObjectUniqueId=parentUid, parentLinkIndex = parentLinkIndex)
knee_back_rightL_link = jointNameToId['knee_back_rightL_link']
motor_back_leftR_joint = jointNameToId['motor_back_leftR_joint']
hip_leftR_link = jointNameToId['hip_leftR_link']
knee_back_leftR_link = jointNameToId['knee_back_leftR_link']
motor_back_leftL_joint = jointNameToId['motor_back_leftL_joint']
motor_back_leftL_link = jointNameToId['motor_back_leftL_link']
knee_back_leftL_link = jointNameToId['knee_back_leftL_link']
#fixtorso = p.createConstraint(-1,-1,quadruped,-1,p.JOINT_FIXED,[0,0,0],[0,0,0],[0,0,0])
motordir = [-1, -1, -1, -1, 1, 1, 1, 1]
halfpi = 1.57079632679
twopi = 4 * halfpi
kneeangle = -2.1834
dyn = p.getDynamicsInfo(quadruped, -1)
mass = dyn[0]
friction = dyn[1]
localInertiaDiagonal = dyn[2]
print("localInertiaDiagonal", localInertiaDiagonal)
#this is a no-op, just to show the API
p.changeDynamics(quadruped, -1, localInertiaDiagonal=localInertiaDiagonal)
#for i in range (nJoints):
# p.changeDynamics(quadruped,i,localInertiaDiagonal=[0.000001,0.000001,0.000001])
drawInertiaBox(quadruped, -1, [1, 0, 0])
#drawInertiaBox(quadruped,motor_front_rightR_joint, [1,0,0])
parentObjectUniqueId=parentUid,
parentLinkIndex=parentLinkIndex)
p.addUserDebugLine(pts[4 + 3],
pts[4 + 2],
color,
1,
parentObjectUniqueId=parentUid,
parentLinkIndex=parentLinkIndex)
p.addUserDebugLine(pts[4 + 2],
pts[4 + 0],
color,
1,
parentObjectUniqueId=parentUid,
parentLinkIndex=parentLinkIndex)
drawInertiaBox(cubeId, -1)
t = 0
while 1:
t = t + 1
if t > 400:
p.changeDynamics(bodyUniqueId=0, linkIndex=-1, lateralFriction=0.01)
mass1, frictionCoeff1 = p.getDynamicsInfo(bodyUniqueId=planeId, linkIndex=-1)
mass2, frictionCoeff2 = p.getDynamicsInfo(bodyUniqueId=cubeId, linkIndex=-1)
print(mass1, frictionCoeff1)
print(mass2, frictionCoeff2)
time.sleep(1. / 240.)
p.stepSimulation()