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