diff --git a/examples/math_understanding/path6d_from_path2d.py b/examples/math_understanding/path6d_from_path2d.py
index 8efa02bc57dc3aa9b70960105369ed614f23c61a..41a2da95d176ce42cdd314bd004505732e99fb89 100644
--- a/examples/math_understanding/path6d_from_path2d.py
+++ b/examples/math_understanding/path6d_from_path2d.py
@@ -123,9 +123,9 @@ for p in pathSE3_handlebar:
# rotation = np.eye(3)
# some_path.append(pin.SE3(rotation, translation))
for i in range(100):
- visualizer_manager.sendCommand({"frame_path": pathSE3_handlebar})
+ visualizer_manager.sendCommand({"fixed_frame_path": pathSE3_handlebar})
# visualizer_manager.sendCommand({"frame_path": some_path})
- visualizer_manager.sendCommand({"path": pathSE3_handlebar})
+ visualizer_manager.sendCommand({"fixed_path": pathSE3_handlebar})
time.sleep(1)
print("send em")
diff --git a/examples/math_understanding/rotate_around_z.py b/examples/math_understanding/rotate_around_z.py
index 0c8b1e8d44d0eb4829184cd2838df71d24c34506..fbc5583bc4ef35fb6937aaf9af413fddb6d5bfea 100644
--- a/examples/math_understanding/rotate_around_z.py
+++ b/examples/math_understanding/rotate_around_z.py
@@ -7,7 +7,8 @@ import meshcat_shapes
A = pin.SE3.Identity()
B = pin.SE3.Identity()
-B.rotation = pin.rpy.rpyToMatrix(0.0, 0.0, np.random.random())
+theta_z = np.random.random()
+B.rotation = pin.rpy.rpyToMatrix(0.0, 0.0, theta_z)
B.translation += np.random.random(3)
interpolated = []
diff --git a/examples/navigation/mobile_base_navigation.py b/examples/navigation/mobile_base_navigation.py
index 8ab2d287959307213895169158bf680520c860a1..446cd99e2cd30a7c3b87f85ba7e708339cfdba73 100644
--- a/examples/navigation/mobile_base_navigation.py
+++ b/examples/navigation/mobile_base_navigation.py
@@ -158,11 +158,11 @@ if __name__ == "__main__":
path2D[:, 1] = path2D[:, 1] * args.map_height
path2D = np.hstack((path2D, np.zeros((len(path2D), 1))))
robot.updateViz({"fixed_path": path2D})
- CrocoBaseP2PMPC(args, robot, path2D[0])
+ # CrocoBaseP2PMPC(args, robot, path2D[0])
path_planner = partial(fixedPathPlanner, [0], path2D)
- CrocoBasePathFollowingMPC(args, robot, x0, path_planner)
- # cartesianPathFollowingWithPlanner(args, robot, path_planner)
+ # CrocoBasePathFollowingMPC(args, robot, x0, path_planner)
+ cartesianPathFollowingWithPlanner(args, robot, path_planner, 0.0)
# MPCWithCLIKFallback(args, robot, path_planner)
if args.real:
diff --git a/examples/ros/dummy_cmds_pub.py b/examples/ros/dummy_cmds_pub.py
index cd733707d1af7661054b6ececeefbeb811387a39..8b68dbca6ec447aaccd0abb294edd953a3e52aca 100755
--- a/examples/ros/dummy_cmds_pub.py
+++ b/examples/ros/dummy_cmds_pub.py
@@ -14,6 +14,7 @@
import rclpy
+from rclpy.time import Time
from rclpy.node import Node
from rclpy.executors import MultiThreadedExecutor
from geometry_msgs.msg import Twist, PoseWithCovarianceStamped
@@ -87,6 +88,10 @@ class DummyNode(Node):
self, args, robot: RealMobileYumiRobotManager, loop_manager: ControlLoopManager
):
super().__init__("dummy_cmds_pub_node")
+ self.set_parameters(
+ [rclpy.parameter.Parameter("use_sim_time", rclpy.Parameter.Type.BOOL, True)]
+ )
+ self.wait_for_sim_time()
self.robot = robot
self.loop_manager = loop_manager
self.args = args
@@ -141,10 +146,13 @@ class DummyNode(Node):
JointState, f"{self._ns}/platform/joints_cmd", 1
)
self.robot.set_publisher_joints_cmd(self._cmd_pub)
- self._cmd_pub = self.create_publisher(JointState, f"{self._ns}joints_cmd", 1)
+ # self._cmd_pub = self.create_publisher(JointState, f"{self._ns}joints_cmd", 1)
self.sub_base_odom = self.create_subscription(
Odometry, f"{self._ns}/platform/odometry", self.callback_base_odom, 1
)
+ self.get_logger().info(f"{self._ns}/platform/joints_cmd")
+ self.get_logger().info(f"{self._ns}/platform/odometry")
+ self.get_logger().info(f"{self._ns}/platform/joint_states")
##########
# base is a twist which is constructed from the first 3 msg.velocities
@@ -156,14 +164,24 @@ class DummyNode(Node):
def send_cmd(self):
breakFlag = self.loop_manager.run_one_iter(self.loop_manager.current_iteration)
+ if breakFlag:
+ self.get_logger().info("control loop finished")
+
+ msg = self.empty_msg
+ for i in range(3):
+ msg.velocity[i] = self.robot._v[i]
+ for i in range(15, 29):
+ msg.velocity[i] = self.robot._v[i - 12]
+
+ # msg.header.stamp = Time().to_msg()
+ ## msg.header.stamp = self.get_clock().now().to_msg()
+ msg.header.stamp = self.get_clock().now().to_msg()
+ # msg.velocity[21] = 1.0
+ # msg.velocity[28] = 1.0
+ self._cmd_pub.publish(msg)
- # msg = self.empty_msg
- # for i in range(3):
- # msg.velocity[i] = self.robot._v[i]
- # for i in range(15, 29):
- # msg.velocity[i] = self.robot._v[i - 12]
- # self.get_logger().info(str(self.robot._v))
- # self._cmd_pub.publish(msg)
+ # self.get_logger().info("SEND_JOINTS_CMD ON TIMER" + str(msg))
+ # self.get_logger().info(str(Time()))
def callback_base_odom(self, msg: Odometry):
# self.robot._v[0] = msg.twist.twist.linear.x
@@ -186,14 +204,22 @@ class DummyNode(Node):
self.robot._q[1] = msg.pose.pose.position.y
self.robot._q[2] = np.cos(th)
self.robot._q[3] = np.sin(th)
+ # self.get_logger().info("ODOM CALLBACK" + str(msg))
def callback_arms_state(self, msg: JointState):
if self.loop_manager.current_iteration < 200 or self.args.unreal:
self.robot._q[-14:-7] = msg.position[-14:-7]
self.robot._q[-7:] = msg.position[-7:]
- # self.get_logger().info(str(msg))
+ # self.get_logger().info("JOINT_STATE CALLBACK" + str(msg))
# self.get_logger().info(str(self.loop_manager.current_iteration))
+ def wait_for_sim_time(self):
+ """Wait for the /clock topic to start publishing."""
+ self.get_logger().info("Waiting for simulated time to be active...")
+ while not self.get_clock().now().nanoseconds > 0:
+ rclpy.spin_once(self, timeout_sec=0.1)
+ self.get_logger().info("Simulated time is now active!")
+
# def main(args=None):
def main(args=None):
@@ -210,14 +236,19 @@ def main(args=None):
T_absgoal_r = pin.SE3.Identity()
T_absgoal_r.translation = np.array([0.0, -0.2, 0.0])
# loop_manager = CrocoIKMPC(args, robot, goal, run=False)
- # robot._mode = robot.control_mode.upper_body
- # loop_manager = moveJP(robot._comfy_configuration, args_smc, robot, run=False)
- # loop_manager = moveLDualArm(
- # args_smc, robot, T_w_absgoal, T_absgoal_l, T_absgoal_r, run=False
- # )
- # loop_manager = moveL(args_smc, robot, T_w_absgoal, run=False)
- robot._mode = robot.control_mode.whole_body
- loop_manager = CrocoBaseP2PMPC(args_smc, robot, np.zeros(3), run=False)
+ # TODO: put all loops and corresponding modes in this list
+ # then in cmd_pub callback go to the next loop
+ # once you get a breakFlag == True
+ loops_n_modes = []
+ mode = robot.control_mode.upper_body
+ loop_manager = moveJP(robot._comfy_configuration, args_smc, robot, run=False)
+ loops_n_modes.append(mode, loop_manager)
+ loop_manager = moveLDualArm(
+ args_smc, robot, T_w_absgoal, T_absgoal_l, T_absgoal_r, run=False
+ )
+ loops_n_modes.append(mode, loop_manager)
+ # robot._mode = robot.control_mode.whole_body
+ # loop_manager = CrocoBaseP2PMPC(args_smc, robot, np.zeros(3), run=False)
rclpy.init(args=args)
diff --git a/examples/ros/navigation_example.py b/examples/ros/navigation_example.py
index 52ae0990347976133b543025358a5a9628b2f80b..f20b10ad94891fde296f9be1ca1953c718a8f959 100644
--- a/examples/ros/navigation_example.py
+++ b/examples/ros/navigation_example.py
@@ -127,7 +127,7 @@ def get_args():
args.ctrl_freq = 50
# NOTE: does not work due to ctrl-c being overriden
args.save_log = True
- args.run_name = "pls"
+ args.run_name = "nav_pls"
args.index_runs = True
return args
@@ -190,6 +190,7 @@ class DummyNode(Node):
self._cmd_pub = self.create_publisher(
JointState, f"{self._ns}/platform/joints_cmd", 1
)
+ self.get_logger().info(f"{self._ns}/platform/joints_cmd")
self.robot.set_publisher_joints_cmd(self._cmd_pub)
self._cmd_pub = self.create_publisher(JointState, f"{self._ns}joints_cmd", 1)
self.sub_base_odom = self.create_subscription(
diff --git a/examples/ros/stop_robot.py b/examples/ros/stop_robot.py
index 5561e75d2907784a39910011fc79cae63ecee3ef..ee3a252e8885e2da80cdfa340238618e953df908 100644
--- a/examples/ros/stop_robot.py
+++ b/examples/ros/stop_robot.py
@@ -41,7 +41,7 @@ class DummyNode(Node):
self.get_logger().info(f"### Starting dummy example under namespace {self._ns}")
self._cmd_pub = self.create_publisher(
- JointState, f"{self._ns}/platform/joints_cmd", 1
+ JointState, f"/{self._ns}/platform/joints_cmd", 1
)
self.empty_msg = JointState()
for i in range(29):
diff --git a/python/smc/control/cartesian_space/cartesian_space_trajectory_following.py b/python/smc/control/cartesian_space/cartesian_space_trajectory_following.py
index 949d83dec5d9dde3caca689c3f409c4bf46be26c..280cb399b0a07d0c8ae7b00c41eb41093ce50661 100644
--- a/python/smc/control/cartesian_space/cartesian_space_trajectory_following.py
+++ b/python/smc/control/cartesian_space/cartesian_space_trajectory_following.py
@@ -21,6 +21,7 @@ import types
def cartesianPathFollowingControlLoop(
ik_solver: Callable[[np.ndarray, np.ndarray], np.ndarray],
path: list[pin.SE3] | np.ndarray,
+ rot_x: float,
args: Namespace,
robot: SingleArmInterface,
t: int,
@@ -34,12 +35,12 @@ def cartesianPathFollowingControlLoop(
# TODO: refactor this horror out of here
if type(path) == np.ndarray:
- path = path2D_to_SE3(path[:, :2], 0.0)
+ path = path2D_to_SE3(path[:, :2], 0.0, rot_x)
# TODO: arbitrary bs, read a book and redo this
# NOTE: assuming the first path point coincides with current pose
SEerror = robot.T_w_e.actInv(path[1])
- V_path = pin.log6(path[0].actInv(path[1])).vector
- err_vector = pin.log6(SEerror).vector + V_path
+ # V_path = pin.log6(path[0].actInv(path[1])).vector
+ err_vector = pin.log6(SEerror).vector # + V_path
J = robot.getJacobian()
v_cmd = ik_solver(J, err_vector)
diff --git a/python/smc/control/control_loop_manager.py b/python/smc/control/control_loop_manager.py
index d7bfefb8a9b59f0337eadddd1400cf989e06cf6d..d67efe15579fcab42b123c4e865dc27e0e23c733 100644
--- a/python/smc/control/control_loop_manager.py
+++ b/python/smc/control/control_loop_manager.py
@@ -75,6 +75,7 @@ class ControlLoopManager:
self.args: Namespace = args
self.iter_n: int = 0
self.past_data: dict[str, deque[np.ndarray]] = {}
+ self.current_iteration = 0
# NOTE: viz update rate is a magic number that seems to work fine and i don't have
# any plans to make it smarter
if args.viz_update_rate < 0 and args.ctrl_freq > 0:
@@ -114,6 +115,7 @@ class ControlLoopManager:
it's the controlLoop's responsibility to break if it achieved it's goals.
this is done via the breakFlag
"""
+ self.current_iteration += 1
# NOTE: all required pre-computations are handled here
self.robot_manager._step()
# TODO make the arguments to controlLoop kwargs or whatever
@@ -185,7 +187,9 @@ class ControlLoopManager:
def run(self):
self.final_iteration = 0
+ self.current_iteration = 0
for i in range(self.max_iterations):
+ self.current_iteration = i
start = time.time()
breakFlag = self.run_one_iter(i)
diff --git a/python/smc/control/controller_templates/path_following_template.py b/python/smc/control/controller_templates/path_following_template.py
index 49aa904b984fd466d9610e7de9d25a2f657c9303..76fa3d074872032946873c3ba3af21b261bdf2e0 100644
--- a/python/smc/control/controller_templates/path_following_template.py
+++ b/python/smc/control/controller_templates/path_following_template.py
@@ -52,7 +52,7 @@ def PathFollowingFromPlannerCtrllLoopTemplate(
breakFlag = True
if data == "done" or data is None:
- robot.sendVelocityCommand(np.zeros(robot.model.nv))
+ robot.sendVelocityCommand(np.zeros(robot.nv))
log_item["qs"] = robot.q
log_item["dqs"] = robot.v
return breakFlag, save_past_item, log_item
diff --git a/python/smc/control/joint_space/joint_space_point_to_point.py b/python/smc/control/joint_space/joint_space_point_to_point.py
index 06bc77d001d1f683a152955de5b50d1c9c0d29ad..823e1970271116790019f6ab0d5e2b96fcaf21bf 100644
--- a/python/smc/control/joint_space/joint_space_point_to_point.py
+++ b/python/smc/control/joint_space/joint_space_point_to_point.py
@@ -55,7 +55,9 @@ def moveJControlLoop(
# fix this by tuning or whatever else.
# MOVEL works just fine, so apply whatever's missing for there here
# and that's it.
-def moveJP(q_desired: np.ndarray, args: Namespace, robot: AbstractRobotManager) -> None:
+def moveJP(
+ q_desired: np.ndarray, args: Namespace, robot: AbstractRobotManager, run=True
+) -> ControlLoopManager | None:
"""
moveJP
---------------
@@ -72,9 +74,10 @@ def moveJP(q_desired: np.ndarray, args: Namespace, robot: AbstractRobotManager)
"err_norm": np.zeros((1,)),
}
loop_manager = ControlLoopManager(robot, controlLoop, args, {}, log_item)
- loop_manager.run()
- if args.debug_prints:
- print("MoveJP done: convergence achieved, reached destionation!")
+ if run:
+ loop_manager.run()
+ else:
+ return loop_manager
def moveJPIControlLoop(
diff --git a/python/smc/control/optimal_control/abstract_croco_ocp.py b/python/smc/control/optimal_control/abstract_croco_ocp.py
index f7a6a6aa09bce8a7c145aa0a075b822a3ad1d21e..6aa842201b646636ce47820011722c8c7ea9e8b0 100644
--- a/python/smc/control/optimal_control/abstract_croco_ocp.py
+++ b/python/smc/control/optimal_control/abstract_croco_ocp.py
@@ -240,10 +240,11 @@ class CrocoOCP(abc.ABC):
# solver = crocoddyl.SolverIpopt(problem)
# TODO: select other solvers from arguments
self.solver = crocoddyl.SolverBoxFDDP(self.problem)
- if self.args.debug_prints:
- self.solver.setCallbacks(
- [crocoddyl.CallbackVerbose(), crocoddyl.CallbackLogger()]
- )
+
+ # if self.args.debug_prints:
+ # self.solver.setCallbacks(
+ # [crocoddyl.CallbackVerbose(), crocoddyl.CallbackLogger()]
+ # )
# NOTE: used by csqp
def createMimSolver(self) -> None:
@@ -257,10 +258,11 @@ class CrocoOCP(abc.ABC):
# solver = mim_solvers.SolverSQP(problem)
# TODO: select other solvers from arguments
self.solver = mim_solvers.SolverCSQP(self.problem)
- if self.args.debug_prints:
- self.solver.setCallbacks(
- [mim_solvers.CallbackVerbose(), mim_solvers.CallbackLogger()]
- )
+
+ # if self.args.debug_prints:
+ # self.solver.setCallbacks(
+ # [mim_solvers.CallbackVerbose(), mim_solvers.CallbackLogger()]
+ # )
# this shouldn't really depend on x0 but i can't be bothered
def solveInitialOCP(self, x0: np.ndarray):
diff --git a/python/smc/control/optimal_control/croco_path_following/mpc/base_reference_mpc.py b/python/smc/control/optimal_control/croco_path_following/mpc/base_reference_mpc.py
index 8a1399373896312db1bc69141a2e34ae7880fb7a..1f5a2b594974434d2f9d04c6c2b015e8d4a79e31 100644
--- a/python/smc/control/optimal_control/croco_path_following/mpc/base_reference_mpc.py
+++ b/python/smc/control/optimal_control/croco_path_following/mpc/base_reference_mpc.py
@@ -51,6 +51,8 @@ def CrocoBasePathFollowingMPCControlLoop(
xs = np.array(ocp.solver.xs)
v_cmd = xs[1, robot.model.nq :]
+ # NOTE: dirty hack for wholebody mode
+ # v_cmd[3:] = 0.0
err_vector_base = np.linalg.norm(p - path_base[0][:2]) # z axis is irrelevant
log_item = {}
log_item["err_norm_base"] = np.linalg.norm(err_vector_base).reshape((1,))
diff --git a/python/smc/control/optimal_control/croco_point_to_point/mpc/base_reference_mpc.py b/python/smc/control/optimal_control/croco_point_to_point/mpc/base_reference_mpc.py
index 0399371fad1fffe53c10dfc6014f142740cb6da5..d74fc556bbdc82aeaa086cf99aac0ac547f9e491 100644
--- a/python/smc/control/optimal_control/croco_point_to_point/mpc/base_reference_mpc.py
+++ b/python/smc/control/optimal_control/croco_point_to_point/mpc/base_reference_mpc.py
@@ -14,6 +14,8 @@ from functools import partial
from collections import deque
from argparse import Namespace
+from IPython import embed
+
def CrocoBaseP2PMPCControlLoop(
ocp: CrocoOCP,
@@ -29,11 +31,14 @@ def CrocoBaseP2PMPCControlLoop(
"""
# set initial state from sensor
x0 = np.concatenate([robot.q, robot.v])
+ # embed()
ocp.warmstartAndReSolve(x0)
xs = np.array(ocp.solver.xs)
# NOTE: for some reason the first value is always some wild bs
- vel_cmd = xs[1, robot.model.nq :]
- return vel_cmd, {}, {}
+ v_cmd = xs[1, robot.model.nq :]
+ # NOTE: dirty hack to work if wholebody mode, don't ask
+ v_cmd[3:] = 0.0
+ return v_cmd, {}, {}
def CrocoBaseP2PMPC(
diff --git a/python/smc/path_generation/path_math/path2d_to_6d.py b/python/smc/path_generation/path_math/path2d_to_6d.py
index 752429325a86aa80631ac172e0d25daf550b2f6b..5adcce9c23af09c9dd597eb6017b00b8da4da7bc 100644
--- a/python/smc/path_generation/path_math/path2d_to_6d.py
+++ b/python/smc/path_generation/path_math/path2d_to_6d.py
@@ -2,7 +2,9 @@ import pinocchio as pin
import numpy as np
-def path2D_to_SE3(path2D: np.ndarray, path_height: float) -> list[pin.SE3]:
+def path2D_to_SE3(
+ path2D: np.ndarray, path_height: float, rot_x: float
+) -> list[pin.SE3]:
"""
path2D_SE3
----------
@@ -35,13 +37,22 @@ def path2D_to_SE3(path2D: np.ndarray, path_height: float) -> list[pin.SE3]:
for i in range(len(path2D) - 1):
# first set the x axis to be in the theta direction
# TODO: make sure this one makes sense
+ # rotation = np.array(
+ # [
+ # [np.cos(thetas[i]), np.sin(thetas[i]), 0.0],
+ # [np.sin(thetas[i]), -np.cos(thetas[i]), 0.0],
+ # [0.0, 0.0, -1.0],
+ # ]
+ # )
rotation = np.array(
[
- [np.cos(thetas[i]), np.sin(thetas[i]), 0.0],
- [np.sin(thetas[i]), -np.cos(thetas[i]), 0.0],
- [0.0, 0.0, -1.0],
+ [np.cos(thetas[i]), -np.sin(thetas[i]), 0.0],
+ [np.sin(thetas[i]), np.cos(thetas[i]), 0.0],
+ [0.0, 0.0, 1.0],
]
)
+ rot_mat_x = pin.rpy.rpyToMaxtrix(rox_t, 0.0, 0.0)
+ rotation = rot_mat_x @ rotation
# rotation = pin.rpy.rpyToMatrix(0.0, 0.0, thetas[i])
# rotation = pin.rpy.rpyToMatrix(np.pi / 2, np.pi / 2, 0.0) @ rotation
translation = np.array([path2D[i][0], path2D[i][1], path_height])
diff --git a/python/smc/robots/abstract_robotmanager.py b/python/smc/robots/abstract_robotmanager.py
index baa81d88320cea0f6982a8f0e25c68dff3cb8507..6c10a00b76eaba0263334541df4851587b515406 100644
--- a/python/smc/robots/abstract_robotmanager.py
+++ b/python/smc/robots/abstract_robotmanager.py
@@ -331,10 +331,7 @@ class AbstractRobotManager(abc.ABC):
else:
print("not implemented yet, so nothing is going to happen!")
-
- def getJacobian(self) -> np.ndarray:
- ...
-
+ def getJacobian(self) -> np.ndarray: ...
########################################################################################
# visualizer management. ideally transferred elsewhere
diff --git a/python/smc/robots/implementations/mobile_yumi.py b/python/smc/robots/implementations/mobile_yumi.py
index f1d6850da7139b95cde04805d08865dfbbb95595..35671a7a13f13e2e5e15dd34fd9595f37a2d3e1f 100644
--- a/python/smc/robots/implementations/mobile_yumi.py
+++ b/python/smc/robots/implementations/mobile_yumi.py
@@ -11,6 +11,7 @@ from smc.robots.abstract_robotmanager import AbstractRobotManager
import pinocchio as pin
from argparse import Namespace
import numpy as np
+from importlib.util import find_spec
if find_spec("rclpy"):
from rclpy.time import Time
@@ -120,28 +121,33 @@ class RealMobileYumiRobotManager(AbstractMobileYuMiRobotManager):
print("set publisher_joints_cmd into RobotManager")
def sendVelocityCommandToReal(self, v: np.ndarray):
- # qd_base = qd[:3]
- # qd_left = qd[3:10]
- # qd_right = qd[10:]
- # self.publisher_vel_base(qd_base)
- # self.publisher_vel_left(qd_left)
- # self.publisher_vel_right(qd_right)
- empty_msg = JointState()
- for i in range(29):
- empty_msg.velocity.append(0.0)
- msg = empty_msg
- msg.header.stamp = Time().to_msg()
- for i in range(3):
- msg.velocity[i] = v[i]
- for i in range(15, 29):
- msg.velocity[i] = v[i - 12]
-
- self.publisher_joints_cmd.publish(msg)
-
- # TODO: define set initial pose by reading it from the real robot (well, the appropriate ros2 topic in this case)
+
+ if self.args.unreal:
+ if any(np.isnan(v)):
+ v = np.zeros(self.model.nv)
+ self._v = v
+ # empty_msg = JointState()
+ # for i in range(29):
+ # empty_msg.velocity.append(0.0)
+ # msg = empty_msg
+ # msg.header.stamp = Time().to_msg()
+ # for i in range(3):
+ # msg.velocity[i] = v[i]
+ # for i in range(15, 29):
+ # msg.velocity[i] = v[i - 12]
+ # if hasattr(self, "publisher_joints_cmd"):
+ # self.publisher_joints_cmd.publish(msg)
+
+ else:
+ self._v = v
+ # NOTE: we update joint angles here, and _updateQ does nothing (there is no communication)
+ self._q = pin.integrate(self.model, self._q, v * self._dt)
+
+ # NOTE: handled by a topic callback
def _updateQ(self):
pass
+ # NOTE: handled by a topic callback
def _updateV(self):
pass
@@ -157,6 +163,18 @@ class RealMobileYumiRobotManager(AbstractMobileYuMiRobotManager):
def connectToRobot(self):
pass
+ def setInitialPose(self):
+ self._q = pin.randomConfiguration(
+ self.model, self.model.lowerPositionLimit, self.model.upperPositionLimit
+ )
+ # pin.RandomConfiguration does not work well for planar joint,
+ # or at least i remember something along those lines being the case
+ self._q[0] = np.random.random() * 0.1
+ self._q[1] = np.random.random() * 0.1
+ theta = np.random.random() * 2 * np.pi - np.pi
+ self._q[2] = np.cos(theta)
+ self._q[3] = np.sin(theta)
+
# TODO: create simulated gripper class to support the move, open, close methods - it's a mess now
# in simulation we can just set the values directly
def connectToGripper(self):
diff --git a/python/smc/visualization/meshcat_viewer_wrapper/visualizer.py b/python/smc/visualization/meshcat_viewer_wrapper/visualizer.py
index 90e833ee646d86ad4c0fefbc6304e3afc2032f2d..b947cbaa3fae5cc21ce81706cde6b8c6ad7671f4 100644
--- a/python/smc/visualization/meshcat_viewer_wrapper/visualizer.py
+++ b/python/smc/visualization/meshcat_viewer_wrapper/visualizer.py
@@ -42,8 +42,9 @@ class MeshcatVisualizer(PMV):
# there will be times when i just want to drop in points
# which will never be changed
self.n_points = 0
- self.n_path_points = 0
+ # self.n_path_points = 0
self.add_path_points_set = set()
+ self.add_fixed_path_points_set = set()
self.n_frame_path_points = 0
self.add_frame_path_points_set = set()
self.n_obstacles = 0
@@ -112,22 +113,28 @@ class MeshcatVisualizer(PMV):
self, name, path: list[pin.SE3] | np.ndarray, radius=5e-3, color=[0, 0, 1, 0.8]
):
# NOTE: there's too much of them so the visualization is slow
- self.n_path_points = len(path) if len(path) < 5 else 5
+ # self.n_path_points = len(path) if len(path) < 5 else 5
if name == "fixed_path":
color = [1, 0, 0, 0.8]
+ path_viz = path.copy()
if type(path) == np.ndarray:
# complete the quartenion
+ # if path.shape[1] == 2:
path_viz = np.hstack((path, np.zeros((len(path), 3))))
path_viz = np.hstack((path_viz, np.ones((len(path), 1))))
- else:
- path_viz = path.copy()
index_step = len(path_viz) // 5
for i, point in enumerate(path_viz):
- if (i % index_step != 0) and name != "fixed_path":
+ if (i % index_step != 0) and "fixed" not in name:
continue
- if i not in self.add_path_points_set:
- self.addSphere(f"world/path_{name}_point_{i}", radius, color)
- self.add_path_points_set.add(i)
+ # TODO: refactor ugly bs with same code under different if
+ if name == "path":
+ if i not in self.add_path_points_set:
+ self.addSphere(f"world/path_{name}_point_{i}", radius, color)
+ self.add_path_points_set.add(i)
+ if name == "fixed_path":
+ if i not in self.add_fixed_path_points_set:
+ self.addSphere(f"world/path_{name}_point_{i}", radius, color)
+ self.add_path_points_set.add(i)
self.applyConfiguration(f"world/path_{name}_point_{i}", point)
# self.n_path_points = max(i, self.n_path_points)
diff --git a/python/smc/visualization/visualizer.py b/python/smc/visualization/visualizer.py
index 3abc3df4232c841325df26899628405c086004cb..5a816e5aee8b7c9fb35379dc176abf7b13071f52 100644
--- a/python/smc/visualization/visualizer.py
+++ b/python/smc/visualization/visualizer.py
@@ -24,6 +24,7 @@ def manipulatorVisualizer(
viz = MeshcatVisualizer(
model=model, collision_model=collision_model, visual_model=visual_model
)
+
viz.loadViewerModel()
# display the initial pose
viz.display(cmd["q"])
@@ -69,15 +70,13 @@ def manipulatorVisualizer(
if key == "obstacle_box":
# stupid and evil but there is no time
viz.addBoxObstacle(cmd["obstacle_box"][0], cmd["obstacle_box"][1])
- if key == "path":
- # stupid and evil but there is no time
- viz.addPath("path", cmd["path"])
- if key == "fixed_path":
- # stupid and evil but there is no time
- viz.addPath("fixed_path", cmd["fixed_path"])
- if key == "frame_path":
+ if "path" in key:
# stupid and evil but there is no time
- viz.addFramePath("", cmd["frame_path"])
+ if not "frame" in key:
+ viz.addPath(key, cmd[key])
+ else:
+ # stupid and evil but there is no time
+ viz.addFramePath(key, cmd[key])
except KeyboardInterrupt:
if args.debug_prints: