Redundancy Control

[SUUHOLEE]

Hi mhubii

I have a question that came up while using a robot.

In the case of Med14's robot, it has 7 degrees of freedom and 1 redundant degree of freedom. Therefore, even if you input the position and orientation of the end effector, there are multiple possible poses (the pose changes every time rviz is run).

Is there a way to resolve this issue? (redundancy control)

Thank you

[mhubii]

are you using the robot through moveit?

[SUUHOLEE]

Yes, i use robot through moveit (med14)

[mhubii]

I have to admit that I don't know how to prefer certain solutions over others with moveit

[SUUHOLEE]

If i am not use moveit, could you admit for me?
How to use redundancy control?

[mhubii]

this depends a little on what exactly you are trying to achieve. You could formulate a controller and project some additional tasks into the nullspace of the Jacobian

[SUUHOLEE]

To be more specific, I wrote code to perform a sequenced motion with multiple movements, but due to the multiple degrees of freedom, there is a problem in that the pose changes each time it is executed due to multiple poses.
So I want to optimize the angle of the joint in the first pose, but I don't know how to do this.
(If you have pose 1, pose 2, and pose 3, you want to optimize the angle of joint 1 of pose 1.)

I know that optimization can be done using Null space Jacobian, but I can't find how to solve inverse kinematics.

[mhubii]

I am assuming right now you are using moveit through search, e.g. RRT.

There is the possibility to

1. Compute inverse kinematics through moveit
2. Send a joint state goal to moveit

You should be able to set an initial joint state and therefore, your results over multiple trials should be equal.

Here is some example code that performs steps 1 and 2: #152 (comment)

You should be able to combine this with your sequenced motion.

[SUUHOLEE]

i use this code ( maybe some modified )#152 (comment)

