How to use bullet3 to create a Parallel structure constraints

[Heyuchen-330]

Hello everyone, my purpose is to create a structure like the picture below from urdf.

But, limited by the urdf, I can only output a structure like the picture below, and try to add the last joint in bullet.

I import the urdf model as btMultiBody in bullet and try to use the function cache.m_bulletMultiBody->setupRevolute to create the last joint. But it seems that this will open another joint and can't close the loop.
So is there a way to solve this problem? Thanks!

[rohit-kumar-j]

Use p.CreateConstraint() method
mentioned in: https://docs.google.com/document/d/10sXEhzFRSnvFcl3XxNGhnD4N2SedqwdAvK3dsihxVUA/edit#

Example: https://www.youtube.com/watch?v=eYOt6SEKHFs

Here is an example that I created for a complex joint: https://youtu.be/0hfkm7vv5o8

However, you'll need to call it in the main script. I've created a class that calls from a .json file to read the info and create all constraints.

import pybullet as p
import pybullet_data
import time
import math
from tabulate import tabulate
import json

class RobotBuilder(object):
	'''
	Builds the robot 
	'''
	def __init__(self, pybullet_client, robot_id):
		self._pybullet_client = pybullet_client
		self._robot_id = robot_id

		self._elbow_L_id = self.create_constraint(constraint_name = "elbow_pitch_L")
		self._hip_L_id = self.create_constraint(constraint_name = "hip_pitch_L")
		self._knee_L_id = self.create_constraint(constraint_name = "knee_pitch_L")
		self._ankle_cyl_L1_id = self.create_constraint(constraint_name = "ankle_cyl_L1")
		self._ankle_cyl_L2_id = self.create_constraint(constraint_name = "ankle_cyl_L2")
		self._ankle_tf_L1_id = self.create_constraint(constraint_name = "ankle_tf_L1")
		self._ankle_tf_L2_id = self.create_constraint(constraint_name = "ankle_tf_L2")

		self._elbow_R_id = self.create_constraint(constraint_name = "elbow_pitch_R")
		self._hip_R_id = self.create_constraint(constraint_name = "hip_pitch_R")
		self._knee_R_id = self.create_constraint(constraint_name = "knee_pitch_R")
		self._ankle_cyl_R1_id = self.create_constraint(constraint_name = "ankle_cyl_R1")
		self._ankle_cyl_R2_id = self.create_constraint(constraint_name = "ankle_cyl_R2")
		self._ankle_tf_R1_id = self.create_constraint(constraint_name = "ankle_tf_R1")
		self._ankle_tf_L2_id = self.create_constraint(constraint_name = "ankle_tf_R2")

	def create_constraint(self,constraint_name="No Constraint Specified!"):
		'''
		Creats constraint from the data from JSON file
		'''
		data = json.load(open('constraints.json'))
		parent_link_idx = data[constraint_name][0]
		child_link_idx = data[constraint_name][1]
		joint_axis = data[constraint_name][2]
		parent_offset = data[constraint_name][3]
		child_offset = data[constraint_name][4]
		parent_frame = self.vector_wrt_com(offset = parent_offset, link_idx = parent_link_idx,lifeTime=1.0)
		child_frame = self.vector_wrt_com(offset = child_offset, link_idx = child_link_idx,lifeTime=1.0)
		self._pybullet_client.createConstraint(parentBodyUniqueId = self._robot_id,
												parentLinkIndex = parent_link_idx,
												childBodyUniqueId = self._robot_id,
												childLinkIndex = child_link_idx,
												jointType = self._pybullet_client.JOINT_POINT2POINT,
												jointAxis = joint_axis,
												parentFramePosition = parent_frame,
												childFramePosition = child_frame)

	def vector_wrt_com(self,offset,link_idx,point_text="Point",
							vector_color=[1.0,1.0,1.0],lifeTime=0.0,text=False):
		'''
		Returns and Draws a position vector pointing to the corresponding point wrt to link origin
		'''
		parent = self._pybullet_client.getLinkState(self._robot_id,link_idx)[4]
		orn = self._pybullet_client.getLinkState(self._robot_id,link_idx)[5]
		# print("p_orn = ",p_orn)
		p_frame = [offset[0]+parent[0],offset[1]+parent[1],offset[2]+parent[2]]
		link_pos_wrt_wrld = self._pybullet_client.getLinkState(self._robot_id,link_idx)[4]
		com = self._pybullet_client.getLinkState(self._robot_id,link_idx)[0]
		link_orn = self._pybullet_client.getLinkState(self._robot_id,link_idx)[1] # 1 or 5
		reorder_orn = [link_orn[3],link_orn[0],link_orn[1],link_orn[2]]
		vector = [p_frame[0]-link_pos_wrt_wrld[0],
					p_frame[1]-link_pos_wrt_wrld[1],
					p_frame[2]-link_pos_wrt_wrld[2]]
		rotated_vector = self.quat_vec_mul(reorder_orn, vector)

		point_vector = [link_pos_wrt_wrld[0]+rotated_vector[0],
								link_pos_wrt_wrld[1]+rotated_vector[1],
								link_pos_wrt_wrld[2]+rotated_vector[2]]

		final_relative_vector = [link_pos_wrt_wrld[0]+rotated_vector[0]-com[0],
								link_pos_wrt_wrld[1]+rotated_vector[1]-com[1],
								link_pos_wrt_wrld[2]+rotated_vector[2]-com[2]]

		self._pybullet_client.addUserDebugLine(point_vector, 
												com,
												lineColorRGB=vector_color,
												lineWidth=3,
												lifeTime=lifeTime)
		self._pybullet_client.addUserDebugText(text=str(point_text),
													 textPosition=point_vector,
													 textColorRGB=[0.0, 0.0, 0.0],
													 textSize=1.0,
													 lifeTime=lifeTime)
		if text == True: my_string = str(self._pybullet_client.getJointInfo(self._robot_id,link_idx)[12])+"..."
		if text == True: self._pybullet_client.addUserDebugText(text=str(my_string),
													 textPosition=link_pos_wrt_wrld,
													 textColorRGB=[0.0, 0.0, 0.0],
													 textSize=1.0,
													 lifeTime=lifeTime)

		return final_relative_vector
		

	def quat_vec_mul(self,q1, v1):
		q2 = [0.0,] + v1
		return self.quat_mul(self.quat_mul(q1, q2), self.q_conjugate(q1))[1:]

	def quat_mul(self,q1, q2):
		w = q1[0] * q2[0] - q1[1] * q2[1] - q1[2] * q2[2] - q1[3] * q2[3]
		x = q1[0] * q2[1] + q1[1] * q2[0] + q1[2] * q2[3] - q1[3] * q2[2]
		y = q1[0] * q2[2] + q1[2] * q2[0] + q1[3] * q2[1] - q1[1] * q2[3]
		z = q1[0] * q2[3] + q1[3] * q2[0] + q1[1] * q2[2] - q1[2] * q2[1]
		return [w, x, y, z]
	
	def q_conjugate(self,q):
		return (q[0], -q[1], -q[2], -q[3])

Regards,
Rohit

[Heyuchen-330]

thanks for your reply, but is there a way to make it using the interface in Cpp but not pybullet?

[chinahuangyong]

@rohit-kumar-j how to get the JSON file ?

[rohit-kumar-j]

@Heyuchen-330 , you mean in C++? Probably translate it into C++ and do it as a configuration file.

[rohit-kumar-j]

@chinahuangyong you need to create the JSON file yourself from your cad program.
Since the JSON file is custom to my robot, I have not provided the JSON file. In fact, much of the code above references joint data from my urdf. It took me a while back then to understand how quaternions and relative vectors work within pybullet and hence create this file.