Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Small fixes and features #65

Merged
merged 10 commits into from
Nov 23, 2022
Merged

Small fixes and features #65

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
10 commits
Select commit Hold shift + click to select a range
8cd5763
wip
janbruedigam Nov 21, 2022
6064cad
Fix min_to_max_jacobian for fixed joints
janbruedigam Nov 21, 2022
562b5d9
Generalize combined shapes and fix capsule inertia
janbruedigam Nov 23, 2022
015f7d1
Fix merged bodies
janbruedigam Nov 23, 2022
948dbdd
Small fixes
janbruedigam Nov 23, 2022
b506155
Add remove-fixed-joints test
janbruedigam Nov 23, 2022
ac4507a
Fix DojoEnvironments tests
janbruedigam Nov 23, 2022
36572da
Fix type-not-used complier warnings
janbruedigam Nov 23, 2022
eed5f81
Fix DojoEnv type-not-used compiler warnings
janbruedigam Nov 23, 2022
b561429
Try panda without fixed joints
janbruedigam Nov 23, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
4 changes: 2 additions & 2 deletions DojoEnvironments/src/ant/methods/initialize.jl
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -73,7 +73,7 @@ function get_ant(;
foot_names = [:front_left_foot, :front_right_foot, :left_back_foot, :right_back_foot]
foot = [get_body(mech, name) for name in foot_names]
p = [[0.2; 0.2; 0.0], [-0.2; 0.2; 0.0], [-0.2; -0.2; 0.0], [0.2; -0.2; 0.0]]
o = [f.shape.rh[1] for f in foot]
o = [f.shape.shapes[1].rh[1] for f in foot]
contacts = [contact_constraint(foot[i], normal; friction_coefficient, contact_origin=p[i], contact_radius=o[i]) for i = 1:length(foot_names)]

if contact_body
Expand All @@ -87,7 +87,7 @@ function get_ant(;
elbow_names = [:aux_1, :aux_2, :aux_3, :aux_4]
elbow = [get_body(mech, e) for e in elbow_names]
p = [-[0.1; 0.1; 0.0], -[-0.1; 0.1; 0.0], -[-0.1;-0.1; 0.0], -[0.1; -0.1; 0.0]]
o = [e.shape.rh[1] for e in elbow]
o = [e.shape.shapes[1].rh[1] for e in elbow]
elbow_contacts = [contact_constraint(elbow[i], normal; friction_coefficient, contact_origin=p[i], contact_radius=o[i]) for i = 1:length(elbow_names)]

contacts = [contacts..., torso_contacts, elbow_contacts...]
Expand Down
4 changes: 2 additions & 2 deletions DojoEnvironments/src/atlas/methods/initialize.jl
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -190,7 +190,7 @@ function initialize_atlas!(mechanism::Mechanism;
link_linear_velocity=[zeros(3) for i=1:length(mechanism.bodies)],
link_angular_velocity=[zeros(3) for i=1:length(mechanism.bodies)],
hip_orientation=0.0,
knee_orientation=0.0) where T
knee_orientation=0.0)

body_position += (model_type == :armless) ? [0.0, 0.0, 0.9385 + 0.14853] : [0.0, 0.0, 0.9385]

Expand Down Expand Up @@ -221,7 +221,7 @@ function initialize_atlas_stance!(mech::Mechanism;
link_linear_velocity=[zeros(3) for i=1:length(mech.bodies)],
link_angular_velocity=[zeros(3) for i=1:length(mech.bodies)],
hip_orientation=0.0,
knee_orienation=0.0) where T
knee_orienation=0.0)

body_position += [0.0, 0.0, 0.9385]

Expand Down
2 changes: 1 addition & 1 deletion DojoEnvironments/src/fourbar/methods/initialize.jl
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -22,7 +22,7 @@ end

function initialize_fourbar!(mechanism::Mechanism;
angle=0.0,
angular_velocity=szeros(2)) where T
angular_velocity=szeros(2))

