How to create a customized joint？

[xuanyaoming]

How to create a customized joint？

[viblo]

I thought customized joints were an interesting thing to try, so I put together a quick example in this branch: https://github.com/viblo/pymunk/tree/custom-joints
Use the joint: https://github.com/viblo/pymunk/blob/custom-joints/dump/joint.py
You would need to override the 4 methods of the CustomJoint class I mentioned above to actually make it do something: https://github.com/viblo/pymunk/blob/custom-joints/pymunk/constraints.py#L884

The very very basics works, but I didnt have time to make a actual joint with it. Quickly looking at the existing joints at least I think a SimpleMotor joint would be possible to re-implement with this new CustomJoint. But overall, how useful it is im not sure of.. I have some memory that people tried to do this with the Chipmunk 2D library (which Pymunk is built on), but in the end I think it never turned out useful or was too much work to implement whatever effect they were after..

Given Im not convinced its something anyone will actually use to something useful I cant promise I will finalize and merge this into Pymunk master, but obviously the chance is higher if you (or someone else) find some use for it.

[viblo]

Just to double check, are you aware that you can have multiple joints between the same bodies? For example use both a SimpleMotor and a PinJoint to rotate a wheel but do not allow it to move , like its done in the video on pymunk.org homepage (code here: def car(space):)

[viblo]

FixedJoint is easy to answer, instead of a joint you can add several shapes to the same body. This is one of the reasons its possible to set an offset when you create a Circle shape, or set the transform when you create a Poly shape.

[viblo]

Im not sure exactly what you are looking for with moveable, but the things you can control are the properties on them. Maybe you could use max_bias, error_bias and max_force to restrict how quickly the joint controls its bodies?

As for creating your own joint its possible, but not very convenient right now (no one has asked about it before). A joint basically has 4 functions, preStep, applyCachedImpulse, applyImpulse and getImpulse. These would need implementation. But since they are not in the Pymunk API, nor even private Pymunk API you would need to wrap the underlying c code, and then implement it.

As a consequence the python joints would also be slower than their C counterparts, because of python and the cffi wrapping between c and python.. but maybe that would be ok for such specialized use cases?

[xuanyaoming]

Many thanks for your generous help! The example code is just what I need to continue. It looks like I need to write some C code if I want to create my ideal joints.

As to why I want customized joints, the major reason is that the preset joints are too free for me. For instance, SimpleMotor and GearJoint allows the two bodies to have any relative positions. This freedom is very problematic in my code. Another reason comes from this tutorial. The rag doll example is really interesting, but the doll can't even stand without external forces applied to its joints connecting its arms and legs. It's quite natural to me to want to directly control the joints because that's what's been done in ISAAC SIM, a 3D simulator.

My use case is indeed special. I'm trying to simulate robots in 2D simulator. Although the common practice is to simulate them in 3D simulators directly, 2D simulator such as pymunk is a lot simpler and faster. That's why I choose pymunk as the environment to test the correctness of my RL algorithms. But before that, I need a set of adjustable joints in order to build the action space.

[xuanyaoming]

Well, I didn't know pymunk supports multiple joints between bodies until now. Sorry for that. Thanks a lot for the tip.

This feature can solve most of my issues, but there are still some joints in my mind that can't be considered as combinations of the preset joints in pymunk. Namely GrooveJoint, SlideJoint and FixedJoint.

• GrooveJoint: I follow the defination of GrooveJoint in pymunk. But I need to directly control the pivot's movement in the linear slide. As the pivot isn't a body, I have no idea how to implement this functionality.
• SlideJoint: Like GrooveJoint, I followed your definition in pymunk. What I want to change is to control the relative speed, and the maximum force to enforce the speed, between the two anchor points. Additionally, to make my simulation look reasonable and realistic, I also plan to create segment bodies, in order to represent the linear slide and connections between bodies and the pivot, so that I can add collisions to the joints.
• FixedJoint: a joint that do nothing but to glue the adjacent bodies. It's useful to build weird-looking bodies. I haven't found a constraint example on how to assemble bodies and shapes for the moment. Is this function implemented elsewhere?