but not always same results joint angle :(

[mhubii]

okay I see. Maybe if you could share your code, I can try to have a look at it. Otherwise we will have to consult Moveit documentation I am afraid

[SUUHOLEE]

from typing import List

import rclpy
from geometry_msgs.msg import Point, Pose, Quaternion
from moveit_msgs.action import MoveGroupSequence
from moveit_msgs.msg import (
    BoundingVolume,
    Constraints,
    MotionPlanRequest,
    MotionSequenceItem,
    OrientationConstraint,
    PositionConstraint,
)
from rclpy.action import ActionClient
from rclpy.node import Node
from shape_msgs.msg import SolidPrimitive
from std_msgs.msg import Header


class SequencedMotion(Node):
    def __init__(self):
        super().__init__("sequenced_motion")
        self._action_client = ActionClient(
            self, MoveGroupSequence, "/lbr/sequence_move_group"
        )
        while not self._action_client.wait_for_server(timeout_sec=1.0):
            self.get_logger().info(f"Waiting for {self._action_client._action_name}...")

        self._base = "link_0"
        self._end_effector = "link_ee"
        self._move_group_name = "arm"

    def _build_motion_plan_request(self, target_pose: Pose, velocity_scaling: float, acceleration_scaling: float) -> MotionPlanRequest:
        req = MotionPlanRequest()

        # general config
        req.pipeline_id = "pilz_industrial_motion_planner"
        req.planner_id = "PTP"  # For Pilz PTP, LIN of CIRC
        req.allowed_planning_time = 10.0
        req.group_name = self._move_group_name
        req.max_acceleration_scaling_factor = acceleration_scaling
        req.max_velocity_scaling_factor = velocity_scaling
        req.num_planning_attempts = 1000

        # goal constraints
        req.goal_constraints.append(
            Constraints(
                position_constraints=[
                    PositionConstraint(
                        header=Header(frame_id=self._base),
                        link_name=self._end_effector,
                        constraint_region=BoundingVolume(
                            primitives=[SolidPrimitive(type=2, dimensions=[0.0001])],
                            primitive_poses=[Pose(position=target_pose.position)],
                        ),
                        weight=1.0,
                    )
                ],
                orientation_constraints=[
                    OrientationConstraint(
                        header=Header(frame_id=self._base),
                        link_name=self._end_effector,
                        orientation=target_pose.orientation,
                        absolute_x_axis_tolerance=0.001,
                        absolute_y_axis_tolerance=0.001,
                        absolute_z_axis_tolerance=0.001,
                        weight=1.0,
                    )
                ],
            )
        )
        return req

    def execute_sequence(self, target_poses: List[Pose], velocity_scalings: List[float], acceleration_scalings: List[float]) -> None:
        goal = MoveGroupSequence.Goal()
        for i, target_pose in enumerate(target_poses):
            goal.request.items.append(
                MotionSequenceItem(
                    blend_radius=0.0,
                    req=self._build_motion_plan_request(target_pose, velocity_scalings[i], acceleration_scalings[i]),
                )
            )
        goal.request.items[-1].blend_radius = 0.0  # last radius must be 0
        future = self._action_client.send_goal_async(goal)
        rclpy.spin_until_future_complete(self, future)


def main() -> None:
    rclpy.init()
    sequenced_motion = SequencedMotion()
    target_poses = [

        Pose(position=Point(x=0.55, y=0.25, z=0.15), orientation=Quaternion(x=1.0, y=0.0, z=0.0, w=0.0)),
        Pose(position=Point(x=0.55, y=0.1, z=0.15), orientation=Quaternion(x=1.0, y=0.0, z=0.0, w=0.0)), 
        Pose(position=Point(x=0.55, y=0.0, z=0.15), orientation=Quaternion(x=1.0, y=0.0, z=0.0, w=0.0)), 
        Pose(position=Point(x=0.55, y=-0.1, z=0.15), orientation=Quaternion(x=1.0, y=0.0, z=0.0, w=0.0)), 
        Pose(position=Point(x=0.55, y=-0.25, z=0.15), orientation=Quaternion(x=1.0, y=0.0, z=0.0, w=0.0)),
        Pose(position=Point(x=0.65, y=-0.25, z=0.15), orientation=Quaternion(x=1.0, y=0.0, z=0.0, w=0.0)),        
        Pose(position=Point(x=0.65, y=-0.1, z=0.15), orientation=Quaternion(x=1.0, y=0.0, z=0.0, w=0.0)),
        Pose(position=Point(x=0.65, y=0.0, z=0.15), orientation=Quaternion(x=1.0, y=0.0, z=0.0, w=0.0)),
        Pose(position=Point(x=0.65, y=0.0, z=0.15), orientation=Quaternion(x=0.866, y=-0.5, z=0.0, w=0.0)),
        Pose(position=Point(x=0.8, y=0.07, z=0.15), orientation=Quaternion(x=0.866, y=-0.5, z=0.0, w=0.0)),        
        
    ]
    velocity_scalings = [0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.05, 0.1]  # Set the desired velocity scaling for each target pose
    acceleration_scalings = [0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.05, 0.1]  # Set the desired acceleration scaling for each target pose


    sequenced_motion.execute_sequence(target_poses=target_poses, velocity_scalings=velocity_scalings, acceleration_scalings=acceleration_scalings)
    rclpy.shutdown()


if __name__ == "__main__":
    main()

here is my code i use.
each pose has another max velocity acceleration.

[mhubii]

you can format code in github through

• 3 quotes at the start ```
• adding python after the first 3 quotes, that is ```python
• 3 quotes at the end ```

[mhubii]

I'll modify your code slightly and try to send it later

[SUUHOLEE]

thank you

[SUUHOLEE]

from typing import List

import rclpy
from moveit_msgs.action import MoveGroup
from moveit_msgs.msg import (
    Constraints,
    JointConstraint,
)
from rclpy.action import ActionClient
from rclpy.node import Node
import time


class MoveGroupActionClientNode(Node):
    def __init__(self, node_name: str) -> None:
        super().__init__(node_name)

        self.action_server = "/lbr/move_action"
        self.move_group_name = "arm"

        self.move_group_action_client = ActionClient(
            self, MoveGroup, self.action_server
        )

        self.get_logger().info(f"Waiting for action server {self.action_server}...")
        if not self.move_group_action_client.wait_for_server(timeout_sec=1):
            raise RuntimeError(
                f"Couldn't connect to action server {self.action_server}."
            )
        self.get_logger().info(f"Done.")

    def send_goal_async(self, joint_angles: List[float]):
        goal = MoveGroup.Goal()
        goal.request.allowed_planning_time = 1.0

        # Define joint constraints
        joint_constraints = [
            JointConstraint(
                joint_name=f"A{i+1}",
                position=joint_angles[i],
                tolerance_above=0.001,
                tolerance_below=0.001,
                weight=1.0,
            )
            for i in range(len(joint_angles))
        ]

        goal.request.goal_constraints.append(
            Constraints(joint_constraints=joint_constraints)
        )
        goal.request.group_name = self.move_group_name
        goal.request.max_acceleration_scaling_factor = 0.1
        goal.request.max_velocity_scaling_factor = 0.05
        goal.request.num_planning_attempts = 1

        return self.move_group_action_client.send_goal_async(goal)


def main(args: List = None) -> None:
    rclpy.init(args=args)
    move_group_action_client_node = MoveGroupActionClientNode(
        "move_group_action_client_node"
    )

    # List of joint angle sets to move the robot to
    joint_angle_sets = [
        [0.0, -0.5, 0.0, -1.5, 0.0, 1.0, 0.0],
        [0.1, -0.6, 0.1, -1.6, 0.1, 1.1, 0.1],
        [0.2, -0.7, 0.2, -1.7, 0.2, 1.2, 0.2],
        # Add more joint configurations as needed
    ]

    for joint_angles in joint_angle_sets:
        future = move_group_action_client_node.send_goal_async(joint_angles)
        rclpy.spin_until_future_complete(move_group_action_client_node, future)
        time.sleep(1)

    rclpy.shutdown()


if __name__ == "__main__":
    main()

This code is designed to move each joint to a target angle, rather than moving the end effector to a target position and orientation. It has been confirmed to work in simulation.

If we add code to solve inverse kinematics, wouldn't that allow us to perform redundancy control as well?

[SUUHOLEE]

import numpy as np
from scipy.linalg import expm, logm, pinv
import rclpy
from moveit_msgs.action import MoveGroup
from moveit_msgs.msg import Constraints, JointConstraint
from rclpy.action import ActionClient
from rclpy.node import Node
import time

class Tra:
    @staticmethod
    def skew(w):
        return np.array([
            [0, -w[2], w[1]],
            [w[2], 0, -w[0]],
            [-w[1], w[0], 0]
        ])

    @staticmethod
    def exp_coord(w, th):
        w_hat = Tra.skew(w)
        return np.eye(3) + np.sin(th) * w_hat + (1 - np.cos(th)) * np.dot(w_hat, w_hat)

    @staticmethod
    def screw_T(screw, th):
        w_hat = screw[:3]
        v = screw[3:]
        R = Tra.exp_coord(w_hat, th)
        G = np.eye(3) * th + Tra.skew(w_hat) * (1 - np.cos(th)) + np.dot(Tra.skew(w_hat), Tra.skew(w_hat)) * (th - np.sin(th))
        T = np.block([[R, np.dot(G, v).reshape(3, 1)], [0, 0, 0, 1]])
        return T

    @staticmethod
    def ad_T(T):
        R = T[:3, :3]
        p = T[:3, 3]
        return np.block([
            [R, np.zeros((3, 3))],
            [np.dot(Tra.skew(p), R), R]
        ])

class SerialRobot:
    def __init__(self, S, th, M, joint_limits):
        self.S = S
        self.th = th
        self.M = M
        self.T = np.eye(4)
        self.T_i = np.zeros((4, 4, len(th)))
        self.Js = np.zeros((6, len(th)))
        self.Jb = np.zeros((6, len(th)))
        self.joint_limits = joint_limits

    def forward_kinematics(self, th):
        T_ = np.eye(4)
        for i in range(len(th)):
            T_ = np.dot(T_, Tra.screw_T(self.S[:, i], th[i]))
            self.T_i[:, :, i] = T_
        self.T = np.dot(T_, self.M)

        for i in range(len(th)):
            if i == 0:
                self.Js[:, i] = self.S[:, i]
            else:
                self.Js[:, i] = np.dot(Tra.ad_T(self.T_i[:, :, i - 1]), self.S[:, i])

        self.Jb = np.dot(Tra.ad_T(np.linalg.inv(self.T)), self.Js)

    def inverse_kinematics(self, T_sd):
        T_bs = np.linalg.inv(self.T)
        T_bd = np.dot(T_bs, T_sd)
        vb_bracket = logm(T_bd)
        vb = np.array([-vb_bracket[2, 1], vb_bracket[0, 2], -vb_bracket[0, 1], vb_bracket[0, 3], vb_bracket[1, 3], vb_bracket[2, 3]])
        dth = np.dot(pinv(self.Jb), vb) * 0.3
        
        th_new = self.th + dth
        for i in range(len(th_new)):
            th_new[i] = max(self.joint_limits[i][0], min(self.joint_limits[i][1], th_new[i]))

        return th_new - self.th

class MoveGroupActionClientNode(Node):
    def __init__(self, node_name: str) -> None:
        super().__init__(node_name)

        self.action_server = "/lbr/move_action"
        self.move_group_name = "arm"

        self.move_group_action_client = ActionClient(
            self, MoveGroup, self.action_server
        )

        self.get_logger().info(f"Waiting for action server {self.action_server}...")
        if not self.move_group_action_client.wait_for_server(timeout_sec=1):
            raise RuntimeError(
                f"Couldn't connect to action server {self.action_server}."
            )
        self.get_logger().info(f"Done.")

    def send_goal_async(self, joint_angles: np.ndarray):
        goal = MoveGroup.Goal()
        goal.request.allowed_planning_time = 1.0

        joint_constraints = [
            JointConstraint(
                joint_name=f"A{i+1}",
                position=joint_angles[i],
                tolerance_above=0.001,
                tolerance_below=0.001,
                weight=1.0,
            )
            for i in range(len(joint_angles))
        ]

        goal.request.goal_constraints.append(
            Constraints(joint_constraints=joint_constraints)
        )
        goal.request.group_name = self.move_group_name
        goal.request.max_acceleration_scaling_factor = 0.1
        goal.request.max_velocity_scaling_factor = 0.05
        goal.request.num_planning_attempts = 1

        return self.move_group_action_client.send_goal_async(goal)

def main():
    rclpy.init()
    move_group_action_client_node = MoveGroupActionClientNode(
        "move_group_action_client_node"
    )

    W = np.array([
        [0.0, 0.0, 1.0],
        [0.0, 1.0, 0.0],
        [0.0, 0.0, 1.0],
        [0.0, -1.0, 0.0],
        [0.0, 0.0, 1.0],
        [0.0, 1.0, 0.0],
        [0.0, 0.0, 1.0],
    ]).T

    Q = np.array([
        [0.0, 0.0, 0.147],
        [0.0, -0.01, 0.3595],
        [0.0, 0.0, 0.5875],
        [0.0, 0.0105, 0.7795],
        [0.0, 0.0, 0.987],
        [0.0, -0.0707, 1.1795],
        [0.0, 0.0, 1.2705],
        [0.0, 0.0, 1.3055]
    ]).T

    S = np.zeros((6, 7))
    for i in range(7):
        S[:3, i] = W[:, i]
        S[3:, i] = -np.cross(W[:, i], Q[:, i])

    M = np.array([
        [1.0000, 0, 0, 0.0],
        [0, 1.0000, 0, 0.0],
        [0, 0, 1.0000, 1.3055],
        [0, 0, 0, 1.0000]
    ])

    joint_limits = [
        np.deg2rad([-170, 170]),
        np.deg2rad([-120, 120]),
        np.deg2rad([-170, 170]),
        np.deg2rad([-120, 120]),
        np.deg2rad([-170, 170]),
        np.deg2rad([-120, 120]),
        np.deg2rad([-175, 175]) 
    ]

    th = np.zeros(7)
    robot = SerialRobot(S, th, M, joint_limits)
    robot.T = np.eye(4)

    T_sd_list = [
        np.block([
            [np.eye(3), np.array([[0.5], [0], [1]])],
            [np.array([0, 0, 0, 1])]
        ]),
        np.block([
            [np.eye(3), np.array([[0.6], [0.2], [0.8]])],
            [np.array([0, 0, 0, 1])]
        ]),
        np.block([
            [np.eye(3), np.array([[0.3], [-0.3], [0.9]])],
            [np.array([0, 0, 0, 1])]
        ])
    ]

    for T_sd in T_sd_list:
        difference = np.linalg.norm(robot.T - T_sd)

        while difference > 1e-2:
            robot.forward_kinematics(th)
            dth = robot.inverse_kinematics(T_sd)
            th += dth
            difference = np.linalg.norm(robot.T - T_sd)
            print(f"Current difference: {difference}, th: {th}")

        print("Final joint angles for this target:", th)

        future = move_group_action_client_node.send_goal_async(th)
        rclpy.spin_until_future_complete(move_group_action_client_node, future)
        time.sleep(1)

    rclpy.shutdown()

if __name__ == "__main__":
    main()

from typing import List

import rclpy
from geometry_msgs.msg import Point, Pose, Quaternion
from moveit_msgs.action import MoveGroup
from moveit_msgs.msg import (
    BoundingVolume,
    Constraints,
    OrientationConstraint,
    PositionConstraint,
)
from rclpy.action import ActionClient
from rclpy.node import Node
from shape_msgs.msg import SolidPrimitive
from std_msgs.msg import Header
import numpy as np
from scipy.spatial.transform import Rotation
import time

class MoveGroupActionClientNode(Node):
    def __init__(self, node_name: str) -> None:
        super().__init__(node_name)

        self.action_server = "/lbr/move_action"
        self.move_group_name = "arm"
        self.base = "link_0"
        self.end_effector = "link_ee"

        self.move_group_action_client = ActionClient(
            self, MoveGroup, self.action_server
        )

        self.get_logger().info(f"Waiting for action server {self.action_server}...")
        if not self.move_group_action_client.wait_for_server(timeout_sec=1):
            raise RuntimeError(
                f"Couldn't connect to action server {self.action_server}."
            )
        self.get_logger().info(f"Done.")

    def send_goal_async(self, target: Pose):
        goal = MoveGroup.Goal()
        goal.request.allowed_planning_time = 1.0
        goal.request.goal_constraints.append(
            Constraints(
                position_constraints=[
                    PositionConstraint(
                        header=Header(frame_id=self.base),
                        link_name=self.end_effector,
                        constraint_region=BoundingVolume(
                            primitives=[SolidPrimitive(type=2, dimensions=[0.0001])],
                            primitive_poses=[Pose(position=target.position)],
                        ),
                        weight=1.0,
                    )
                ],
                orientation_constraints=[
                    OrientationConstraint(
                        header=Header(frame_id=self.base),
                        link_name=self.end_effector,
                        orientation=target.orientation,
                        absolute_x_axis_tolerance=0.0001,
                        absolute_y_axis_tolerance=0.0001,
                        absolute_z_axis_tolerance=0.0001,
                        weight=1.0,
                    )
                ],
            )
        )
        goal.request.group_name = self.move_group_name
        goal.request.max_acceleration_scaling_factor = 0.1
        goal.request.max_velocity_scaling_factor = 0.05
        goal.request.num_planning_attempts = 1
        
        return self.move_group_action_client.send_goal_async(goal)

def euler_to_quaternion(euler_angles):
    rotation = Rotation.from_euler('xyz', euler_angles)  # Adjust the order of rotations if needed
    quaternion = rotation.as_quat()
    return Quaternion(x=quaternion[0], y=quaternion[1], z=quaternion[2], w=quaternion[3])


def main(args: List = None) -> None:
    
    rclpy.init(args=args)
    move_group_action_client_node = MoveGroupActionClientNode(
        "move_group_action_client_node"
    )

    # List of poses to move the robot to
    poses = [
        
        Pose(
            position=Point(x=0.5, y=0.0, z=1.0),
            orientation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0),
        ),
        
        Pose(
            position=Point(x=0.6, y=0.2, z=0.8),
            orientation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0),
        ),
        
        Pose(
            position=Point(x=0.3, y=-0.3, z=0.9),
            orientation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0),
        )
        # Add more poses as needed
    ]

    for pose in poses:
        future = move_group_action_client_node.send_goal_async(pose)
        rclpy.spin_until_future_complete(move_group_action_client_node, future)
        time.sleep(100)

    rclpy.shutdown()

if __name__ == "__main__":
    main()

I have attached two codes.

The first code solves inverse kinematics to control the angles, and the second code is a modified version of the code from #152 (comment).

Both codes aim to achieve the same goal.

The second code operates the robot without any issues, but the first code frequently gets stuck in an infinite loop. (It is expected that modifying the Jacobian part of the inverse kinematics in the first code to perform redundancy control could allow it to run without problems.)

I’m wondering if there is a way to resolve this issue.

Thank you