Python pybullet.JOINT_FIXED Examples
The following are 15
code examples of pybullet.JOINT_FIXED().
You can vote up the ones you like or vote down the ones you don't like,
and go to the original project or source file by following the links above each example.
You may also want to check out all available functions/classes of the module
pybullet
, or try the search function
.
.
Example #1
| Source File: minitaur.py From midlevel-reps with MIT License | 7 votes |
|
def robot_specific_reset(self, reload_urdf=True):
"""Reset the minitaur to its initial states.
Args:
reload_urdf: Whether to reload the urdf file. If not, Reset() just place
the minitaur back to its starting position.
"""
if self.minitaur is None:
self.minitaur = self.robot_ids[0]
if self.joint_name_to_id is None:
self._BuildJointNameToIdDict()
self._BuildMotorIdList()
self._RecordMassInfoFromURDF()
self.ResetPose(add_constraint=True)
self._overheat_counter = np.zeros(self.num_motors)
self._motor_enabled_list = [True] * self.num_motors
if self.on_rack:
p.createConstraint(self.minitaur, -1, -1, -1, p.JOINT_FIXED,[0, 0, 0], [0, 0, 0], [0, 0, 1])
self.ResetPose(add_constraint=True)
Example #2
| Source File: bullet_physics_engine.py From NTP-vat-release with MIT License | 6 votes |
|
def create_cstr(descr):
if descr['joint_type'] == 'revolute':
joint_type = p.JOINT_REVOLUTE
elif descr['joint_type'] == 'prismatic':
joint_type = p.JOINT_PRISMATIC
elif descr['joint_type'] == 'fixed':
joint_type = p.JOINT_FIXED
elif descr['joint_type'] == 'point2point':
joint_type = p.JOINT_POINT2POINT
else:
raise ValueError('Unrecognized joint type {}'.format(joint_type))
if (descr['parent_frame_quat'] is None) or \
(descr['child_frame_quat'] is None):
cstr = p.createConstraint(
parentBodyUniqueId=descr['parent_body'],
parentLinkIndex=descr['parent_link'],
childBodyUniqueId=descr['child_body'],
childLinkIndex=descr['child_link'],
jointType=joint_type,
jointAxis=descr['joint_axis'],
parentFramePosition=descr['parent_frame_pos'],
childFramePosition=descr['child_frame_pos'])
else:
cstr = p.createConstraint(
parentBodyUniqueId=descr['parent_body'],
parentLinkIndex=descr['parent_link'],
childBodyUniqueId=descr['child_body'],
childLinkIndex=descr['child_link'],
jointType=joint_type,
jointAxis=descr['joint_axis'],
parentFramePosition=descr['parent_frame_pos'],
childFramePosition=descr['child_frame_pos'],
parentFrameOrientation=descr['parent_frame_quat'],
childFrameOrientation=descr['child_frame_quat'])
return cstr
Example #3
| Source File: iiwa_robotiq_3_finger.py From costar_plan with Apache License 2.0 | 6 votes |
|
def place(self, pos, rot, joints):
pb.resetBasePositionAndOrientation(self.handle, pos, rot)
pb.createConstraint(
self.handle, -1, -1, -1, pb.JOINT_FIXED, pos, [0, 0, 0], rot)
for i, q in enumerate(joints):
pb.resetJointState(self.handle, i, q)
# gripper
pb.resetJointState(self.handle, self.left_knuckle, 0)
pb.resetJointState(self.handle, self.right_knuckle, 0)
pb.resetJointState(self.handle, self.left_finger, 0)
pb.resetJointState(self.handle, self.right_finger, 0)
pb.resetJointState(self.handle, self.left_fingertip, 0)
pb.resetJointState(self.handle, self.right_fingertip, 0)
self.arm(joints,)
self.gripper(0)
Example #4
| Source File: jaco_robotiq.py From costar_plan with Apache License 2.0 | 6 votes |
|
def place(self, pos, rot, joints):
pb.resetBasePositionAndOrientation(self.handle, pos, rot)
pb.createConstraint(
self.handle, -1, -1, -1, pb.JOINT_FIXED, pos, [0, 0, 0], rot)
for i, q in enumerate(joints):
pb.resetJointState(self.handle, i, q)
# gripper
pb.resetJointState(self.handle, self.left_knuckle, 0)
pb.resetJointState(self.handle, self.right_knuckle, 0)
pb.resetJointState(self.handle, self.left_finger, 0)
pb.resetJointState(self.handle, self.right_finger, 0)
pb.resetJointState(self.handle, self.left_fingertip, 0)
pb.resetJointState(self.handle, self.right_fingertip, 0)
self.arm(joints,)
self.gripper(0)
Example #5
| Source File: urdfEditor.py From soccer-matlab with BSD 2-Clause "Simplified" License | 5 votes |
|
def initialize(self):
self.urdfLinks=[]
self.urdfJoints=[]
self.robotName = ""
self.linkNameToIndex={}
self.jointTypeToName={p.JOINT_FIXED:"JOINT_FIXED" ,\
p.JOINT_REVOLUTE:"JOINT_REVOLUTE",\
p.JOINT_PRISMATIC:"JOINT_PRISMATIC" }
Example #6
| Source File: urdfEditor.py From soccer-matlab with BSD 2-Clause "Simplified" License | 5 votes |
|
def writeJoint(self, file, urdfJoint, precision=5):
jointTypeStr = "invalid"
if urdfJoint.joint_type == p.JOINT_REVOLUTE:
if urdfJoint.joint_upper_limit < urdfJoint.joint_lower_limit:
jointTypeStr = "continuous"
else:
jointTypeStr = "revolute"
if urdfJoint.joint_type == p.JOINT_FIXED:
jointTypeStr = "fixed"
if urdfJoint.joint_type == p.JOINT_PRISMATIC:
jointTypeStr = "prismatic"
str = '\t<joint name=\"{}\" type=\"{}\">\n'.format(urdfJoint.joint_name, jointTypeStr)
file.write(str)
str = '\t\t<parent link=\"{}\"/>\n'.format(urdfJoint.parent_name)
file.write(str)
str = '\t\t<child link=\"{}\"/>\n'.format(urdfJoint.child_name)
file.write(str)
if urdfJoint.joint_type == p.JOINT_PRISMATIC:
#todo: handle limits
lowerLimit=-0.5
upperLimit=0.5
str='<limit effort="1000.0" lower="{:.{prec}f}" upper="{:.{prec}f}" velocity="0.5"/>'.format(lowerLimit,upperLimit,prec=precision)
file.write(str)
file.write("\t\t<dynamics damping=\"1.0\" friction=\"0.0001\"/>\n")
str = '\t\t<origin xyz=\"{:.{prec}f} {:.{prec}f} {:.{prec}f}\"/>\n'.format(urdfJoint.joint_origin_xyz[0],\
urdfJoint.joint_origin_xyz[1],urdfJoint.joint_origin_xyz[2], prec=precision)
str = '\t\t<origin rpy=\"{:.{prec}f} {:.{prec}f} {:.{prec}f}\" xyz=\"{:.{prec}f} {:.{prec}f} {:.{prec}f}\"/>\n'.format(urdfJoint.joint_origin_rpy[0],\
urdfJoint.joint_origin_rpy[1],urdfJoint.joint_origin_rpy[2],\
urdfJoint.joint_origin_xyz[0],\
urdfJoint.joint_origin_xyz[1],urdfJoint.joint_origin_xyz[2], prec=precision)
file.write(str)
str = '\t\t<axis xyz=\"{:.{prec}f} {:.{prec}f} {:.{prec}f}\"/>\n'.format(urdfJoint.joint_axis_xyz[0],\
urdfJoint.joint_axis_xyz[1],urdfJoint.joint_axis_xyz[2], prec=precision)
file.write(str)
file.write("\t</joint>\n")
Example #7
| Source File: util.py From assistive-gym with MIT License | 5 votes |
|
def ik(self, body, target_joint, target_pos, target_orient, ik_indices=range(29, 29+7), max_iterations=1000, half_range=False):
key = '%d_%d' % (body, target_joint)
if key not in self.ik_lower_limits:
self.ik_lower_limits[key] = []
self.ik_upper_limits[key] = []
self.ik_joint_ranges[key] = []
self.ik_rest_poses[key] = []
j_names = []
for j in range(p.getNumJoints(body, physicsClientId=self.id)):
if p.getJointInfo(body, j, physicsClientId=self.id)[2] != p.JOINT_FIXED:
joint_info = p.getJointInfo(body, j, physicsClientId=self.id)
lower_limit = joint_info[8]
upper_limit = joint_info[9]
# print(len(self.ik_lower_limits[key]), joint_info[1], lower_limit, upper_limit)
if lower_limit == 0 and upper_limit == -1:
# lower_limit = -1e10
# upper_limit = 1e10
lower_limit = -2*np.pi
upper_limit = 2*np.pi
self.ik_lower_limits[key].append(lower_limit)
self.ik_upper_limits[key].append(upper_limit)
if not half_range:
self.ik_joint_ranges[key].append(upper_limit - lower_limit)
else:
self.ik_joint_ranges[key].append((upper_limit - lower_limit)/2.0)
# self.ik_rest_poses[key].append((upper_limit + lower_limit)/2.0)
j_names.append([len(j_names)] + list(joint_info[:2]))
self.ik_rest_poses[key] = self.np_random.uniform(self.ik_lower_limits[key], self.ik_upper_limits[key]).tolist()
if target_orient is not None:
ik_joint_poses = np.array(p.calculateInverseKinematics(body, target_joint, targetPosition=target_pos, targetOrientation=target_orient, lowerLimits=self.ik_lower_limits[key], upperLimits=self.ik_upper_limits[key], jointRanges=self.ik_joint_ranges[key], restPoses=self.ik_rest_poses[key], maxNumIterations=max_iterations, physicsClientId=self.id))
else:
ik_joint_poses = np.array(p.calculateInverseKinematics(body, target_joint, targetPosition=target_pos, lowerLimits=self.ik_lower_limits[key], upperLimits=self.ik_upper_limits[key], jointRanges=self.ik_joint_ranges[key], restPoses=self.ik_rest_poses[key], maxNumIterations=max_iterations, physicsClientId=self.id))
# print(j_names)
# print(ik_joint_poses)
# exit()
target_joint_positions = ik_joint_poses[ik_indices]
return target_joint_positions
Example #8
| Source File: world_creation.py From assistive-gym with MIT License | 5 votes |
|
def init_tool(self, robot, mesh_scale=[1]*3, pos_offset=[0]*3, orient_offset=[0, 0, 0, 1], left=True, maximal=False, alpha=1.0):
if left:
gripper_pos, gripper_orient = p.getLinkState(robot, 76 if self.robot_type=='pr2' else 18 if self.robot_type=='sawyer' else 47 if self.robot_type=='baxter' else 8, computeForwardKinematics=True, physicsClientId=self.id)[:2]
else:
gripper_pos, gripper_orient = p.getLinkState(robot, 54 if self.robot_type=='pr2' else 18 if self.robot_type=='sawyer' else 25 if self.robot_type=='baxter' else 8, computeForwardKinematics=True, physicsClientId=self.id)[:2]
transform_pos, transform_orient = p.multiplyTransforms(positionA=gripper_pos, orientationA=gripper_orient, positionB=pos_offset, orientationB=orient_offset, physicsClientId=self.id)
if self.task == 'scratch_itch':
tool = p.loadURDF(os.path.join(self.directory, 'scratcher', 'tool_scratch.urdf'), basePosition=transform_pos, baseOrientation=transform_orient, physicsClientId=self.id)
elif self.task == 'bed_bathing':
tool = p.loadURDF(os.path.join(self.directory, 'bed_bathing', 'wiper.urdf'), basePosition=transform_pos, baseOrientation=transform_orient, physicsClientId=self.id)
elif self.task in ['drinking', 'scooping', 'feeding', 'arm_manipulation']:
if self.task == 'drinking':
visual_filename = os.path.join(self.directory, 'dinnerware', 'plastic_coffee_cup.obj')
collision_filename = os.path.join(self.directory, 'dinnerware', 'plastic_coffee_cup_vhacd.obj')
elif self.task in ['scooping', 'feeding']:
visual_filename = os.path.join(self.directory, 'dinnerware', 'spoon_reduced_compressed.obj')
collision_filename = os.path.join(self.directory, 'dinnerware', 'spoon_vhacd.obj')
elif self.task == 'arm_manipulation':
visual_filename = os.path.join(self.directory, 'arm_manipulation', 'arm_manipulation_scooper.obj')
collision_filename = os.path.join(self.directory, 'arm_manipulation', 'arm_manipulation_scooper_vhacd.obj')
tool_visual = p.createVisualShape(shapeType=p.GEOM_MESH, fileName=visual_filename, meshScale=mesh_scale, rgbaColor=[1, 1, 1, alpha], physicsClientId=self.id)
tool_collision = p.createCollisionShape(shapeType=p.GEOM_MESH, fileName=collision_filename, meshScale=mesh_scale, physicsClientId=self.id)
tool = p.createMultiBody(baseMass=0.01, baseCollisionShapeIndex=tool_collision, baseVisualShapeIndex=tool_visual, basePosition=transform_pos, baseOrientation=transform_orient, useMaximalCoordinates=maximal, physicsClientId=self.id)
if left:
# Disable collisions between the tool and robot
for j in (range(71, 86) if self.robot_type == 'pr2' else [18, 20, 21, 22, 23] if self.robot_type == 'sawyer' else [47, 49, 50, 51, 52] if self.robot_type == 'baxter' else [7, 8, 9, 10, 11, 12, 13, 14]):
for tj in list(range(p.getNumJoints(tool, physicsClientId=self.id))) + [-1]:
p.setCollisionFilterPair(robot, tool, j, tj, False, physicsClientId=self.id)
# Create constraint that keeps the tool in the gripper
constraint = p.createConstraint(robot, 76 if self.robot_type=='pr2' else 18 if self.robot_type=='sawyer' else 47 if self.robot_type=='baxter' else 8, tool, -1, p.JOINT_FIXED, [0, 0, 0], parentFramePosition=pos_offset, childFramePosition=[0, 0, 0], parentFrameOrientation=orient_offset, physicsClientId=self.id)
else:
# Disable collisions between the tool and robot
for j in (range(49, 64) if self.robot_type == 'pr2' else [18, 20, 21, 22, 23] if self.robot_type == 'sawyer' else [25, 27, 28, 29, 30] if self.robot_type == 'baxter' else [7, 8, 9, 10, 11, 12, 13, 14]):
for tj in list(range(p.getNumJoints(tool, physicsClientId=self.id))) + [-1]:
p.setCollisionFilterPair(robot, tool, j, tj, False, physicsClientId=self.id)
# Create constraint that keeps the tool in the gripper
constraint = p.createConstraint(robot, 54 if self.robot_type=='pr2' else 18 if self.robot_type=='sawyer' else 25 if self.robot_type=='baxter' else 8, tool, -1, p.JOINT_FIXED, [0, 0, 0], parentFramePosition=pos_offset, childFramePosition=[0, 0, 0], parentFrameOrientation=orient_offset, physicsClientId=self.id)
p.changeConstraint(constraint, maxForce=500, physicsClientId=self.id)
return tool
Example #9
| Source File: world_creation.py From assistive-gym with MIT License | 5 votes |
|
def print_joint_info(self, body, show_fixed=True):
joint_names = []
for j in range(p.getNumJoints(body, physicsClientId=self.id)):
if show_fixed or p.getJointInfo(body, j, physicsClientId=self.id)[2] != p.JOINT_FIXED:
print(p.getJointInfo(body, j, physicsClientId=self.id))
joint_names.append((j, p.getJointInfo(body, j, physicsClientId=self.id)[1]))
print(joint_names)
Example #10
| Source File: env.py From assistive-gym with MIT License | 5 votes |
|
def get_motor_joint_states(self, robot):
num_joints = p.getNumJoints(robot, physicsClientId=self.id)
joint_states = p.getJointStates(robot, range(num_joints), physicsClientId=self.id)
joint_infos = [p.getJointInfo(robot, i, physicsClientId=self.id) for i in range(num_joints)]
joint_states = [j for j, i in zip(joint_states, joint_infos) if i[2] != p.JOINT_FIXED]
joint_positions = [state[0] for state in joint_states]
joint_velocities = [state[1] for state in joint_states]
joint_torques = [state[3] for state in joint_states]
return joint_positions, joint_velocities, joint_torques
Example #11
| Source File: ur5_robotiq.py From costar_plan with Apache License 2.0 | 5 votes |
|
def place(self, pos, rot, joints):
pb.resetBasePositionAndOrientation(self.handle, pos, rot)
pb.createConstraint(
self.handle, -1, -1, -1, pb.JOINT_FIXED, pos, [0, 0, 0], rot)
for i, q in enumerate(joints):
pb.resetJointState(self.handle, i, q)
# gripper state
for joint in self.gripper_indices:
pb.resetJointState(self.handle, joint, 0.)
# send commands
self.arm(joints,)
self.gripper(self.gripperOpenCommand())
Example #12
| Source File: robot_locomotors.py From GtS with MIT License | 5 votes |
|
def robot_specific_reset(self):
WalkerBase.robot_specific_reset(self)
humanoidId = -1
numBodies = p.getNumBodies()
for i in range (numBodies):
bodyInfo = p.getBodyInfo(i)
if bodyInfo[1].decode("ascii") == 'humanoid':
humanoidId = i
## Spherical radiance/glass shield to protect the robot's camera
if self.glass_id is None:
glass_id = p.loadMJCF(os.path.join(self.physics_model_dir, "glass.xml"))[0]
#print("setting up glass", glass_id, humanoidId)
p.changeVisualShape(glass_id, -1, rgbaColor=[0, 0, 0, 0])
cid = p.createConstraint(humanoidId, -1, glass_id,-1,p.JOINT_FIXED,[0,0,0],[0,0,1.4],[0,0,1])
self.motor_names = ["abdomen_z", "abdomen_y", "abdomen_x"]
self.motor_power = [100, 100, 100]
self.motor_names += ["right_hip_x", "right_hip_z", "right_hip_y", "right_knee"]
self.motor_power += [100, 100, 300, 200]
self.motor_names += ["left_hip_x", "left_hip_z", "left_hip_y", "left_knee"]
self.motor_power += [100, 100, 300, 200]
self.motor_names += ["right_shoulder1", "right_shoulder2", "right_elbow"]
self.motor_power += [75, 75, 75]
self.motor_names += ["left_shoulder1", "left_shoulder2", "left_elbow"]
self.motor_power += [75, 75, 75]
self.motors = [self.jdict[n] for n in self.motor_names]
Example #13
| Source File: robot_locomotors.py From midlevel-reps with MIT License | 5 votes |
|
def robot_specific_reset(self):
WalkerBase.robot_specific_reset(self)
humanoidId = -1
numBodies = p.getNumBodies()
for i in range (numBodies):
bodyInfo = p.getBodyInfo(i)
if bodyInfo[1].decode("ascii") == 'humanoid':
humanoidId = i
## Spherical radiance/glass shield to protect the robot's camera
if self.glass_id is None:
glass_id = p.loadMJCF(os.path.join(self.physics_model_dir, "glass.xml"))[0]
#print("setting up glass", glass_id, humanoidId)
p.changeVisualShape(glass_id, -1, rgbaColor=[0, 0, 0, 0])
cid = p.createConstraint(humanoidId, -1, glass_id,-1,p.JOINT_FIXED,[0,0,0],[0,0,1.4],[0,0,1])
self.motor_names = ["abdomen_z", "abdomen_y", "abdomen_x"]
self.motor_power = [100, 100, 100]
self.motor_names += ["right_hip_x", "right_hip_z", "right_hip_y", "right_knee"]
self.motor_power += [100, 100, 300, 200]
self.motor_names += ["left_hip_x", "left_hip_z", "left_hip_y", "left_knee"]
self.motor_power += [100, 100, 300, 200]
self.motor_names += ["right_shoulder1", "right_shoulder2", "right_elbow"]
self.motor_power += [75, 75, 75]
self.motor_names += ["left_shoulder1", "left_shoulder2", "left_elbow"]
self.motor_power += [75, 75, 75]
self.motors = [self.jdict[n] for n in self.motor_names]
Example #14
| Source File: robot_bases.py From GtS with MIT License | 4 votes |
|
def addToScene(self, bodies):
if self.parts is not None:
parts = self.parts
else:
parts = {}
if self.jdict is not None:
joints = self.jdict
else:
joints = {}
if self.ordered_joints is not None:
ordered_joints = self.ordered_joints
else:
ordered_joints = []
dump = 0
for i in range(len(bodies)):
if p.getNumJoints(bodies[i]) == 0:
part_name, robot_name = p.getBodyInfo(bodies[i], 0)
robot_name = robot_name.decode("utf8")
part_name = part_name.decode("utf8")
parts[part_name] = BodyPart(part_name, bodies, i, -1, self.scale, model_type=self.model_type)
for j in range(p.getNumJoints(bodies[i])):
p.setJointMotorControl2(bodies[i],j,p.POSITION_CONTROL,positionGain=0.1,velocityGain=0.1,force=0)
## TODO (hzyjerry): the following is diabled due to pybullet update
#_,joint_name,joint_type, _,_,_, _,_,_,_, _,_, part_name = p.getJointInfo(bodies[i], j)
_,joint_name,joint_type, _,_,_, _,_,_,_, _,_, part_name, _,_,_,_ = p.getJointInfo(bodies[i], j)
joint_name = joint_name.decode("utf8")
part_name = part_name.decode("utf8")
if dump: print("ROBOT PART '%s'" % part_name)
if dump: print("ROBOT JOINT '%s'" % joint_name) # limits = %+0.2f..%+0.2f effort=%0.3f speed=%0.3f" % ((joint_name,) + j.limits()) )
parts[part_name] = BodyPart(part_name, bodies, i, j, self.scale, model_type=self.model_type)
if part_name == self.robot_name:
self.robot_body = parts[part_name]
if i == 0 and j == 0 and self.robot_body is None: # if nothing else works, we take this as robot_body
parts[self.robot_name] = BodyPart(self.robot_name, bodies, 0, -1, self.scale, model_type=self.model_type)
self.robot_body = parts[self.robot_name]
if joint_name[:6] == "ignore":
Joint(joint_name, bodies, i, j, self.scale).disable_motor()
continue
if joint_name[:8] != "jointfix" and joint_type != p.JOINT_FIXED:
joints[joint_name] = Joint(joint_name, bodies, i, j, self.scale, model_type=self.model_type)
ordered_joints.append(joints[joint_name])
joints[joint_name].power_coef = 100.0
debugmode = 0
if debugmode:
for j in ordered_joints:
print(j, j.power_coef)
return parts, joints, ordered_joints, self.robot_body
Example #15
| Source File: robot_bases.py From midlevel-reps with MIT License | 4 votes |
|
def addToScene(self, bodies):
if self.parts is not None:
parts = self.parts
else:
parts = {}
if self.jdict is not None:
joints = self.jdict
else:
joints = {}
if self.ordered_joints is not None:
ordered_joints = self.ordered_joints
else:
ordered_joints = []
dump = 0
for i in range(len(bodies)):
if p.getNumJoints(bodies[i]) == 0:
part_name, robot_name = p.getBodyInfo(bodies[i], 0)
robot_name = robot_name.decode("utf8")
part_name = part_name.decode("utf8")
parts[part_name] = BodyPart(part_name, bodies, i, -1, self.scale, model_type=self.model_type)
for j in range(p.getNumJoints(bodies[i])):
p.setJointMotorControl2(bodies[i],j,p.POSITION_CONTROL,positionGain=0.1,velocityGain=0.1,force=0)
## TODO (hzyjerry): the following is diabled due to pybullet update
#_,joint_name,joint_type, _,_,_, _,_,_,_, _,_, part_name = p.getJointInfo(bodies[i], j)
_,joint_name,joint_type, _,_,_, _,_,_,_, _,_, part_name, _,_,_,_ = p.getJointInfo(bodies[i], j)
joint_name = joint_name.decode("utf8")
part_name = part_name.decode("utf8")
if dump: print("ROBOT PART '%s'" % part_name)
if dump: print("ROBOT JOINT '%s'" % joint_name) # limits = %+0.2f..%+0.2f effort=%0.3f speed=%0.3f" % ((joint_name,) + j.limits()) )
parts[part_name] = BodyPart(part_name, bodies, i, j, self.scale, model_type=self.model_type)
if part_name == self.robot_name:
self.robot_body = parts[part_name]
if i == 0 and j == 0 and self.robot_body is None: # if nothing else works, we take this as robot_body
parts[self.robot_name] = BodyPart(self.robot_name, bodies, 0, -1, self.scale, model_type=self.model_type)
self.robot_body = parts[self.robot_name]
if joint_name[:6] == "ignore":
Joint(joint_name, bodies, i, j, self.scale).disable_motor()
continue
if joint_name[:8] != "jointfix" and joint_type != p.JOINT_FIXED:
joints[joint_name] = Joint(joint_name, bodies, i, j, self.scale, model_type=self.model_type)
ordered_joints.append(joints[joint_name])
joints[joint_name].power_coef = 100.0
debugmode = 0
if debugmode:
for j in ordered_joints:
print(j, j.power_coef)
return parts, joints, ordered_joints, self.robot_body