[xuanyaoming]

Thanks, it's a good solution but this method will break the premise that a constraint is a relationship between two bodies.

I just figure out an alternative. A fixedJoint could be a combination of RotaryLinitJoint (set the min and max angles to 0) and a PivotJoint (set the pivot coordinate for the second body as (0, 0)). It will also work but leave the premise intact.

[viblo]

Yeah, that could work. Note that something that might be important is stability. Two shapes to the same body is as stable as a single shape and the shapes will never move in relation to each other. However, constraints work by adjusting the relative velocity of the two bodies, and are by default less stable. You can see that in some of the pymunk examples if you look carefully.

…

On Wed, Sep 6, 2023, 12:31 xuanyaoming ***@***.***> wrote: Thanks, it's a good solution but this method will break the premise that a constraint is a relationship between two bodies. I just figure out an alternative. A fixedJoint could be a combination of *RotaryLinitJoint* (set the min and max angles to 0) and a *PivotJoint* (set the pivot coordinate for the second body as (0, 0)). It will also work but leave the premise intact. — Reply to this email directly, view it on GitHub <#232 (comment)>, or unsubscribe <https://github.com/notifications/unsubscribe-auth/AAJD2CA3QNB5VTGKGBLI3K3XZBGJ5ANCNFSM6AAAAAA4J6QEPU> . You are receiving this because you commented.Message ID: ***@***.***>

[xuanyaoming]

The instability comes from bias. I found setting error_bias to 0 and max_force to math.inf can generate stable enough results in my tests, although this setting will break the collision system sometimes. I guess I need to look into the collision mechanism next, but that's another topic.

As to the topic of joints, I think what are missing is a velocity constraint and a position constraint. Most constraints are designed for angular properties, such as limiting the relative angles and keeping the relative angular speed fixed to a number. But no preset joints in pymunk provides tools for limiting the relative position or maintaining a relative speed between two bodies, to the best of my knowledge. I don't think PinJoint and PivotJoint are enough to do the job.

Anyway, I guess my best option is to dive into C code and customize the engine to my needs. Thanks for your help, guidance and patience nevertheless.

[terrilHu]

Hi. I browsed the page because I have similar issues with customized joints. I find Xuan's problem surprisingly similar to mine: My real-world model has two arms moving in reverse directions at the same speed, which are driven by a rotating gear in the center, but such mechanical interactions seem hard to be achieved using Pymunk. Thus I also need customized joints to control the relative speed between two robot arms.

The methods I've tried include applying forces or impulses manually according to arm locations. It proved effective in simple cases but errors emerge when more running individuals are connected. I guessed that's because a fixed function of setting the driving forces or impulses only applies for a certain situation. For example, if a robot is connected to other two while running, the force needed to drive it correctly must be different from when it runs alone.

If customized joints that control the relative velocity can be achieved(like how Simplemotor controls the relative angular velocity), I guess it would provide a good solution. I don't know if it's possible to write one based on Simplemotor but that seems worth trying.

[viblo]

Im not sure I understand your simulation. Could you provide one or a couple of pictures to illustrate?

Anyway, on top of my head I cant see a reason why there couldn't be a SimpleLinearMotor (or something).. I dont think it would be too difficult to write such a constraint.

[terrilHu]

My simulation is basically two sticks shuffling with each other back and forth at a certain relative velocity.

I have tried two ways to simulate such movement:
1. applying forces based on their relative speed--pushing them apart if they move too slowly and vice versa.
2. applying impulses based on their relative locations (just like constraints do).

Both methods work correctly on a single robot, but if more sticks are connected with each other there would be problems. A single robot cannot expect how much load it would take and only has a fixed output force limit. It's hard to ensure a certain setting of such force limit works in multiple situations. If the force or impulse is too small compared to the mass it drives (for example, when a robot runs with several connected to it), it would move slower than expected; otherwise(for example, when a robot runs alone) it may not move continually.

Oh as I write I realize this is not the problem of constraints... I guess I actually need to change the way a robot decides its force output so that it resembles the real life where such forces are not fixed. I gonna try this first. Thanks for your reply nevertheless.

[Menginventor]

Hi, I'm working on 2D robot sim too, mostly focusing on walking robot.
My framework is optimizing joint trajectory to make robot move faster or more efficient.
I came up with 2 approaches.

1. Position servo joint.

This approach based on simple motor by limiting max_force then control the rate using proportional control for example motor.rate = K*(target_angle - joint angle) we can also clamped the rate for realistic result

2. Damped Rotary spring as motor

If the orecision is not critial, spring-damper is equivalent to PD control so you can set the stiffness and damp to reasonable value and then set rest_angle as desired angle, However max_force is not implement in the damped Rotary spring but you can work around as mention in https://github.com/viblo/pymunk/issues/241. However we can't control the max_rate directly is result in more extream locomotion like jumping.

For now I successfully implement both method. this is show case for Position servo joint.
https://youtu.be/1qMH0vyjoEc?si=tUh4lGNHonP2PDe4

After that I just want to know how robot learn to walk with just one leg and this result implement with damped rotary spring method
https://www.youtube.com/watch?v=Jp2aWZFTXqs

I working on making github repo on this. If you interested you can join this project.

This is example code for position servo.
import sys
import numpy as np
import pygame
from pygame.locals import USEREVENT, QUIT, KEYDOWN, KEYUP, K_s, K_r, K_q, K_ESCAPE, K_UP, K_DOWN, K_RIGHT, K_LEFT, K_x
from pygame.color import THECOLORS

import pymunk
from pymunk import Vec2d
import pymunk.pygame_util

class ServoMotor(pymunk.SimpleMotor):
    def __init__(self, body_a, body_b, max_force, max_rate, p_gain):
        super().__init__(body_a, body_b, 0)  # Start with rate = 0
        self.body_a = body_a
        self.body_b = body_b
        self.max_force = max_force
        self.max_rate = max_rate
        self.p_gain = p_gain
        self.target_angle = 0


    def angle(self):
        """
        Get the current angle of the motor.
        """
        return self.body_a.angle - self.body_b.angle

    def set_target_angle(self, target_angle):
        """
        Set the target angle for the motor to rotate towards.
        """
        self.target_angle = target_angle
    def update(self):
        rate = (self.target_angle - self.angle()) * self.p_gain
        #to clamping the turning rate
        self.rate = max(min(rate, self.max_rate), -self.max_rate)
class Simulator(object):

    def __init__(self):
        self.display_flags = 0

        self.display_size = (1000, 800)

        self.space = pymunk.Space()
        self.space.gravity = (0.0, -1900.0)
        # self.space.damping = 0.999 # to prevent it from blowing up.

        # Pymunk physics coordinates start from the lower right-hand corner of the screen.
        self.ground_y = 100
        ground = pymunk.Segment(self.space.static_body, (0, self.ground_y), (1000, self.ground_y), 5.0)
        ground.friction = 1.0
        self.space.add(ground)

        self.screen = None

        self.draw_options = None

    def reset_bodies(self,chassisXY):
        legWd_a = 75
        legWd_b = 75
        chWd = 70


        self.chassis_b.position = chassisXY
        self.leftLeg_1a_body.position = chassisXY - ((chWd / 2) + (legWd_a / 2), 0)
        self.leftLeg_1a_body.angle = 0
        self.leftLeg_1b_body.position = self.leftLeg_1a_body.position - ((legWd_a / 2) + (legWd_b / 2), 0)
        self.leftLeg_1b_body.angle = 0
        self.rightLeg_1a_body.position = chassisXY + ((chWd / 2) + (legWd_a / 2), 0)
        self.rightLeg_1a_body.angle = 0
        self.rightLeg_1b_body.position = self.rightLeg_1a_body.position + ((legWd_a / 2) + (legWd_b / 2), 0)
        self.rightLeg_1b_body.angle = 0

    def draw(self):
        self.screen.fill(THECOLORS["white"])  ### Clear the screen
        self.space.debug_draw(self.draw_options)  ### Draw space
        pygame.display.flip()  ### All done, lets flip the display


    def main(self):
        pygame.init()
        self.screen = pygame.display.set_mode(self.display_size, self.display_flags)
        width, height = self.screen.get_size()
        self.draw_options = pymunk.pygame_util.DrawOptions(self.screen)
        pymunk.pygame_util.positive_y_is_up = True

        clock = pygame.time.Clock()
        running = True
        font = pygame.font.Font(None, 16)

        # Create the spider
        chassisXY = Vec2d(self.display_size[0] / 2, self.ground_y + 100)
        chWd = 70
        chHt = 50
        chassisMass = 10

        legWd_a = 75
        legHt_a = 5
        legWd_b = 75
        legHt_b = 5
        legMass = 1

        motor_max_force = np.inf
        motor_max_rate = 3
        motor_p_gain = 50
        # ---chassis
        self.chassis_b = pymunk.Body(chassisMass, pymunk.moment_for_box(chassisMass, (chWd, chHt)))
        self.chassis_b.position = chassisXY
        chassis_shape = pymunk.Poly.create_box(self.chassis_b, (chWd, chHt))
        chassis_shape.color = 200, 200, 200, 100
        chassis_shape.friction = 0.5
        print("chassis position")
        print(self.chassis_b.position)

        # ---first left leg a
        self.leftLeg_1a_body = pymunk.Body(legMass, pymunk.moment_for_box(legMass, (legWd_a, legHt_a)))
        self.leftLeg_1a_body.position = chassisXY - ((chWd / 2) + (legWd_a / 2), 0)

        leftLeg_1a_shape = pymunk.Poly.create_box(self.leftLeg_1a_body, (legWd_a, legHt_a))
        leftLeg_1a_shape.color = 255, 0, 0, 100
        leftLeg_1a_shape.friction = 1

        # ---first left leg b
        self.leftLeg_1b_body = pymunk.Body(legMass, pymunk.moment_for_box(legMass, (legWd_b, legHt_b)))
        self.leftLeg_1b_body.position = self.leftLeg_1a_body.position - ((legWd_a / 2) + (legWd_b / 2), 0)
        leftLeg_1b_shape = pymunk.Poly.create_box(self.leftLeg_1b_body, (legWd_b, legHt_b))
        leftLeg_1b_shape.color = 0, 255, 0, 100
        leftLeg_1b_shape.friction = 1
        # ---first right leg a
        self.rightLeg_1a_body = pymunk.Body(legMass, pymunk.moment_for_box(legMass, (legWd_a, legHt_a)))
        self.rightLeg_1a_body.position = chassisXY + ((chWd / 2) + (legWd_a / 2), 0)
        rightLeg_1a_shape = pymunk.Poly.create_box(self.rightLeg_1a_body, (legWd_a, legHt_a))
        rightLeg_1a_shape.color = 255, 0, 0, 100
        rightLeg_1a_shape.friction = 1
        # ---first right leg b
        self.rightLeg_1b_body = pymunk.Body(legMass, pymunk.moment_for_box(legMass, (legWd_b, legHt_b)))
        self.rightLeg_1b_body.position = self.rightLeg_1a_body.position + ((legWd_a / 2) + (legWd_b / 2), 0)
        rightLeg_1b_shape = pymunk.Poly.create_box(self.rightLeg_1b_body, (legWd_b, legHt_b))
        rightLeg_1b_shape.color = 0, 255, 0, 100
        rightLeg_1b_shape.friction = 1
        # ---link left leg b with left leg a
        pj_ba1left = pymunk.PinJoint(self.leftLeg_1b_body, self.leftLeg_1a_body, (legWd_b / 2, 0),
                                     (-legWd_a / 2, 0))  # anchor point coordinates are wrt the body; not the space
        motor_ba1Left = ServoMotor(self.leftLeg_1b_body, self.leftLeg_1a_body, motor_max_force,motor_max_rate,motor_p_gain)
        # ---link left leg a with chassis
        pj_ac1left = pymunk.PinJoint(self.leftLeg_1a_body, self.chassis_b, (legWd_a / 2, 0), (-chWd / 2, 0))
        motor_ac1Left = ServoMotor(self.leftLeg_1a_body, self.chassis_b, motor_max_force,motor_max_rate,motor_p_gain)
        # ---link right leg b with right leg a
        pj_ba1Right = pymunk.PinJoint(self.rightLeg_1b_body, self.rightLeg_1a_body, (-legWd_b / 2, 0),
                                      (legWd_a / 2, 0))  # anchor point coordinates are wrt the body; not the space
        motor_ba1Right = ServoMotor(self.rightLeg_1b_body, self.rightLeg_1a_body, motor_max_force,motor_max_rate,motor_p_gain)
        # ---link right leg a with chassis
        pj_ac1Right = pymunk.PinJoint(self.rightLeg_1a_body, self.chassis_b, (-legWd_a / 2, 0), (chWd / 2, 0))

        motor_ac1Right = ServoMotor(self.rightLeg_1a_body, self.chassis_b, motor_max_force,motor_max_rate,motor_p_gain)
        self.space.add(self.chassis_b, chassis_shape)
        self.space.add(self.leftLeg_1a_body, leftLeg_1a_shape, self.rightLeg_1a_body, rightLeg_1a_shape)
        self.space.add(self.leftLeg_1b_body, leftLeg_1b_shape, self.rightLeg_1b_body, rightLeg_1b_shape)
        self.space.add(pj_ba1left, motor_ba1Left, pj_ac1left, motor_ac1Left)
        self.space.add(pj_ba1Right, motor_ba1Right, pj_ac1Right, motor_ac1Right)

        motor_list = [motor_ba1Left,motor_ac1Left,motor_ba1Right,motor_ac1Right]
        # ---prevent collisions with ShapeFilter
        shape_filter = pymunk.ShapeFilter(group=1)
        chassis_shape.filter = shape_filter
        leftLeg_1a_shape.filter = shape_filter
        rightLeg_1a_shape.filter = shape_filter
        leftLeg_1b_shape.filter = shape_filter
        rightLeg_1b_shape.filter = shape_filter

        simulate = True
        rotationRate = 2
        while running:
            for event in pygame.event.get():
                if event.type == QUIT or (event.type == KEYDOWN and event.key in (K_q, K_ESCAPE)):
                    # running = False
                    sys.exit(0)
                elif event.type == KEYDOWN and event.key == K_s:
                    # Start/stop simulation.
                    simulate = not simulate
                elif event.type == KEYDOWN and event.key == K_r:
                    # Reset.
                    # simulate = False
                    print('reset')
                    self.chassis_b.position = chassisXY
                    self.reset_bodies(chassisXY)


                elif event.type == KEYDOWN and event.key == K_RIGHT:
                    motor_ba1Left.set_target_angle(np.deg2rad(170))
                    motor_ba1Right.set_target_angle(np.deg2rad(0))
                elif event.type == KEYDOWN and event.key == K_LEFT:
                    motor_ba1Left.set_target_angle(np.deg2rad(0))
                    motor_ba1Right.set_target_angle(np.deg2rad(-170))
                elif event.type == KEYDOWN and event.key == K_DOWN:
                    motor_ac1Left.set_target_angle(np.deg2rad(10))
                    motor_ac1Right.set_target_angle(np.deg2rad(-10))
                elif event.type == KEYDOWN and event.key == K_UP:
                    motor_ac1Left.set_target_angle(np.deg2rad(-60))
                    motor_ac1Right.set_target_angle(np.deg2rad(60))
                elif event.type == KEYUP:
                    motor_ba1Left.rate = 0
                    motor_ac1Left.rate = 0

            for motor in motor_list:
                motor.update()

            #print(np.rad2deg(self.leftLeg_1a_body.angle-self.chassis_b.angle))
            self.draw()

            ### Update physics
            fps = 50
            iterations = 25
            dt = 1.0 / float(fps) / float(iterations)
            if simulate:
                for x in range(iterations):  # 10 iterations to get a more stable simulation
                    self.space.step(dt)

            pygame.display.flip()
            clock.tick(fps)


if __name__ == '__main__':
    sim = Simulator()
    sim.main()