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: https://github.com/lbr-stack/lbr_fri_ros2_stack/issues/152#issuecomment-1924424184

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

[SUUHOLEE]

i use this code ( maybe some modified )https://github.com/lbr-stack/lbr_fri_ros2_stack/issues/152#issuecomment-1924424184

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