zero_velocity!(mechanism)
set_minimal_coordinates_velocities!(mechanism, get_joint(mechanism, :jointb1);
Expand Down
14 changes: 7 additions & 7 deletions DojoEnvironments/src/halfcheetah/methods/initialize.jl
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -67,9 +67,9 @@ function get_halfcheetah(;
body = get_body(mech, name)
if name == :torso # need special case for torso
# torso
pf = [+0.5 * body.shape.shape[1].rh[2]; 0.0; 0.0]
pb = [-0.5 * body.shape.shape[1].rh[2]; 0.0; 0.0]
o = body.shape.shape[1].rh[1]
pf = [+0.5 * body.shape.shapes[1].shapes[1].rh[2]; 0.0; 0.0]
pb = [-0.5 * body.shape.shapes[1].shapes[1].rh[2]; 0.0; 0.0]
o = body.shape.shapes[1].shapes[1].rh[1]
push!(models, contact_constraint(body, normal;
friction_coefficient,
contact_origin=pf,
Expand All @@ -80,15 +80,15 @@ function get_halfcheetah(;
contact_radius=o))

# head
pf = [+0.5 * body.shape.shape[1].rh[2] + 0.214; 0.0; 0.1935]
o = body.shape.shape[2].rh[1]
pf = [+0.5 * body.shape.shapes[1].shapes[1].rh[2] + 0.214; 0.0; 0.1935]
o = body.shape.shapes[1].shapes[1].rh[1]
push!(models, contact_constraint(body, normal;
friction_coefficient,
contact_origin=pf,
contact_radius=o))
else
p = [0;0; -0.5 * body.shape.rh[2]]
o = body.shape.rh[1]
p = [0;0; -0.5 * body.shape.shapes[1].rh[2]]
o = body.shape.shapes[1].rh[1]
push!(models, contact_constraint(body, normal;
friction_coefficient,
contact_origin=p,
Expand Down
10 changes: 5 additions & 5 deletions DojoEnvironments/src/hopper/methods/initialize.jl
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -56,9 +56,9 @@ function get_hopper(;
body = get_body(mech, name)
if name == :foot # need special case for foot
# torso
pf = [0.0, 0.0, +0.5 * body.shape.rh[2]]
pb = [0.0, 0.0, -0.5 * body.shape.rh[2]]
o = body.shape.rh[1]
pf = [0.0, 0.0, +0.5 * body.shape.shapes[1].rh[2]]
pb = [0.0, 0.0, -0.5 * body.shape.shapes[1].rh[2]]
o = body.shape.shapes[1].rh[1]
push!(models, contact_constraint(body, normal;
friction_coefficient,
contact_origin=pf,
Expand All @@ -68,8 +68,8 @@ function get_hopper(;
contact_origin=pb,
contact_radius=o))
else
p = [0.0; 0.0; 0.5 * body.shape.rh[2]]
o = body.shape.rh[1]
p = [0.0; 0.0; 0.5 * body.shape.shapes[1].rh[2]]
o = body.shape.shapes[1].rh[1]
push!(models, contact_constraint(body, normal;
friction_coefficient,
contact_origin=p,
Expand Down
18 changes: 9 additions & 9 deletions DojoEnvironments/src/humanoid/methods/initialize.jl
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -32,12 +32,12 @@ function get_humanoid(;
qll = ql * RotX(-1.57080)*RotY(1.47585)*RotZ(-1.47585) # Quaternion(RotXYZ(roll=-1.57080, pitch=1.47585, yaw=-1.47585)) # roll pitch yaw
qlr = ql * RotX(+1.57080)*RotY(1.47585)*RotZ(+1.47585) # Quaternion(RotXYZ(roll=+1.57080, pitch=1.47585, yaw=+1.47585)) # roll pitch yaw

pfll = vector_rotate([ 0.5 * left_foot.shape.shape[1].rh[2] + 0.03500; -0.03; 0.0], qll)
pbll = vector_rotate([-0.5 * left_foot.shape.shape[1].rh[2] + 0.03500; -0.03; 0.0], qll)
pflr = vector_rotate([ 0.5 * left_foot.shape.shape[1].rh[2] + 0.03500; +0.01; 0.0], qlr)
pblr = vector_rotate([-0.5 * left_foot.shape.shape[1].rh[2] + 0.03500; +0.01; 0.0], qlr)
pfll = vector_rotate([ 0.5 * left_foot.shape.shapes[1].shapes[1].rh[2] + 0.03500; -0.03; 0.0], qll)
pbll = vector_rotate([-0.5 * left_foot.shape.shapes[1].shapes[1].rh[2] + 0.03500; -0.03; 0.0], qll)
pflr = vector_rotate([ 0.5 * left_foot.shape.shapes[1].shapes[1].rh[2] + 0.03500; +0.01; 0.0], qlr)
pblr = vector_rotate([-0.5 * left_foot.shape.shapes[1].shapes[1].rh[2] + 0.03500; +0.01; 0.0], qlr)
p = [0.0,0.054,0.]
o = left_foot.shape.shape[1].rh[1]
o = left_foot.shape.shapes[1].shapes[1].rh[1]
contacts = [
p,
# pfll,
Expand All @@ -62,10 +62,10 @@ function get_humanoid(;

right_foot = get_body(mech, :right_foot)

pfr = [0.5 * right_foot.shape.shape[1].rh[2]; 0.0; 0.0]
ofr = right_foot.shape.shape[1].rh[1]
pbr = [-0.5 * right_foot.shape.shape[1].rh[2]; 0.0; 0.0]
obr = right_foot.shape.shape[1].rh[1]
pfr = [0.5 * right_foot.shape.shapes[1].shapes[1].rh[2]; 0.0; 0.0]
ofr = right_foot.shape.shapes[1].shapes[1].rh[1]
pbr = [-0.5 * right_foot.shape.shapes[1].shapes[1].rh[2]; 0.0; 0.0]
obr = right_foot.shape.shapes[1].shapes[1].rh[1]

contacts = [
pfr,
Expand Down
4 changes: 2 additions & 2 deletions DojoEnvironments/src/panda/deps/panda_end_effector.urdf
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -9,12 +9,12 @@
<!-- || by: Claudio Gaz, Marco Cognetti, Alexander Oliva, || -->
<!-- || Paolo Robuffo Giordano, Alessandro de Luca third-party || -->
<!-- =============================================================== -->
<link name="world"/>
<!-- <link name="world"/>
<joint name="joint0" type="fixed">
<parent link="world"/>
<child link="link0"/>
<origin rpy="0 0 0" xyz="0 0 0"/>
</joint>
</joint> -->
<link name="link0">
<inertial>
<origin rpy="0 0 0" xyz="-0.0410181918537986 -0.000143266349590146 0.0499742749991159"/>
Expand Down
4 changes: 2 additions & 2 deletions DojoEnvironments/src/panda/deps/panda_no_end_effector.urdf
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -9,12 +9,12 @@
<!-- || by: Claudio Gaz, Marco Cognetti, Alexander Oliva, || -->
<!-- || Paolo Robuffo Giordano, Alessandro de Luca third-party || -->
<!-- =============================================================== -->
<link name="world"/>
<!-- <link name="world"/>
<joint name="joint0" type="fixed">
<parent link="world"/>
<child link="link0"/>
<origin rpy="0 0 0" xyz="0 0 0"/>
</joint>
</joint> -->
<link name="link0">
<inertial>
<origin rpy="0 0 0" xyz="-0.0410181918537986 -0.000143266349590146 0.0499742749991159"/>
Expand Down
4 changes: 2 additions & 2 deletions DojoEnvironments/src/panda/deps/panda_original.urdf
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -9,12 +9,12 @@
<!-- || by: Claudio Gaz, Marco Cognetti, Alexander Oliva, || -->
<!-- || Paolo Robuffo Giordano, Alessandro de Luca third-party || -->
<!-- =============================================================== -->
<link name="world"/>
<!-- <link name="world"/>
<joint name="panda_joint_panda_mount" type="fixed">
<parent link="panda_mount"/>
<child link="panda_link0"/>
<origin rpy="0 0 0" xyz="0 0 0"/>
</joint>
</joint> -->
<link name="panda_link0">
<inertial>
<origin rpy="0 0 0" xyz="-0.0410181918537986 -0.000143266349590146 0.0499742749991159"/>
Expand Down
Loading