Commit d3f5c302 authored by Jan Koniarik's avatar Jan Koniarik
Browse files

again lost track of what changed, anyway launch files generation complete for...

again lost track of what changed, anyway launch files generation complete for devices, but will need review
parent 9dae8052
<launch>
<arg name="gui" default="False" />
<param name="robot_description" textfile="$(find schpin_tote)/src/tote.urdf" />
<param name="use_gui" value="$(arg gui)"/>
<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" />
<node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" />
<node name="rviz" pkg="rviz" type="rviz" args="-d $(find schpin_tote)/urdf.rviz" required="true" />
</launch>
......@@ -40,7 +40,13 @@
<!-- Use test_depend for packages you need only for testing: -->
<!-- <test_depend>gtest</test_depend> -->
<buildtool_depend>catkin</buildtool_depend>
<run_depend>python</run_depend>
<run_depend>python-lxm</run_depend>
<run_depend>rviz</run_depend>
<run_depend>iconv</run_depend>
<!-- The export tag contains other, unspecified, tags -->
<export>
......
......@@ -3,8 +3,17 @@ clearance = 10.0;
camera_holder_t = 3.0;
yaw_rot = [0,0,180];
yaw_pos = [9.0,0,39.1];
segments_yaw_scad_args = [];
segments_yaw_scad_module = "camera_yaw";
segments_yaw_joint_axis = [0,0,1];
segments_yaw_rot = [0,0,180];
segments_yaw_next_segment = "pitch";
segments_yaw_pos = [9.0,0,39.1];
segments_pitch_joint_axis = [0,1,0];
segments_pitch_rot = [0,0,0];
segments_pitch_pos = [-12.5,-5.85,17.05];
segments_pitch_scad_args = [];
segments_pitch_scad_module = "camera_pitch";
horn_angle = -10.0;
horn_holes_step = 2.0;
......@@ -13,9 +22,9 @@ horn_holes_n = 6;
horn_d = 7.1;
horn_d2 = 1.0;
camera_offset = [25.0,5.5,-21.5];
chain_root = "yaw";
pitch_offset = [-12.5,-5.85,17.05];
camera_offset = [25.0,5.5,-21.5];
t = 3.0;
......
......@@ -90,7 +90,7 @@ module camera_yaw(){
rotate([180, 0, 0])
mg90s();
translate(pitch_offset)
translate(segments_pitch_pos)
rotate([90, 0 ,0])
mg90s();
}
......
include <gimbal/gimbal.scad>module gimbal_pos(){
translate([9.0, 0, 39.1])rotate([0, 0, 180]){
camera_yaw();
translate([-12.5, -5.8500000000000005, 17.05])rotate([0, 0, 0]){
camera_pitch();
}
}
}
......@@ -20,7 +20,9 @@ module tibia() {
rotate([90, 0, -90])
rotate([0, 0, 90]){
mg90s();
translate([0, 11, -4.5])
rotate([0, 0, 90])
import("leg.stl");
}
}
......
include <leg_modules.scad>;
module fr_beta_joint_pos() translate([0, 12.5, -22.375]) rotate([0.0, 0, 0]){
module rl_leg()translate([-55.0, 25.0, 0])rotate([0, 0, 29.999999999999996])mirror([0, 0, 0]){
translate([0, 0, 0])rotate([0, 0, 0]){
coxa();
translate([0, 12.5, -22.375])rotate([0.0, 0, 0]){
femur(45.0);
children();
translate([0, 45.0, 0])rotate([-90.0, 0, 0]){
tibia();
translate([-5.0, 50.0, 0])rotate([0, 0, 0]){
}
module fr_alfa_joint_pos() translate([0, 0, 0]) rotate([0, 0, 0]){
coxa();
children();
}
module fr_gama_joint_pos() translate([0, 45.0, 0]) rotate([-90.0, 0, 0]){
tibia();
children();
}
module fr_leg()translate([55.0, -25.0, 0])rotate([0, 0, -150.0])mirror([0, 0, 0]){
fr_alfa_joint_pos() fr_beta_joint_pos() fr_gama_joint_pos();
}
module rr_beta_joint_pos() translate([0, 12.5, -22.375]) rotate([0.0, 0, 0]){
femur(45.0);
children();
}
module rr_alfa_joint_pos() translate([0, 0, 0]) rotate([0, 0, 0]){
coxa();
children();
;
}
module rr_gama_joint_pos() translate([0, 45.0, 0]) rotate([-90.0, 0, 0]){
module fr_leg()translate([55.0, -25.0, 0])rotate([0, 0, -150.0])mirror([0, 0, 0]){
translate([0, 0, 0])rotate([0, 0, 0]){
coxa();
translate([0, 12.5, -22.375])rotate([0.0, 0, 0]){
femur(45.0);
translate([0, 45.0, 0])rotate([-90.0, 0, 0]){
tibia();
children();
translate([-5.0, 50.0, 0])rotate([0, 0, 0]){
}
module rr_leg()translate([-55.0, -25.0, 0])rotate([0, 0, 150.0])mirror([1, 0, 0]){
rr_alfa_joint_pos() rr_beta_joint_pos() rr_gama_joint_pos();
}
module mr_beta_joint_pos() translate([0, 12.5, -22.375]) rotate([0.0, 0, 0]){
femur(45.0);
children();
}
module mr_alfa_joint_pos() translate([0, 0, 0]) rotate([0, 0, 0]){
coxa();
children();
}
module mr_gama_joint_pos() translate([0, 45.0, 0]) rotate([-90.0, 0, 0]){
tibia();
children();
}
module mr_leg()translate([0, -30.0, 0])rotate([0, 0, 180.0])mirror([1, 0, 0]){
mr_alfa_joint_pos() mr_beta_joint_pos() mr_gama_joint_pos();
;
}
module rl_beta_joint_pos() translate([0, 12.5, -22.375]) rotate([0.0, 0, 0]){
module fl_leg()translate([55.0, 25.0, 0])rotate([0, 0, -29.999999999999996])mirror([1, 0, 0]){
translate([0, 0, 0])rotate([0, 0, 0]){
coxa();
translate([0, 12.5, -22.375])rotate([0.0, 0, 0]){
femur(45.0);
children();
translate([0, 45.0, 0])rotate([-90.0, 0, 0]){
tibia();
translate([-5.0, 50.0, 0])rotate([0, 0, 0]){
}
module rl_alfa_joint_pos() translate([0, 0, 0]) rotate([0, 0, 0]){
coxa();
children();
}
module rl_gama_joint_pos() translate([0, 45.0, 0]) rotate([-90.0, 0, 0]){
tibia();
children();
}
module rl_leg()translate([-55.0, 25.0, 0])rotate([0, 0, 29.999999999999996])mirror([0, 0, 0]){
rl_alfa_joint_pos() rl_beta_joint_pos() rl_gama_joint_pos();
}
module ml_beta_joint_pos() translate([0, 12.5, -22.375]) rotate([0.0, 0, 0]){
femur(45.0);
children();
}
module ml_alfa_joint_pos() translate([0, 0, 0]) rotate([0, 0, 0]){
coxa();
children();
;
}
module ml_gama_joint_pos() translate([0, 45.0, 0]) rotate([-90.0, 0, 0]){
module rr_leg()translate([-55.0, -25.0, 0])rotate([0, 0, 150.0])mirror([1, 0, 0]){
translate([0, 0, 0])rotate([0, 0, 0]){
coxa();
translate([0, 12.5, -22.375])rotate([0.0, 0, 0]){
femur(45.0);
translate([0, 45.0, 0])rotate([-90.0, 0, 0]){
tibia();
children();
translate([-5.0, 50.0, 0])rotate([0, 0, 0]){
}
module ml_leg()translate([0, 30.0, 0])rotate([0, 0, 0])mirror([0, 0, 0]){
ml_alfa_joint_pos() ml_beta_joint_pos() ml_gama_joint_pos();
}
module fl_beta_joint_pos() translate([0, 12.5, -22.375]) rotate([0.0, 0, 0]){
femur(45.0);
children();
}
module fl_alfa_joint_pos() translate([0, 0, 0]) rotate([0, 0, 0]){
coxa();
children();
}
module fl_gama_joint_pos() translate([0, 45.0, 0]) rotate([-90.0, 0, 0]){
tibia();
children();
}
module fl_leg()translate([55.0, 25.0, 0])rotate([0, 0, -29.999999999999996])mirror([1, 0, 0]){
fl_alfa_joint_pos() fl_beta_joint_pos() fl_gama_joint_pos();
;
}
module legs(){
fr_leg();
rr_leg();
mr_leg();
rl_leg();
ml_leg();
fr_leg();
fl_leg();
rr_leg();
}
module leg_pos(){
translate([55.0, -25.0, 0])rotate([0, 0, -150.0])mirror([0, 0, 0])children();
translate([-55.0, -25.0, 0])rotate([0, 0, 150.0])mirror([1, 0, 0])children();
translate([0, -30.0, 0])rotate([0, 0, 180.0])mirror([1, 0, 0])children();
translate([-55.0, 25.0, 0])rotate([0, 0, 29.999999999999996])mirror([0, 0, 0])children();
translate([0, 30.0, 0])rotate([0, 0, 0])mirror([0, 0, 0])children();
translate([55.0, -25.0, 0])rotate([0, 0, -150.0])mirror([0, 0, 0])children();
translate([55.0, 25.0, 0])rotate([0, 0, -29.999999999999996])mirror([1, 0, 0])children();
translate([-55.0, -25.0, 0])rotate([0, 0, 150.0])mirror([1, 0, 0])children();
}
......@@ -7,6 +7,7 @@ use <servo/sg9.scad>;
use <strengthen/servo.scad>;
use <base_plate/plate.scad>;
use <leg_pos.scad>;
use <gimbal_pos.scad>;
......@@ -39,10 +40,10 @@ module body(){
translate([0, 0, 25]){
rotate([180, 0, 0])
raspbpi();
raspbpi();
}
translate(cam_base_pos) rotate(yaw_rot)
translate(cam_base_pos)
camera_base();
}
......@@ -55,14 +56,11 @@ module model() {
legs();
translate(yaw_pos) rotate(yaw_rot) {
camera_yaw();
translate(pitch_offset)
camera_pitch();
}
gimbal_pos();
}
}
model();
//model();
......@@ -7,27 +7,27 @@ servo_horn_holes_step = 2.0;
servo_horn_holes_first = 4.5;
servo_horn_holes_n = 6;
mg90s_angle_max = 75.0;
mg90s_angle_min = -75.0;
mg90s_stall_current = 0.65;
mg90s_servo_width = 12.5;
mg90s_servo_length = 22.9;
mg90s_servo_height = 22.9;
mg90s_min = -75.0;
mg90s_turret_diameter = 11.7;
mg90s_turret_margin_y = 11.2;
mg90s_turret_margin_x = 0.4;
mg90s_turret_height = 5.2;
mg90s_max = 75.0;
mg90s_frame_length = 32.2;
mg90s_frame_v_offset = 16.0;
mg90s_frame_thickness = 2.45;
mg90s_torque = 0.1568;
mg90s_max_velocity = 10.471975512;
mg90s_max_torque = 0.1568;
mg90s_horn_offset = 11.1;
mg90s_gimbal_vector = [-5.85,12.5,-17.05];
mg90s_axle_diameter = 4.82;
mg90s_axle_screw_diameter = 2.0;
mg90s_axle_height = 3.9;
mg90s_running_current = 0.22;
mg90s_velocity = 10.471975512;
mg90s_mounting_hole_slot = 1;
mg90s_mounting_hole_diameter = 2.0;
mg90s_mounting_hole_center_offset_y = 2.325;
......
include <model.scad>;
$fn=128;body();
\ No newline at end of file
......@@ -29,12 +29,16 @@ mg90s: &MG90S
- "% -mg90s.turret.diameter / 2 %"
- "% mg90s.servo.width %"
- "% -(mg90s.servo.length - mg90s.turret.diameter /2 ) %"
velocity: 0.1sec/60°
torque: 1.6kg/cm
max_velocity: 0.1sec/60°
max_torque: 1.6kg/cm
stall_current: 650mA
running_current: 220mA
min: -75°
max: 75°
angle:
min: -75°
max: 75°
pulse:
min: 900
max: 2100
servo_horn:
t: 2mm # thickness
......@@ -75,66 +79,98 @@ gimbal:
screw_d: "% rpi.screw_d %"
y_offset: -2mm
yaw:
pos:
- "% gimbal.cam.base_pos.0 %"
- "% gimbal.cam.base_pos.1 %"
- "% gimbal.cam.base_pos.2 + mg90s.horn_offset + gimbal.t %"
rot: [0, 0, 180]
pitch_offset:
- "% -mg90s.gimbal_vector.1 %"
- "% mg90s.gimbal_vector.0 %"
- "% -mg90s.gimbal_vector.2 %"
chain_root: 'yaw'
segments:
yaw:
pos:
- "% gimbal.cam.base_pos.0 %"
- "% gimbal.cam.base_pos.1 %"
- "% gimbal.cam.base_pos.2 + mg90s.horn_offset + gimbal.t %"
rot: [0,0,180°]
scad:
module: 'camera_yaw'
args: []
joint:
<<: *MG90S
axis: [0,0,1]
reverse: False
next_segment: 'pitch'
pitch:
pos:
- "% -mg90s.gimbal_vector.1 %"
- "% mg90s.gimbal_vector.0 %"
- "% -mg90s.gimbal_vector.2 %"
rot: [0,0,0]
joint:
<<: *MG90S
axis: [0, 1, 0]
reverse: False
scad:
module: 'camera_pitch'
args: []
battery:
offset: [-5mm,0,0]
dim: [80mm,12mm,35mm]
default_leg: &DEFAULT_LEG
mirror: [0,0,0]
joint_chain: ["alfa", "beta", "gama"]
joints: # min( torque_needed / max_torque , 1 ) * ( stall_current - running_current ) + stall_current
alfa:
<<: *MG90S
chain_root: coxa
segments: # min( torque_needed / max_torque , 1 ) * ( stall_current - running_current ) + stall_current
coxa:
pos: [0,0,0]
rot: [0,0,0]
axis: [0, 0, 1]
graph:
steps: 16
weight: 30g
mass_center: [0,0,0]
joint:
<<: *MG90S
axis: [0, 0, 1]
graph:
steps: 16
reverse: False
mass: 30g
mass_center: [0,10mm,-5mm]
scad:
module: "coxa"
args: []
beta:
<<: *MG90S
next_segment: femur
femur:
pos:
- 0
- "% mg90s.servo.width %"
- "% -( mg90s.servo.length - mg90s.turret.diameter / 2 + strengthen.mount_h ) %"
rot: [, 0, 0]
axis: [1, 0, 0]
graph:
steps: 16
weight: 10g
mass_center: [0,0,0]
joint:
<<: *MG90S
axis: [1, 0, 0]
graph:
steps: 16
reverse: False
mass: 10g
mass_center: [0, 25mm ,0]
scad:
module: "femur"
args: ["% default_leg.joints.gama.pos.1 %"]
gama:
<<: *MG90S
args: ["% default_leg.segments.tibia.pos.1 %"]
next_segment: tibia
tibia:
pos: [0,45mm,0]
rot: [-90°, 0, 0]
axis: [1, 0, 0]
graph:
steps: 16
weight: 15g
mass_center: [0,0,0]
joint:
<<: *MG90S
axis: [1, 0, 0]
graph:
steps: 16
reverse: False
mass: 15g
mass_center: [0,25mm,0]
scad:
module: "tibia"
args: []
tip: [-5mm, 50mm, 0]
next_segment: tip
tip:
pos: [-5mm, 50mm, 0]
rot: [0,0,0]
mass_center: [0,0,0]
mass: 0
legs:
fl:
<<: *DEFAULT_LEG
......@@ -146,15 +182,6 @@ legs:
<<: *DEFAULT_LEG
pos: [55mm, -25mm, 0]
rot: [0,0,-150°]
ml:
<<: *DEFAULT_LEG
pos: [0, 30mm, 0]
rot: [0,0,0]
mr:
<<: *DEFAULT_LEG
mirror: [1,0,0]
pos: [0, -30mm, 0]
rot: [0,0,180°]
rl:
<<: *DEFAULT_LEG
pos: [-55mm, 25mm, 0]
......
import os
import math
from lib.config import *
from lxml import etree
def check_stl(filename, module):
assert filename[-1] == 'b'
scad = "../scad/tmp.scad"
with open(scad, "w") as fd:
fd.write("include <model.scad>;\n")
fd.write(module + "();")
tmp_stl = "../stl/tmp.stl"
os.system(" ".join(['openscad', '-o', tmp_stl, scad]))
os.system("ivcon {} {}".format(tmp_stl, filename))
os.remove(tmp_stl)
os.remove(scad)
def generate_link(robot, name, scad_module):
link = etree.SubElement(robot, 'link', name=name)
visual = etree.SubElement(link, 'visual')
geometry = etree.SubElement(visual, 'geometry')
if scad_module is not None:
stl_filename = 'stl/' + name + ".stlb"
if not os.path.exists("../" + stl_filename):
check_stl("../" + stl_filename, scad_module)
etree.SubElement(
geometry, 'mesh', filename="package://schpin_tote/" + stl_filename)
material = etree.SubElement(
visual, 'material', name=name + "_material")
etree.SubElement(
material, 'color', rgba="{} {} {} 1".format(*get_color(name)))
return link
def get_color(name):
m = hashlib.md5()
m.update(name.encode('utf8'))
h = m.digest()
return [x / 256 for x in h[:3]]
def generate_urdf_joint(robot,
name,
parent,
child,
pos,
rot,
axis,
static=False):
joint = etree.SubElement(robot, 'joint', name=name, type='revolute')
etree.SubElement(joint, 'parent', link=parent.name)
etree.SubElement(joint, 'child', link=child.name)
if not static:
etree.SubElement(
joint,
'limit',
effort="1000",
velocity="50",
lower="-2",
upper="2")
etree.SubElement(
joint,
'origin',
xyz="{} {} {}".format(* [x for x in pos]),
rpy="{} {} {}".format(* [math.radians(x) for x in rot]))
if axis is not None:
etree.SubElement(joint, 'axis', xyz="{} {} {}".format(*axis))
" return number of replaced items, unreplaced items, new value "
"""
Library module to handle configuratio yaml file.
Units:
- units are implemented as object, where each unit inherits Unit() object
- in cases yaml library matches 'config_re' from this module, it's than
passed to module mechanism.
- regexp extracts value and unit, based on 'unit_config', method is
executed and unit object is expected
- resulting object is stored into final dictionary
"""
import yaml
import re
import copy
import math
import collections
import parser
class Unit(float):
"""Base class for units, makes sure that mathematical operations won't
change the unit type. Forbids math operations with different unit types
"""
def __add__(self, other):
assert isinstance(other, (float, int, self.__class__))
return self.__class__(float.__add__(self, other))
def __radd__(self, other):
assert isinstance(other, (float, int, self.__class__))