Skip to content
GitLab
Menu
Projects
Groups
Snippets
/
Help
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Sign in
Toggle navigation
Menu
Open sidebar
Jan Koniarik
schpin-robot
Commits
11d61ecf
Commit
11d61ecf
authored
Apr 14, 2021
by
Jan Koniarik
Browse files
Refactored for changes in robot lib
parent
723c7498
Pipeline
#80121
passed with stage
in 10 minutes and 22 seconds
Changes
3
Pipelines
1
Hide whitespace changes
Inline
Side-by-side
schpin_koke/src/Main.hs
100755 → 100644
View file @
11d61ecf
import
Language.OpenSCAD
as
LS
import
Linear.Quaternion
import
Linear.V3
import
Numeric.Units.Dimensional.Prelude
...
...
@@ -13,263 +12,249 @@ import qualified Numeric.Units.Dimensional.Prelude
as
D
import
Package
import
Parser
(
extractParts
)
import
SCAD
import
Shared
import
Schpin.
SCAD
import
Schpin.
Shared
import
System.IO.Unsafe
import
Walker
import
Schpin.
Walker
data
LegPos
=
RL
|
RR
|
FR
|
FL
deriving
Eq
;
kokeTipPos
::
Pose
kokeTipPos
=
Pose
(
V3
(
110
*~
mm
)
(
0
*~
mm
)
(
0
*~
mm
))
(
toQuat
zAxis
(
0
*~
degree
))
kokeTipPos
=
Pose
(
V3
(
110
*~
mm
)
(
0
*~
mm
)
(
0
*~
mm
))
neutralQuat
femurOffset
::
ConfType
->
Pose
femurOffset
conf_type
=
Pose
(
V3
(
if
conf_type
==
Expanded
then
100
*~
mm
else
65
*~
mm
)
(
0
*~
mm
)
(
0
*~
mm
)
)
$
toQuat
yAxis
(
180
*~
degree
)
Pose
(
V3
(
if
conf_type
==
Expanded
then
100
*~
mm
else
65
*~
mm
)
(
0
*~
mm
)
(
0
*~
mm
)
)
$
toQuat
yAxis
(
180
*~
degree
)
tibiaOffset
::
Pose
tibiaOffset
=
Pose
(
V3
(
95
*~
mm
)
(
0
*~
mm
)
(
30
*~
mm
))
$
toQuat
yAxis
(
270
*~
degree
)
Pose
(
V3
(
95
*~
mm
)
(
0
*~
mm
)
(
30
*~
mm
))
$
toQuat
yAxis
(
270
*~
degree
)
legPose
::
LegPos
->
ConfType
->
Pose
legPose
pos
conf_type
=
Pose
(
V3
x
y
(
0
*~
mm
))
$
toQuat
zAxis
(
angle
*~
degree
)
where
x_val
=
if
conf_type
==
Expanded
then
85
else
60
x
=
case
pos
of
FR
->
x_val
*~
mm
FL
->
x_val
*~
mm
RR
->
(
-
x_val
)
*~
mm
RL
->
(
-
x_val
)
*~
mm
y_val
=
if
conf_type
==
Expanded
then
85
else
52
y
=
case
pos
of
FR
->
(
-
y_val
)
*~
mm
FL
->
y_val
*~
mm
RR
->
(
-
y_val
)
*~
mm
RL
->
y_val
*~
mm
$
toQuat
zAxis
(
angle
*~
degree
)
where
x_val
=
if
conf_type
==
Expanded
then
85
else
60
x
=
case
pos
of
FR
->
x_val
*~
mm
FL
->
x_val
*~
mm
RR
->
(
-
x_val
)
*~
mm
RL
->
(
-
x_val
)
*~
mm
y_val
=
if
conf_type
==
Expanded
then
85
else
52
y
=
case
pos
of
FR
->
(
-
y_val
)
*~
mm
FL
->
y_val
*~
mm
RR
->
(
-
y_val
)
*~
mm
RL
->
y_val
*~
mm
angle
=
case
pos
of
FR
->
0
FL
->
0
RR
->
180
RL
->
180
angle
=
case
pos
of
FR
->
0
FL
->
0
RR
->
180
RL
->
180
baseAxis
::
LegPos
->
Axis
baseAxis
pos
=
case
pos
of
FR
->
V3
(
0
*~
mm
)
(
0
*~
mm
)
((
-
1000
)
*~
mm
)
FL
->
zAxis
RR
->
zAxis
RL
->
V3
(
0
*~
mm
)
(
0
*~
mm
)
((
-
1000
)
*~
mm
)
FR
->
V3
(
0
*~
mm
)
(
0
*~
mm
)
((
-
1000
)
*~
mm
)
FL
->
zAxis
RR
->
zAxis
RL
->
V3
(
0
*~
mm
)
(
0
*~
mm
)
((
-
1000
)
*~
mm
)
actuatorName
::
LegPos
->
UrdfName
->
String
actuatorName
lp
jname
=
"servo/"
++
(
show
id
)
where
id
::
Int
id
=
case
lp
of
RL
->
case
jname
of
"alfa"
->
11
"beta"
->
12
"gama"
->
13
RR
->
case
jname
of
"alfa"
->
21
"beta"
->
22
"gama"
->
23
FL
->
case
jname
of
"alfa"
->
1
"beta"
->
2
"gama"
->
3
FR
->
case
jname
of
"alfa"
->
21
"beta"
->
22
"gama"
->
23
where
id
::
Int
id
=
case
lp
of
RL
->
case
jname
of
"alfa"
->
11
"beta"
->
12
"gama"
->
13
RR
->
case
jname
of
"alfa"
->
21
"beta"
->
22
"gama"
->
23
FL
->
case
jname
of
"alfa"
->
1
"beta"
->
2
"gama"
->
3
FR
->
case
jname
of
"alfa"
->
21
"beta"
->
22
"gama"
->
23
leg
::
LegPos
->
ConfType
->
SimpleLeg
leg
leg_pos
conf_type
=
Leg
{
leg_name
=
case
leg_pos
of
RL
->
"rl"
RR
->
"rr"
FL
->
"fl"
FR
->
"fr"
,
root_joint
=
coxa
{
leg_name
=
case
leg_pos
of
RL
->
"rl"
RR
->
"rr"
FL
->
"fl"
FR
->
"fr"
,
root_joint
=
coxa
}
where
coxa
::
SimpleJoint
coxa
=
SimpleJoint
$
Joint
{
joint_name
=
"alfa"
,
offset
=
legPose
leg_pos
conf_type
,
jtype
=
RevoluteJoint
{
axis
=
baseAxis
leg_pos
,
min_angle
=
0
*~
radian
,
default_angle
=
case
conf_type
of
Folded
->
0
*~
degree
_
->
45
*~
degree
,
max_angle
=
90
*~
degree
,
vel_lim
=
0.5
*~
(
radian
D
./
second
)
,
effor_lim
=
100
,
g_steps
=
16
,
actuator_name
=
actuatorName
leg_pos
"alfa"
}
,
sub_link
=
Link
{
link_name
=
"coxa"
,
weight
=
320
*~
gram
,
assembly
=
Just
coxa_assembly
}
,
sub_joint
=
Just
femur
}
where
coxa
::
SimpleJoint
coxa
=
SimpleJoint
$
Joint
{
joint_name
=
"alfa"
,
offset
=
legPose
leg_pos
conf_type
,
jtype
=
RevoluteJoint
{
axis
=
baseAxis
leg_pos
,
min_angle
=
0
*~
radian
,
default_angle
=
case
conf_type
of
Folded
->
0
*~
degree
_
->
45
*~
degree
,
max_angle
=
90
*~
degree
,
vel_lim
=
0.5
*~
(
radian
D
./
second
)
,
effor_lim
=
100
,
g_steps
=
16
,
actuator_name
=
actuatorName
leg_pos
"alfa"
}
,
sub_link
=
Link
{
link_name
=
"coxa"
,
weight
=
320
*~
gram
,
assembly
=
Just
$
Assembly
{
amodel
=
SCADModel
"coxa"
[
toArg
"femurOffset"
$
pos
$
femurOffset
Default
,
toArg
"expand"
(
if
conf_type
==
Expanded
then
10
*~
mm
else
0
*~
mm
)
]
[
"coxa.scad"
]
,
collision_models
=
[
SCADModel
"coxa_collision"
[
toArg
"femurOffset"
$
pos
$
femurOffset
Default
]
[
"coxa.scad"
]
]
}
}
,
sub_joint
=
Just
femur
}
femur
::
SimpleJoint
femur
=
SimpleJoint
$
Joint
{
joint_name
=
"beta"
,
offset
=
femurOffset
conf_type
,
jtype
=
RevoluteJoint
{
axis
=
V3
(
0
*~
meter
)
((
-
1
)
*~
meter
)
(
0
*~
meter
)
,
min_angle
=
0
*~
radian
,
default_angle
=
case
conf_type
of
Folded
->
6
*~
degree
_
->
180
*~
degree
,
max_angle
=
240
*~
degree
,
vel_lim
=
0.5
*~
(
radian
D
./
second
)
,
effor_lim
=
100
,
g_steps
=
16
,
actuator_name
=
actuatorName
leg_pos
"beta"
}
,
sub_link
=
Link
{
link_name
=
"femur"
,
weight
=
50
*~
gram
,
assembly
=
Just
$
Assembly
{
amodel
=
SCADModel
"femur"
[
toArg
"tibiaOffset"
$
pos
tibiaOffset
,
toArg
"expand"
(
if
conf_type
==
Expanded
then
10
*~
mm
else
0
*~
mm
)
]
[
"femur.scad"
]
,
collision_models
=
[
SCADModel
"femur_collision_a"
[
toArg
"tibiaOffset"
$
pos
tibiaOffset
]
[
"femur.scad"
]
,
SCADModel
"femur_collision_b"
[
toArg
"tibiaOffset"
$
pos
tibiaOffset
]
[
"femur.scad"
]
]
}
}
,
sub_joint
=
Just
tibia
}
coxa_assembly
::
SimpleAssembly
coxa_assembly
=
Assembly
{
amodel
=
SCADModel
"coxa"
[
toArg
"femurOffset"
$
pos
$
femurOffset
Default
,
toArg
"expand"
(
if
conf_type
==
Expanded
then
10
*~
mm
else
0
*~
mm
)
]
[
"coxa.scad"
]
,
collision_models
=
[
SCADModel
"coxa_collision"
[
toArg
"femurOffset"
$
pos
$
femurOffset
Default
]
[
"coxa.scad"
]
]
}
femur
::
SimpleJoint
femur
=
SimpleJoint
$
Joint
{
joint_name
=
"beta"
,
offset
=
femurOffset
conf_type
,
jtype
=
RevoluteJoint
{
axis
=
V3
(
0
*~
meter
)
((
-
1
)
*~
meter
)
(
0
*~
meter
)
,
min_angle
=
0
*~
radian
,
default_angle
=
case
conf_type
of
Folded
->
6
*~
degree
_
->
180
*~
degree
,
max_angle
=
240
*~
degree
,
vel_lim
=
0.5
*~
(
radian
D
./
second
)
,
effor_lim
=
100
,
g_steps
=
16
,
actuator_name
=
actuatorName
leg_pos
"beta"
}
,
sub_link
=
Link
{
link_name
=
"femur"
,
weight
=
50
*~
gram
,
assembly
=
Just
femur_assembly
}
,
sub_joint
=
Just
tibia
}
femur_assembly
::
SimpleAssembly
femur_assembly
=
Assembly
{
amodel
=
SCADModel
"femur"
[
toArg
"tibiaOffset"
$
pos
tibiaOffset
,
toArg
"expand"
(
if
conf_type
==
Expanded
then
10
*~
mm
else
0
*~
mm
)
]
[
"femur.scad"
]
,
collision_models
=
[
SCADModel
"femur_collision_a"
[
toArg
"tibiaOffset"
$
pos
tibiaOffset
]
[
"femur.scad"
]
,
SCADModel
"femur_collision_b"
[
toArg
"tibiaOffset"
$
pos
tibiaOffset
]
[
"femur.scad"
]
]
}
tibia
::
SimpleJoint
tibia
=
SimpleJoint
$
Joint
{
joint_name
=
"gama"
,
offset
=
tibiaOffset
,
jtype
=
RevoluteJoint
{
axis
=
yAxis
,
min_angle
=
0
*~
degree
,
default_angle
=
case
conf_type
of
Folded
->
276
*~
degree
_
->
180
*~
degree
,
max_angle
=
240
*~
degree
,
vel_lim
=
0.5
*~
(
radian
D
./
second
)
,
effor_lim
=
100
,
g_steps
=
16
,
actuator_name
=
actuatorName
leg_pos
"gama"
}
,
sub_link
=
Link
{
link_name
=
"tibia"
,
weight
=
170
*~
gram
,
assembly
=
Just
$
Assembly
{
amodel
=
SCADModel
"tibia"
[
toArg
"tipOffset"
(
pos
kokeTipPos
)
,
toArg
"expand"
(
if
conf_type
==
Expanded
then
10
*~
mm
else
0
*~
mm
)
]
[
"tibia.scad"
]
,
collision_models
=
[
SCADModel
"tibia_collision"
[
toArg
"tipOffset"
(
pos
kokeTipPos
)]
[
"tibia.scad"
]
]
}
}
,
sub_joint
=
Just
tip
}
tip
::
SimpleJoint
tip
=
SimpleJoint
$
Joint
{
joint_name
=
"tipj"
,
offset
=
kokeTipPos
,
jtype
=
FixedJoint
,
sub_link
=
Link
{
link_name
=
"tipl"
,
weight
=
0
*~
gram
,
assembly
=
Nothing
}
,
sub_joint
=
Nothing
}
tibia
::
SimpleJoint
tibia
=
SimpleJoint
$
Joint
{
joint_name
=
"gama"
,
offset
=
tibiaOffset
,
jtype
=
RevoluteJoint
{
axis
=
yAxis
,
min_angle
=
0
*~
degree
,
default_angle
=
case
conf_type
of
Folded
->
276
*~
degree
_
->
180
*~
degree
,
max_angle
=
240
*~
degree
,
vel_lim
=
0.5
*~
(
radian
D
./
second
)
,
effor_lim
=
100
,
g_steps
=
16
,
actuator_name
=
actuatorName
leg_pos
"gama"
}
,
sub_link
=
Link
{
link_name
=
"tibia"
,
weight
=
170
*~
gram
,
assembly
=
Just
tibia_assembly
}
,
sub_joint
=
Just
tip
}
tibia_assembly
::
SimpleAssembly
tibia_assembly
=
Assembly
{
amodel
=
SCADModel
"tibia"
[
toArg
"tipOffset"
(
pos
kokeTipPos
)
,
toArg
"expand"
(
if
conf_type
==
Expanded
then
10
*~
mm
else
0
*~
mm
)
]
[
"tibia.scad"
]
,
collision_models
=
[
SCADModel
"tibia_collision"
[
toArg
"tipOffset"
(
pos
kokeTipPos
)]
[
"tibia.scad"
]
]
}
tip
::
SimpleJoint
tip
=
SimpleJoint
$
Joint
{
joint_name
=
"tipj"
,
offset
=
kokeTipPos
,
jtype
=
FixedJoint
,
sub_link
=
Link
{
link_name
=
"tipl"
,
weight
=
0
*~
gram
,
assembly
=
Nothing
}
,
sub_joint
=
Nothing
}
koke
::
ConfType
->
SimpleWalker
koke
conf_type
=
Walker
{
walker_name
=
"koke"
,
legs
=
legs
,
body
=
Body
{
body_name
=
"base_link"
,
body_weight
=
1000
*~
gram
,
body_ass
=
Assembly
{
amodel
=
SCADModel
"body"
[
toArg
"positions"
leg_positions
,
toArg
"expand"
(
if
conf_type
==
Expanded
then
10
*~
mm
else
0
*~
mm
)
]
[
"body.scad"
]
,
collision_models
=
[
SCADModel
"body_collision"
[
toArg
"positions"
leg_positions
]
[
"body.scad"
]
]
}
}
{
walker_name
=
"koke"
,
legs
=
legs
,
body
=
Body
{
body_name
=
"base_link"
,
body_weight
=
1000
*~
gram
,
body_ass
=
Assembly
{
amodel
=
SCADModel
"body"
[
toArg
"positions"
leg_positions
,
toArg
"expand"
(
if
conf_type
==
Expanded
then
10
*~
mm
else
0
*~
mm
)
]
[
"body.scad"
]
,
collision_models
=
[
SCADModel
"body_collision"
[
toArg
"positions"
leg_positions
]
[
"body.scad"
]
]
}
}
where
leg_positions
=
(
\
x
->
pos
$
legPose
x
Default
)
<$>
[
RR
,
RL
,
FR
,
FL
]
legs
=
(`
leg
`
conf_type
)
<$>
[
RR
,
RL
,
FR
,
FL
]
}
where
leg_positions
=
(
\
x
->
pos
$
legPose
x
Default
)
<$>
[
RR
,
RL
,
FR
,
FL
]
legs
=
(`
leg
`
conf_type
)
<$>
[
RR
,
RL
,
FR
,
FL
]
main
::
IO
()
main
=
do
exportPackage
koke
exportExtras
koke
exportPackage
koke
exportExtras
koke
schpin_koke/src/Package.hs
View file @
11d61ecf
...
...
@@ -3,17 +3,17 @@ module Package where
import
Control.Monad.Reader
(
runReader
)
import
Parser
import
SCAD
import
Schpin.
SCAD
import
System.Directory
(
copyFile
,
createDirectory
,
createDirectoryIfMissing
,
getCurrentDirectory
)
import
Walker
import
Walker.Pkg
import
Walker.SCAD
import
Walker.SRDF
import
Walker.URDF
import
Schpin.
Walker
import
Schpin.
Walker.Pkg
import
Schpin.
Walker.SCAD
import
Schpin.
Walker.SRDF
import
Schpin.
Walker.URDF
data
ConfType
=
Default
|
Folded
|
Expanded
deriving
(
Show
,
Eq
);
...
...
schpin_koke/src/Parser.hs
View file @
11d61ecf
...
...
@@ -21,12 +21,12 @@ import Debug.Trace ( traceShow
,
traceShowM
)
import
qualified
Language.OpenSCAD
as
LS
import
qualified
SCAD
as
SC
import
Shared
import
qualified
Schpin.
SCAD
as
SC
import
Schpin.
Shared
import
System.Directory
import
System.FilePath
import
System.IO.Unsafe
import
Walker
import
Schpin.
Walker
import
Data.Hashable
(
hash
)
import
Debug.Trace
(
traceShowId
)
...
...
Write
Preview
Supports
Markdown
0%
Try again
or
attach a new file
.
Attach a file
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Cancel
Please
register
or
sign in
to comment