# Compas\_fab motion planning and constrains

**URL:** <https://forum.compas-framework.org/t/compas-fab-motion-planning-and-constrains/712>\
**Category:** Uncategorized\
**Created:** [May 12, 2023, 4:51pm UTC](https://forum.compas-framework.org/t/compas-fab-motion-planning-and-constrains/712 "2023-05-12T16:51:51Z")\
**Posts on this page:** 3\
**Page:** 1

<div class="post-metadata">

**Author:** ![petrasvestartas](https://sea1.discourse-cdn.com/flex015/user_avatar/forum.compas-framework.org/petrasvestartas/32/305_2.png) [@petrasvestartas](https://forum.compas-framework.org/u/petrasvestartas)\
**Post date:** [May 12, 2023, 4:51pm UTC](https://forum.compas-framework.org/t/compas-fab-motion-planning-and-constrains/712/1 "2023-05-12T16:51:51Z")

</div>

Hi,

I would like to ask about motion planning and constraints.

In the following code, does the name of a joint must correspond to names seen in a robot configuration? The reason to ask is I do not know when and how they are defined, do they correspond to urdf link naming? Once I append them to the list I do not know if they influence the planning, for example when the name of a joints would be wrongly written.

```auto
            pc_5_ = PositionConstraint.from_box('rob1_joint_4', box)
            pc_6_ = PositionConstraint.from_box('rob1_joint_5', box)
            jc_3_ = JointConstraint('rob1_joint_3', 10, 70, -10, 1.0)
            jc_2_ = JointConstraint('rob1_joint_2', 10, 20, -15, 1.0)

```

Another question not related to compas\_fab, is there any manual how to disable SafeMove?

Larger code snippet:

```auto

##################################################################################################
    # Move robot to place position
    ##################################################################################################

    x = 4000
    y = 1650 # MIN 1100 MAX 2200
    z = -630
    x_axis = Vector(0.707107,0.707107,0)
    y_axis = Vector(0.707107,-0.707107,0)

    frame_place_0 = Frame(Point(x, y, z_safety),x_axis, y_axis)
    frame_place_1 = Frame(Point(x, y, z+500), x_axis, y_axis) # upper position before release
    frame_place_2 = Frame(Point(x, y, z), x_axis, y_axis) # target position

    frames = [frame_place_0, frame_place_1, frame_place_2]

      with RosClient("localhost") as client:
         
            robot = client.load_robot()
            group = robot.main_group_name
            frame_place_0_fab = Frame(Point(frame_place_0.point[0]/1000.0, frame_place_0.point[1]/1000.0, 0.564985+frame_place_0.point[2]/1000.0),x_axis, y_axis)
            #frame_pick_0 = Frame(Point(7000/1000.0, 1500/1000.0, 0.564985+z_safety/1000.0),Vector(-0.260359,0.965511,-0.001113), Vector(0.96545,0.260355,0.011044)) #0.564985+
            robot_joints, external_axes = abb.send_and_wait(rrc.GetJoints())
            angles = to_radians([robot_joints.rax_1,robot_joints.rax_2,robot_joints.rax_3,robot_joints.rax_4,robot_joints.rax_5,robot_joints.rax_6])
            start_configuration.joint_values = ( clamp(external_axes[0]/1000-0,5.0,6.7), angles[0],angles[1],angles[2],angles[3],angles[4],angles[5] )
            print(start_configuration.joint_values)

            # create goal constraints from frame
            constraints = []
            pc_5_ = PositionConstraint.from_box('rob1_joint_4', box)
            pc_6_ = PositionConstraint.from_box('rob1_joint_5', box)
            jc_3_ = JointConstraint('rob1_joint_3', 10, 70, -10, 1.0)
            jc_2_ = JointConstraint('rob1_joint_2', 10, 20, -15, 1.0)

            constraints.append(pc_5_)
            constraints.append(pc_6_)
            constraints.append(jc_3_)
            constraints.append(jc_2_)
            constraints = constraints + robot.constraints_from_frame(frame_place_0_fab, tolerance_position, tolerance_axes, group, True)

            trajectory = robot.plan_motion(constraints, start_configuration, group, options=dict(planner_id="RRT"))
            

            for i in range(len(trajectory.points)):
               c = trajectory.points[i]
      
               track_position = c.joint_values[0]*1000
               joints = to_degrees(c.joint_values)
               joints.pop(0)
               if i < len(trajectory.points) - 1:
                     zone = rrc.Zone.Z10
               else:
                     zone = rrc.Zone.FINE
               print(c.joint_values)
               
               abb.send(rrc.MoveToJoints(joints, ext_axes=[track_position], speed=speed, zone=zone))
         

    abb.send_and_wait(rrc.MoveToFrame(frame_place_1, speed, rrc.Zone.FINE, rrc.Motion.LINEAR)) # place frame 
    abb.send_and_wait(rrc.MoveToFrame(frame_place_2, 50, rrc.Zone.FINE, rrc.Motion.LINEAR)) # place frame 
    abb.send_and_wait(rrc.SetDigital('Local_IO_0_DO1', 0), timeout=5) # open gripper
    abb.send_and_wait(rrc.WaitTime(1))
    abb.send_and_wait(rrc.MoveToFrame(frame_place_0, speed, rrc.Zone.FINE, rrc.Motion.LINEAR)) # place frame

```

---

<div class="post-metadata">

**Author:** ![Zac\_Zhuo\_Zhang](https://sea1.discourse-cdn.com/flex015/user_avatar/forum.compas-framework.org/zac_zhuo_zhang/32/433_2.png) [@Zac\_Zhuo\_Zhang](https://forum.compas-framework.org/u/Zac_Zhuo_Zhang)\
**Post date:** [May 14, 2023, 11:41am UTC](https://forum.compas-framework.org/t/compas-fab-motion-planning-and-constrains/712/2 "2023-05-14T11:41:51Z")

</div>

Hi Petras,

I was also looking at `compas_fab.robots.Constraint` so I may provide some personal views that might be correct and helpful.

The `link_name` and the `joint_name` which you need to put in `PositionConstraint` and `JointConstraint` should be one of the joint/link names in the `robot` that you defined in your previous code. You can use `robot.get_link_names()` and `robot.get_configurable_joint_names ()` to retrieve the names if you don’t know them. Normally they are defined when creating the `robot` instance. You can change the names _(like into `rob1_joint_4`)_ or add joints/links for your robot if you want.

However, if you input an unfamiliar name that the program doesn’t know which joint/link you refer to, an error can happen.

---

<div class="post-metadata">

**Author:** ![petrasvestartas](https://sea1.discourse-cdn.com/flex015/user_avatar/forum.compas-framework.org/petrasvestartas/32/305_2.png) [@petrasvestartas](https://forum.compas-framework.org/u/petrasvestartas)\
**Post date:** [May 14, 2023, 3:44pm UTC](https://forum.compas-framework.org/t/compas-fab-motion-planning-and-constrains/712/3 "2023-05-14T15:44:34Z")

</div>

Thank you:)  
This resolvew my question.