diff --git a/examples/run_jump_ocp.py b/examples/run_jump_ocp.py index af9135d..5dc185a 100644 --- a/examples/run_jump_ocp.py +++ b/examples/run_jump_ocp.py @@ -2,6 +2,7 @@ import crocoddyl # noqa import aligator import numpy as np +import pinocchio as pin import matplotlib.pyplot as plt from quadruped_reactive_walking import Params diff --git a/examples/simple_ocp.py b/examples/simple_ocp.py index df28081..6d87eff 100644 --- a/examples/simple_ocp.py +++ b/examples/simple_ocp.py @@ -1,4 +1,5 @@ import numpy as np +import pinocchio as pin import quadruped_reactive_walking as qrw from quadruped_reactive_walking.controller import Controller import time diff --git a/include/qrw/Estimator.hpp b/include/qrw/Estimator.hpp index ea612ce..d82ee90 100644 --- a/include/qrw/Estimator.hpp +++ b/include/qrw/Estimator.hpp @@ -174,13 +174,13 @@ class Estimator { Vector4 feetStancePhaseDuration_; //< Number of loops during which each foot has been in contact Vector4 feetStatus_; //< Contact status of the four feet - pinocchio::Model model_; //< Pinocchio models for frame computations and forward kinematics - pinocchio::Data data_; //< Pinocchio datas for frame computations and forward kinematics - Vector19 q_FK_; //< Configuration vector for Forward Kinematics - Vector18 v_FK_; //< Velocity vector for Forward Kinematics - Vector3 baseVelocityFK_; //< Base linear velocity estimated by Forward Kinematics - Vector3 basePositionFK_; //< Base position estimated by Forward Kinematics - Vector3 b_baseVelocity_; //< Filtered estimated velocity at center base (base frame) + pinocchio::Model model_; //< Pinocchio models for frame computations and forward kinematics + pinocchio::Data data_; //< Pinocchio datas for frame computations and forward kinematics + Vector19 q_FK_; //< Configuration vector for Forward Kinematics + Vector18 v_FK_; //< Velocity vector for Forward Kinematics + Vector3 baseVelocityFK_; //< Base linear velocity estimated by Forward Kinematics + Vector3 basePositionFK_; //< Base position estimated by Forward Kinematics + Vector3 b_baseVelocity_; //< Filtered estimated velocity at center base (base frame) ComplementaryFilter positionFilter_; //< Complementary filter for base position ComplementaryFilter velocityFilter_; //< Complementary filter for base velocity diff --git a/python/quadruped_reactive_walking/ocp_defs/walking.py b/python/quadruped_reactive_walking/ocp_defs/walking.py index 8e9c4d6..0b08de2 100644 --- a/python/quadruped_reactive_walking/ocp_defs/walking.py +++ b/python/quadruped_reactive_walking/ocp_defs/walking.py @@ -2,7 +2,7 @@ import pinocchio as pin import crocoddyl -from typing import List, Optional +from typing import Optional from quadruped_reactive_walking import Params, ResidualModelFlyHigh from ..wb_mpc import task_spec from ..tools.utils import no_copy_roll, no_copy_roll_insert @@ -361,7 +361,6 @@ def update_model( self.update_tracking_costs(model.differential.costs, base_vel_ref, support_feet) def update_tracking_costs(self, costs, base_vel_ref: pin.Motion, support_feet): - index = 0 for i in self.task.feet_ids: name = "{}_forceReg".format(self.rmodel.frames[i].name) costs.changeCostStatus(name, i in support_feet) @@ -379,4 +378,4 @@ def update_tracking_costs(self, costs, base_vel_ref: pin.Motion, support_feet): if base_vel_ref is not None and self.has_base_vel_cost: name = "base_velocity_tracking" - costs.costs[name].cost.residual.reference = base_vel_ref \ No newline at end of file + costs.costs[name].cost.residual.reference = base_vel_ref diff --git a/python/quadruped_reactive_walking/tools/utils.py b/python/quadruped_reactive_walking/tools/utils.py index a4b16c2..043dc50 100644 --- a/python/quadruped_reactive_walking/tools/utils.py +++ b/python/quadruped_reactive_walking/tools/utils.py @@ -7,6 +7,7 @@ except ImportError: from multiprocessing.shared_memory import SharedMemory + def quaternionToRPY(quat): """ Quaternion (4 x 0) to Roll Pitch Yaw (3 x 1) diff --git a/python/quadruped_reactive_walking/wb_mpc/ocp_crocoddyl.py b/python/quadruped_reactive_walking/wb_mpc/ocp_crocoddyl.py index 2afdcf7..43f3590 100644 --- a/python/quadruped_reactive_walking/wb_mpc/ocp_crocoddyl.py +++ b/python/quadruped_reactive_walking/wb_mpc/ocp_crocoddyl.py @@ -10,6 +10,7 @@ from . import task_spec from ..ocp_defs.walking import WalkingOCPBuilder + class CrocOCP(OCPAbstract): """ Generate a Crocoddyl OCP for the control task. diff --git a/tests/ros/test_ros_service.py b/tests/ros/test_ros_service.py index 231d1bd..52dcfcc 100644 --- a/tests/ros/test_ros_service.py +++ b/tests/ros/test_ros_service.py @@ -50,7 +50,6 @@ def _trigger_solve(self, msg): ) - if __name__ == "__main__": # Parsing arguments parser = argparse.ArgumentParser() @@ -59,17 +58,17 @@ def _trigger_solve(self, msg): parser.add_argument("--service", help="Name of the service to test", default="qrw_wbmpc/test") args = parser.parse_args() - start_client = (args.client or not args.server) - start_server = (args.server or not args.client) + start_client = args.client or not args.server + start_server = args.server or not args.client # Starintg ros node rospy.init_node("qrw_wbmpc" + ("_client" if start_client else "") + ("_server" if start_server else "")) # Running client and/or server - if(start_server): + if start_server: server = TestServer(args.service) - if(start_client): + if start_client: client = TestClient(args.service) client.solve() @@ -77,6 +76,6 @@ def _trigger_solve(self, msg): assert not rospy.is_shutdown() # If the client is not started, let the server running - if(not start_client): + if not start_client: print(f"Server running ({args.service})...") - rospy.spin() \ No newline at end of file + rospy.spin()