diff --git a/python/examples/.smc_yumi_challenge.py.swp b/python/examples/.smc_yumi_challenge.py.swp
new file mode 100644
index 0000000000000000000000000000000000000000..df6c5270b503b62a62ad1f626b8dd88eaca12a40
Binary files /dev/null and b/python/examples/.smc_yumi_challenge.py.swp differ
diff --git a/python/examples/dummy_cmds_pub.py b/python/examples/dummy_cmds_pub.py
index a3aa1517a82a9f95a16170b63e3e5cf5c52feec1..b77cb146e9c27f4feda135921c5b630b6479b85b 100755
--- a/python/examples/dummy_cmds_pub.py
+++ b/python/examples/dummy_cmds_pub.py
@@ -12,6 +12,24 @@
# WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY
# WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+
+import rclpy
+from rclpy.node import Node
+from rclpy.executors import MultiThreadedExecutor
+from geometry_msgs.msg import Twist, PoseWithCovarianceStamped
+from sensor_msgs.msg import JointState
+from nav_msgs.msg import Odometry
+from abb_python_utilities.names import get_rosified_name
+
+import numpy as np
+import argparse
+from ur_simple_control.managers import getMinimalArgParser, ControlLoopManager, RobotManager
+from ur_simple_control.clik.clik import getClikArgs, getClikController, controlLoopClik, moveL, compliantMoveL, controlLoopCompliantClik, invKinmQP, moveLDualArm
+from ur_simple_control.optimal_control.get_ocp_args import get_OCP_args
+from ur_simple_control.optimal_control.crocoddyl_mpc import CrocoIKMPC
+import pinocchio as pin
+
import numpy as np
import rclpy
@@ -23,15 +41,33 @@ from rclpy.node import Node
from abb_python_utilities.names import get_rosified_name
+# you will need to manually set everything here :(
+def get_args():
+ parser = getMinimalArgParser()
+ parser.description = 'Run closed loop inverse kinematics \
+ of various kinds. Make sure you know what the goal is before you run!'
+ parser = getClikArgs(parser)
+ parser = get_OCP_args(parser)
+ parser.add_argument('--ros-namespace', type=str, default="maskinn", help="you MUST put in ros namespace you're using")
+ #parser.add_argument('--ros-args', action='extend', nargs="+")
+ #parser.add_argument('-r', action='extend', nargs="+")
+ parser.add_argument('--ros-args', action='append', nargs='*')
+ parser.add_argument('-r', action='append', nargs='*')
+ parser.add_argument('-p', action='append', nargs='*')
+ args = parser.parse_args()
+ args.robot = "yumi"
+ args.save_log = True
+ args.real_time_plotting = False
+ return args
+
+
class DummyNode(Node):
- def __init__(self):
+ def __init__(self, args, robot_manager : RobotManager, loop_manager : ControlLoopManager):
super().__init__("dummy_cmds_pub_node")
self._cb = ReentrantCallbackGroup()
self._ns = get_rosified_name(self.get_namespace())
- # test
- self._ns = "maskinn"
self._cmd_pub = self.create_publisher(JointState, f"{self._ns}/joints_cmd", 1)
@@ -48,26 +84,99 @@ class DummyNode(Node):
self._pub_timer = self.create_timer(self._dt, self.send_cmd)
- def send_cmd(self):
- msg = self.empty_msg
- msg.header.stamp = self.get_clock().now().to_msg()
- msg.velocity[0] = 1.0
- msg.velocity[1] = 1.0
- msg.velocity[2] = 1.0
- msg.velocity[3] = 1.0
- msg.velocity[4] = 1.0
- msg.velocity[5] = 1.0
- msg.velocity[21] = self._A * np.sin(2 * np.pi / self._T * self._t)
- msg.velocity[28] = self._A * np.sin(2 * np.pi / self._T * self._t)
- self._cmd_pub.publish(msg)
- #print("published")
- self._t += self._dt
+########################################################
+# connect to smc
+###########################################################
+
+ robot_manager.set_publisher_joints_cmd(self._cmd_pub)
+ self.robot_manager = robot_manager
+ self.loop_manager = loop_manager
+ self.args = args
+ # give me the latest thing even if there wasn't an update
+ qos_prof = rclpy.qos.QoSProfile(
+ reliability=rclpy.qos.QoSReliabilityPolicy.RELIABLE,
+ durability = rclpy.qos.DurabilityPolicy.TRANSIENT_LOCAL,
+ history = rclpy.qos.HistoryPolicy.KEEP_LAST,
+ depth = 1)
+
+# self.sub_base_odom = self.create_subscription(Odometry, f"{self._ns}platform/odometry", self.callback_base_odom, qos_prof)
+ # self.sub_amcl = self.create_subscription(PoseWithCovarianceStamped, '/amcl_pose', self.pose_callback, qos_prof)
+
+ # this qos is incompatible
+ #self.sub_joint_states = self.create_subscription(JointState, f"{self._ns}/joint_states", self.callback_arms_state, qos_prof)
+
+ qos_prof2 = rclpy.qos.QoSProfile(
+ # reliability=rclpy.qos.QoSReliabilityPolicy.RELIABLE,
+ # durability = rclpy.qos.DurabilityPolicy.TRANSIENT_LOCAL,
+ # history = rclpy.qos.HistoryPolicy.KEEP_LAST,
+ depth = 1)
+ self.sub_joint_states = self.create_subscription(JointState, f"{self._ns}/joint_states", self.callback_arms_state, qos_prof2)
+
+##########
+# base is a twist which is constructed from the first 3 msg.velocities
+# 0 -> twist.linear.x
+# 1 -> twist.linear.y
+# 2 -> twist.angular.z
+# left arm indeces are 15-21 (last included)
+# right arm indeces are 22-28 (last included)
+
+ def send_cmd(self):
+ breakFlag = self.loop_manager.run_one_iter(0)
+ #msg = self.empty_msg
+ #msg.header.stamp = self.get_clock().now().to_msg()
+ #msg.velocity[0] = self._A * np.sin(2 * np.pi / self._T * self._t)
+ #msg.velocity[1] = self._A * np.sin(2 * np.pi / self._T * self._t)
+ #msg.velocity[2] = self._A * np.sin(2 * np.pi / self._T * self._t)
+ #self._cmd_pub.publish(msg)
+ #self._t += self._dt
+
+
+ def callback_base_odom(self, msg : Odometry):
+ self.robot_manager.v_q[0] = msg.twist.twist.linear.x
+ self.robot_manager.v_q[1] = msg.twist.twist.linear.y
+ # TODO: check that z can be used as cos(theta) and w as sin(theta)
+ # (they could be defined some other way or theta could be offset of something)
+ self.robot_manager.v_q[2] = msg.twist.twist.angular.z
+ self.robot_manager.v_q[3] = 0 # for consistency
+ print("received /odom")
+
+ def callback_arms_state(self, msg : JointState):
+ self.robot_manager.q[0] = 0.0
+ self.robot_manager.q[1] = 0.0
+ self.robot_manager.q[2] = 1.0
+ self.robot_manager.q[3] = 0.0
+ self.robot_manager.q[-14:-7] = msg.position[-14:-7]
+ self.robot_manager.q[-7:] = msg.position[-7:]
+ self.get_logger().info('jebem ti mamu')
+
+
+#def main(args=None):
def main(args=None):
+ # evil and makes args unusable but what can you do
+ args_smc = get_args()
+ #print(args_smc)
rclpy.init(args=args)
+
+ robot = RobotManager(args_smc)
+# you can't do terminal input with ros
+ #goal = robot.defineGoalPointCLI()
+ goal = pin.SE3.Identity()
+ goal.translation = np.array([0.3,0.0,0.5])
+ if robot.robot_name == "yumi":
+ goal.rotation = np.eye(3)
+ goal_transform = pin.SE3.Identity()
+ goal_transform.translation[1] = 0.1
+ goal_left = goal_transform.act(goal)
+ goal_left = goal_transform.inverse().act(goal)
+# goal = (goal_left, goal_right)
+ #loop_manager = CrocoIKMPC(args, robot, goal, run=False)
+ loop_manager = moveLDualArm(args_smc, robot, goal, goal_transform, run=False)
+
+
executor = MultiThreadedExecutor()
- node = DummyNode()
+ node = DummyNode(args, robot, loop_manager)
executor.add_node(node)
executor.spin()
diff --git a/python/examples/smc_yumi_challenge.py b/python/examples/smc_yumi_challenge.py
index 2af6a7c911be229f85c081adbf7beb389e99b317..d317f054684711ea3b6c5fc43c66bd25abada0d3 100644
--- a/python/examples/smc_yumi_challenge.py
+++ b/python/examples/smc_yumi_challenge.py
@@ -188,10 +188,3 @@ def main(args=None):
if __name__ == "__main__":
main()
-
-
-
-
-
-
-
diff --git a/python/ur_simple_control/.managers.py.swp b/python/ur_simple_control/.managers.py.swp
index d0587e1010aa996ade5bcbe92c010aa75a4c6838..e7d74c6d65fcc80968d7fb50b836b82eb7e3379c 100644
Binary files a/python/ur_simple_control/.managers.py.swp and b/python/ur_simple_control/.managers.py.swp differ
diff --git a/python/ur_simple_control/__pycache__/managers.cpython-310.pyc b/python/ur_simple_control/__pycache__/managers.cpython-310.pyc
index 8344427e05627676833671090b0d83c03dc1c605..1600e6db912b8abfba2f23fb2dcc1c45018cf938 100644
Binary files a/python/ur_simple_control/__pycache__/managers.cpython-310.pyc and b/python/ur_simple_control/__pycache__/managers.cpython-310.pyc differ
diff --git a/python/ur_simple_control/managers.py b/python/ur_simple_control/managers.py
index 48467bbdf32b98a0fbce034211a61f277ba5f70f..00c2447c8d0df5fa51009575d34759075b0ea940 100644
--- a/python/ur_simple_control/managers.py
+++ b/python/ur_simple_control/managers.py
@@ -32,6 +32,8 @@ import pickle
import importlib.util
if importlib.util.find_spec('rclpy'):
from geometry_msgs import msg
+ from sensor_msgs.msg import JointState
+ from rclpy.time import Time
"""
general notes
@@ -934,17 +936,28 @@ class RobotManager:
self.q = pin.integrate(self.model, self.q, qd_cmd * self.dt)
if self.robot_name == "yumi":
- qd_cmd = np.clip(qd, -1 * self.model.velocityLimit, self.model.velocityLimit)
+ qd_cmd = np.clip(qd, -0.1 * self.model.velocityLimit, 0.1 *self.model.velocityLimit)
self.v_q = qd_cmd
- if self.args.pinocchio_only:
- self.q = pin.integrate(self.model, self.q, qd_cmd * self.dt)
- else:
- 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)
+ # if self.args.pinocchio_only:
+ # self.q = pin.integrate(self.model, self.q, qd_cmd * self.dt)
+ # else:
+ # 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] = qd_cmd[i]
+ for i in range(15, 30):
+ msg.velocity[i] = qd_cmd[i - 12]
+
+ self.publisher_joints_cmd.publish(msg)
@@ -1091,6 +1104,11 @@ class RobotManager:
def set_publisher_vel_right(self, publisher_vel_right):
self.publisher_vel_right = publisher_vel_right
print("set vel_right_publisher into robotmanager")
+
+ def set_publisher_joints_cmd(self, publisher_joints_cmd):
+ self.publisher_joints_cmd = publisher_joints_cmd
+ print("set publisher_joints_cmd into RobotManager")
+
class ProcessManager:
diff --git a/python/ur_simple_control/robot_descriptions/urdf/__pycache__/__init__.cpython-310.pyc b/python/ur_simple_control/robot_descriptions/urdf/__pycache__/__init__.cpython-310.pyc
index b21ae860d8993fbb7c2b56d3cb77e67b296b9dff..90610d17abec485ff327a863a03e55b8bc09ef3f 100644
Binary files a/python/ur_simple_control/robot_descriptions/urdf/__pycache__/__init__.cpython-310.pyc and b/python/ur_simple_control/robot_descriptions/urdf/__pycache__/__init__.cpython-310.pyc differ