Skip to content

Commit

Permalink
Changed Parameters in Wlfgang
Browse files Browse the repository at this point in the history
  • Loading branch information
lvaddi committed Nov 6, 2024
1 parent 034b187 commit bd83c2d
Show file tree
Hide file tree
Showing 6 changed files with 53 additions and 84 deletions.
61 changes: 0 additions & 61 deletions animations/.json

This file was deleted.

2 changes: 1 addition & 1 deletion bitbots_motion/bitbots_dynup/config/dynup_sim.yaml
Original file line number Diff line number Diff line change
Expand Up @@ -29,7 +29,7 @@ dynup:
wait_in_squat_front: 0.032

rise:
rise_time: 0.4
rise_time: 2.0

stabilizer:
end_pause:
Expand Down
24 changes: 12 additions & 12 deletions bitbots_motion/bitbots_dynup/src/dynup_engine.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -595,18 +595,18 @@ double DynupEngine::calcWalkreadySplines(double time, double travel_time) {
r_foot_spline_.pitch()->addPoint(time, -params_.end_pose.trunk_pitch * M_PI / 180);
r_foot_spline_.yaw()->addPoint(time, 0);

// l_hand_spline_.x()->addPoint(time, 0);
// l_hand_spline_.y()->addPoint(time, 0);
// l_hand_spline_.z()->addPoint(time, params_.end_pose.hand_walkready_height);
// l_hand_spline_.roll()->addPoint(time, 0);
// l_hand_spline_.pitch()->addPoint(time, params_.end_pose.hand_walkready_pitch * M_PI / 180);
// l_hand_spline_.yaw()->addPoint(time, 0);
// r_hand_spline_.x()->addPoint(time, 0);
// r_hand_spline_.y()->addPoint(time, 0);
// r_hand_spline_.z()->addPoint(time, params_.end_pose.hand_walkready_height);
// r_hand_spline_.roll()->addPoint(time, 0);
// r_hand_spline_.pitch()->addPoint(time, params_.end_pose.hand_walkready_pitch * M_PI / 180);
// r_hand_spline_.yaw()->addPoint(time, 0);
l_hand_spline_.x()->addPoint(time, 0);
l_hand_spline_.y()->addPoint(time, 0);
l_hand_spline_.z()->addPoint(time, params_.end_pose.hand_walkready_height);
l_hand_spline_.roll()->addPoint(time, 0);
l_hand_spline_.pitch()->addPoint(time, params_.end_pose.hand_walkready_pitch * M_PI / 180);
l_hand_spline_.yaw()->addPoint(time, 0);
r_hand_spline_.x()->addPoint(time, 0);
r_hand_spline_.y()->addPoint(time, 0);
r_hand_spline_.z()->addPoint(time, params_.end_pose.hand_walkready_height);
r_hand_spline_.roll()->addPoint(time, 0);
r_hand_spline_.pitch()->addPoint(time, params_.end_pose.hand_walkready_pitch * M_PI / 180);
r_hand_spline_.yaw()->addPoint(time, 0);

return time;
}
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -39,7 +39,7 @@
"LHipYaw": -3.08,
"LKnee": 160.21,
"LShoulderPitch": -58.89,
"LShoulderRoll": 19.35,
"LShoulderRoll": 5.0,
"RAnklePitch": 58.480000000000004,
"RAnkleRoll": -20.39,
"RElbow": 82.66,
Expand All @@ -48,7 +48,33 @@
"RHipYaw": 17.5,
"RKnee": -157.68,
"RShoulderPitch": 56.54,
"RShoulderRoll": -12.66
"RShoulderRoll": -5.0
},
"name": "start frame_copy_1",
"pause": 0.0,
"torque": {}
},
{
"duration": 1.2,
"goals": {
"LAnklePitch": -57.48,
"LAnkleRoll": 11.96,
"LElbow": -86.22,
"LHipPitch": 121.61,
"LHipRoll": 14.250000000000002,
"LHipYaw": -3.08,
"LKnee": 160.21,
"LShoulderPitch": -58.89,
"LShoulderRoll": 15.0,
"RAnklePitch": 58.480000000000004,
"RAnkleRoll": -20.39,
"RElbow": 82.66,
"RHipPitch": -118.74,
"RHipRoll": -18.19,
"RHipYaw": 17.5,
"RKnee": -157.68,
"RShoulderPitch": 56.54,
"RShoulderRoll": -15.0
},
"name": "start frame_copy_1",
"pause": 0.0,
Expand All @@ -64,8 +90,8 @@
"LHipRoll": 13.37,
"LHipYaw": 1.23,
"LKnee": 142.19,
"LShoulderPitch": -164.79,
"LShoulderRoll": 19.35,
"LShoulderPitch": -164.0,
"LShoulderRoll": 12.66,
"RAnklePitch": 59.71,
"RAnkleRoll": -15.560000000000002,
"RElbow": 82.66,
Expand All @@ -84,4 +110,4 @@
"last_edited": "2024-10-30 19:16:16.384874",
"name": "grab_ball",
"version": ""
}
}
Original file line number Diff line number Diff line change
Expand Up @@ -3,7 +3,7 @@
"description": "",
"keyframes": [
{
"duration": 1.0,
"duration": 0.0,
"goals": {
"HeadPan": 0.0,
"HeadTilt": 0.0,
Expand Down Expand Up @@ -70,4 +70,4 @@
"last_edited": "2024-10-30 19:13:18.833923",
"name": "throw",
"version": ""
}
}
Original file line number Diff line number Diff line change
Expand Up @@ -483,6 +483,7 @@ PROTO Wolfgang [
RotationalMotor {
name "RShoulderRoll"
maxVelocity IS MX64-vel
minPosition -0.4
maxPosition 3.14159
maxTorque IS MX64-torque
controlPID IS pid
Expand Down Expand Up @@ -1041,6 +1042,7 @@ PROTO Wolfgang [
name "LShoulderRoll"
maxVelocity IS MX64-vel
minPosition -3.14159
maxPosition 0.4
maxTorque IS MX64-torque
controlPID IS pid
}
Expand Down Expand Up @@ -1135,7 +1137,7 @@ PROTO Wolfgang [
RotationalMotor {
name "LElbow"
maxVelocity IS MX64-vel
minPosition -1.5708
minPosition -1.7
maxPosition 1.0472
maxTorque IS MX64-torque
controlPID IS pid
Expand Down Expand Up @@ -1735,6 +1737,7 @@ PROTO Wolfgang [
name "RKnee"
maxVelocity IS XH540W270-vel
minPosition -2.96706
maxPosition 0.2
maxTorque IS XH540W270-torque
controlPID IS pid
}
Expand Down Expand Up @@ -1826,7 +1829,7 @@ PROTO Wolfgang [
name "RAnklePitch"
maxVelocity IS MX106-vel
minPosition -1.74533
maxPosition 1.22173
maxPosition 1.45
maxTorque IS MX106-torque
controlPID IS pid
}
Expand Down Expand Up @@ -3154,6 +3157,7 @@ PROTO Wolfgang [
RotationalMotor {
name "LKnee"
maxVelocity IS XH540W270-vel
minPosition -0.2
maxPosition 2.96706
maxTorque IS XH540W270-torque
controlPID IS pid
Expand Down Expand Up @@ -3245,7 +3249,7 @@ PROTO Wolfgang [
RotationalMotor {
name "LAnklePitch"
maxVelocity IS MX106-vel
minPosition -1.22173
minPosition -1.45
maxPosition 1.74533
maxTorque IS MX106-torque
controlPID IS pid
Expand Down

0 comments on commit bd83c2d

Please sign in to comment.