def __create_link_text(self, shape): name = ah.get_required_value(shape, ArmsValue.name.value) position = shape[ArmsValue.position.value] if ArmsValue.radius.value in shape: shape_type = "sphere" radius = ah.get_required_value(shape, ArmsValue.radius.value) size = f"<radius>{radius}</radius>" elif ArmsValue.sides.value in shape: shape_type = "box" sides = ah.get_required_value(shape, ArmsValue.sides.value) size = f"<size>{sides[0]} {sides[1]} {sides[2]}</size>" return f"""
def convert_shape_positions(parent_name, parent_tree): this_parent_position = ah.get_required_value( ah.get_shape_by_name(parent_name, arms), 'position') for child in parent_tree: this_child = ah.get_shape_by_name(child, arms) this_child['position'] = [0, 0, 0] this_child_relative_position = ah.get_required_value( this_child, 'relative_position') this_child['position'][ 0] = this_parent_position[0] + this_child_relative_position[0] this_child['position'][ 1] = this_parent_position[1] + this_child_relative_position[1] this_child['position'][ 2] = this_parent_position[2] + this_child_relative_position[2] convert_shape_positions(child, parent_tree[child])
def __create_models(self): for group_of_things in self.__arms: if group_of_things in shapes: for thing in self.__arms[group_of_things]: #link variables name = ah.get_required_value(thing, ArmsValue.name.value) position = thing[ArmsValue.position.value] if ah.shape_is_root(name, self.__arms): new_model = f""" <model name='model_{name}'> {self.__create_link_text(thing)} {self.__create_joints_from(name)} {self.__create_child_links(ah.get_children_of(thing[ArmsValue.name.value], self.__arms))} </model> """ self.__models = f'{self.__models}{new_model}'
def __body_variables(self): result = "" #spheres if ArmsValue.sphere.value in self.__arms: for body in self.__arms[ArmsValue.sphere.value]: new_body = f""" //Gernerated by Armature static dBodyID body_{body[ArmsValue.name.value]}; static dGeomID geom_{body[ArmsValue.name.value]}; """ result = f'{result}{new_body}' #boxes if 'box' in self.__arms: for body in self.__arms['box']: sides = ah.get_required_value(body, ArmsValue.sides.value) new_body = f""" //Gernerated by Armature static dBodyID body_{body[ArmsValue.name.value]}; static dGeomID geom_{body[ArmsValue.name.value]}; const float sides_{body[ArmsValue.name.value]}[3] = {{ {sides[0]}, {sides[1]}, {sides[2]} }}; """ result = f'{result}{new_body}' return result
def make_joint_positions_relative_to_parent_shape(): for joint_type in joints: if joint_type in arms: for joint in arms[joint_type]: parent = ah.get_required_value(joint, 'parent') parent_position = ah.get_required_value( ah.get_shape_by_name(parent, arms), 'position') joint['position'] = [0, 0, 0] joint_relative_position = ah.get_required_value( joint, 'relative_position') joint['position'][ 0] = parent_position[0] + joint_relative_position[0] joint['position'][ 1] = parent_position[1] + joint_relative_position[1] joint['position'][ 2] = parent_position[2] + joint_relative_position[2]
def make_shape_positions_relative_to_parents(): #tree of parents and children ancestor_tree = {} #Get all the roots for shape_type in shapes: if shape_type in arms: for shape in arms[shape_type]: if (ah.shape_is_root(shape[ArmsValue.name.value], arms)): ancestor_tree[shape[ArmsValue.name.value]] = {} for root in ancestor_tree: ancestor_tree[root] = ah.get_children_of(root, arms) for root in ancestor_tree: convert_shape_positions(root, ancestor_tree[root])
def __reset_bodies(self): result = "" #spheres if ArmsValue.sphere.value in self.__arms: for body in self.__arms[ArmsValue.sphere.value]: name = ah.get_required_value(body, ArmsValue.name.value) position = ah.get_required_value(body, ArmsValue.position.value) new_code = f""" //Generated by Armature dQuaternion q{name}; dQSetIdentity(q{name}); dBodySetPosition(body_{name}, {position[0]}, {position[1]}, {position[2]}); dBodySetQuaternion(body_{name}, q{name}); dBodySetLinearVel(body_{name}, 0, 0, 0); dBodySetAngularVel(body_{name}, 0, 0, 0); """ result = f'{result}{new_code}' #boxes if 'box' in self.__arms: for body in self.__arms['box']: name = ah.get_required_value(body, ArmsValue.name.value) position = ah.get_required_value(body, ArmsValue.position.value) new_code = f""" //Generated by Armature dBodySetPosition(body_{name}, {position[0]}, {position[1]}, {position[2]}); dBodySetLinearVel(body_{name}, 0, 0, 0); dBodySetAngularVel(body_{name}, 0, 0, 0); """ result = f'{result}{new_code}' return result
def __create_joints(self): result = "" for joint_type in joints: if joint_type in self.__arms: for joint in self.__arms[joint_type]: joint_name = ah.get_required_value(joint, ArmsValue.name.value) joint_position = ah.get_required_value( joint, ArmsValue.position.value) if joint_type == 'ball_and_socket': #get parent and child parent = ah.get_shape_by_name( ah.get_required_value(joint, ArmsValue.parent.value), self.__arms) child = ah.get_shape_by_name( ah.get_required_value(joint, ArmsValue.child.value), self.__arms) #get their positions and names parent_position = ah.get_required_value( parent, ArmsValue.position.value) parent_name = ah.get_required_value( parent, ArmsValue.name.value) child_position = ah.get_required_value( child, ArmsValue.position.value) child_name = ah.get_required_value( child, ArmsValue.name.value) new_joint = f""" //Generated by Armature dJointID {joint_name} = dJointCreateBall(world, 0); dJointAttach({joint_name}, body_{parent_name}, body_{child_name}); dJointSetBallAnchor({joint_name}, {joint_position[0]}, {joint_position[1]}, {joint_position[2]}); """ result = f'{result}{new_joint}' return result
def __create_shapes(self): result = "" #spheres if ArmsValue.sphere.value in self.__arms: for body in self.__arms[ArmsValue.sphere.value]: name = ah.get_required_value(body, ArmsValue.name.value) radius = ah.get_required_value(body, ArmsValue.radius.value) position = ah.get_required_value(body, ArmsValue.position.value) new_code = f""" //Generated by Armature //Create Sphere_{name} body_{name} = dBodyCreate(world); dMassSetSphere(&m, 1, {radius}); dBodySetMass(body_{name}, &m); geom_{name} = dCreateSphere(0, {radius}); dGeomSetBody(geom_{name}, body_{name}); dBodySetPosition(body_{name}, {position[0]}, {position[1]}, {position[2]}); dSpaceAdd(space, geom_{name}); """ result = f'{result}{new_code}' #boxes if 'box' in self.__arms: for body in self.__arms['box']: name = ah.get_required_value(body, ArmsValue.name.value) sides = ah.get_required_value(body, ArmsValue.sides.value) position = ah.get_required_value(body, ArmsValue.position.value) new_code = f""" //Generated by Armature //Create Box_{name} body_{name} = dBodyCreate(world); geom_{name} = dCreateBox(0, {sides[0]}, {sides[1]}, {sides[2]}); dGeomSetBody(geom_{name}, body_{name}); m.setBox(1, 1,1,1); dBodySetMass(body_{name}, &m); dBodySetPosition(body_{name}, {position[0]}, {position[1]}, {position[2]}); dSpaceAdd(space, geom_{name}); """ result = f'{result}{new_code}' return result
def __draw_bodies(self): result = "" #spheres if ArmsValue.sphere.value in self.__arms: for body in self.__arms[ArmsValue.sphere.value]: name = ah.get_required_value(body, ArmsValue.name.value) color = ah.get_optional_value(body, ArmsValue.color.value) radius = ah.get_required_value(body, ArmsValue.radius.value) new_code = f""" //Generated by Armature dsSetColor({color[0]}, {color[1]}, {color[2]}); const dReal *SPos_{name} = dBodyGetPosition(body_{name}); const dReal *SRot_{name} = dBodyGetRotation(body_{name}); float spos_{name}[3] = {{ SPos_{name}[0], SPos_{name}[1], SPos_{name}[2] }}; float srot_{name}[12] = {{ SRot_{name}[0], SRot_{name}[1], SRot_{name}[2], SRot_{name}[3], SRot_{name}[4], SRot_{name}[5], SRot_{name}[6], SRot_{name}[7], SRot_{name}[8], SRot_{name}[9], SRot_{name}[10], SRot_{name}[11] }}; dsDrawSphere ( spos_{name}, srot_{name}, {radius} ); """ result = f'{result}{new_code}' #boxes if 'box' in self.__arms: for body in self.__arms['box']: name = ah.get_required_value(body, ArmsValue.name.value) color = ah.get_optional_value(body, ArmsValue.color.value) sides = ah.get_required_value(body, ArmsValue.sides.value) new_code = f""" //Generated by Armature dsSetColor({color[0]}, {color[1]}, {color[2]}); dsDrawBox(dBodyGetPosition(body_{name}), dBodyGetRotation(body_{name}), sides_{name}); """ result = f'{result}{new_code}' return result
def __create_joints_from(self, shape_name): result = "" shape_joints = {} for group in self.__arms: if group in joints: shape_joints[group] = [] for joint in self.__arms[group]: if joint['parent'] == shape_name: shape_joints[group].append(joint) for group in shape_joints: for joint in shape_joints[group]: #TODO: change to get_required_value and pass in joint parent_position = ah.get_shape_by_name(joint['parent'], self.__arms)['position'] child_position = ah.get_shape_by_name(joint['child'], self.__arms)['position'] lower_limit = ah.get_optional_value(joint, 'lower_limit') upper_limit = ah.get_optional_value(joint, 'upper_limit') axis = ah.get_optional_value(joint, 'axis') position = joint['relative_position'] ########################################################################################### # Gazebo parses the joints pose from the child link. Hence the [Parent - Child + position]# # This puts the joint's position in the right place relative to the parent. # ########################################################################################### result = f"""{result} <joint name='{joint['name']}' type='{group}'> <parent>{joint['parent']}</parent> <child>{joint['child']}</child> <pose frame=''>{parent_position[0] - child_position[0] + position[0]} {parent_position[1] - child_position[1] + position[1]} {parent_position[2] - child_position[2] + position[2]} 0 -0 0</pose> <axis> <xyz>{axis[0]} {axis[1]} {axis[2]}</xyz> <use_parent_model_frame>0</use_parent_model_frame> <limit> <lower>{lower_limit}</lower> <upper>{upper_limit}</upper> <effort>-1</effort> <velocity>-1</velocity> </limit> <dynamics> <spring_reference>0</spring_reference> <spring_stiffness>0</spring_stiffness> <damping>0</damping> <friction>0</friction> </dynamics> </axis> <physics> <ode> <limit> <cfm>0</cfm> <erp>0.2</erp> </limit> <suspension> <cfm>0</cfm> <erp>0.2</erp> </suspension> </ode> </physics> </joint> """ return result