koobas.hobune.stream
General construction of kinematic trees with scaled capsules
I'm trying to generally create a kinematic tree out of capsules. I know that capsules are effectively 2x1x1 objects, the 2 being length, which makes the bookkeeping here a little annoying. For each capsule, I want to be able to specify:
A length - this is the length oft he capsule. We'll assume the other dimensions are unchanged.
A position and orientation in which the capsule is connected to a specified...
Parent capsule. That is, unless it's the root of the tree.
The relative location of the joint in the parent and child frames.
In essence, I'm trying to make something that can eventually generate characters from urdfs.
I'm trying to link these all up with joints, so I can actuate this character and do some physical experiments with it. While I find this works okay when I don't scale my capsules, things start to break once I do.
To get started, I've tried specifying a character semi-algorithmically with the above-specified inputs. Right now, there's just a head, a torso, and a forearm.
The torso code extrapolates fine beyond unit length. I can rescale it to whatever I want and it's just fine. But the arm has a couple of problems, which may or not be related.
First, the joint is slightly off at unit length, to the left.
Unit length. The axis of rotation is a little bit to the left.
Meanwhile, while the length and positioning looks fine when I make it longer, the joint is all the way to the right:
I'm pasting my code below. Is anyone able to explain to me what I'm doing wrong, and how I can algorithmically create a kinematic tree of scaled capsules? Beyond the specification of the positions and orientations of each capsule and their joints, I feel like this should be very short and simple script.
using System.Collections;
using System.Collections.Generic;
using UnityEngine;
public class Runner : MonoBehaviour {
//creates a super simple rigid biped as 10 limbs
//head
//torso
//two three linked legs
//two single linked arms
// Use this for initialization
List<float> transforms;
List<GameObject> links;
void Start () {
Vector3 global_position = new Vector3(0.0f, 8.0f, 0.0f);
Dictionary<GameObject, float> lengths = new Dictionary<GameObject, float>();
Dictionary<GameObject, Vector3> positions = new Dictionary<GameObject, Vector3>();
Dictionary<GameObject, Quaternion> rotations = new Dictionary<GameObject, Quaternion>();
Dictionary<GameObject, Vector3> parentJointPosition = new Dictionary<GameObject, Vector3>();
Dictionary<GameObject, Vector3> axis = new Dictionary<GameObject, Vector3>();
Dictionary<GameObject, GameObject> parents = new Dictionary<GameObject, GameObject>();
GameObject capsule = GameObject.CreatePrimitive(PrimitiveType.Capsule);
//turn on physics
capsule.AddComponent<Rigidbody>();
capsule.GetComponent<Rigidbody>().SetDensity(1000f);
//instantiate the biped:
GameObject head = Instantiate(capsule);
GameObject torso = Instantiate(capsule);
GameObject left_arm = Instantiate(capsule);
lengths[head] = 1.0f;
positions[head] = global_position;
rotations[head] = Quaternion.Euler(new Vector3(0.0f, 0.0f, 0.0f));
parents[head] = null;
parents[torso] = head;
lengths[torso] = 3.0f;
//TODO: generalize this code
positions[torso] = positions[parents[torso]] + Vector3.down * (lengths[parents[torso]] + lengths[torso]); // a /2.0f factors out since capsules are naturally 2x1x1, so the length is actually twice its value
rotations[torso] = Quaternion.Euler(new Vector3(0.0f, 0.0f, 0.0f));
parentJointPosition[torso] = positions[torso] + Vector3.up * lengths[torso];// a /2.0f factors out since capsules are naturally 2x1x1, so the length is actually twice its value
axis[torso] = Vector3.back;
parents[left_arm] = torso;
lengths[left_arm] = 3.0f;
//TODO: generalize this code
//want it to be at top of the torso, to the left so it's not intersecting
positions[left_arm] = positions[parents[left_arm]] + Vector3.up * (lengths[parents[left_arm]]) + Vector3.left * lengths[left_arm];
rotations[left_arm] = Quaternion.Euler(new Vector3(0.0f, 0.0f, -90.0f));
parentJointPosition[left_arm] = positions[left_arm] + Vector3.up * lengths[left_arm];
axis[left_arm] = Vector3.left;
head.name = "head";
torso.name = "torso";
left_arm.name = "left_arm";
Destroy(capsule);
links = new List<GameObject> { head, torso, left_arm };
foreach (GameObject link in links)
{
link.transform.localPosition = positions[link];
link.GetComponent<Rigidbody>().velocity = Vector3.zero;
link.transform.localRotation = rotations[link];
//hinge it up if necessary
if (parents[link] != null) {
HingeJoint hinge = link.AddComponent<HingeJoint>();
hinge.connectedBody = parents[link].GetComponent<Rigidbody>();
hinge.anchor = (parentJointPosition[link] - positions[link]);
Debug.Log(hinge.anchor);
/*
* if we want to we should be able to specify this manually...
hinge.autoConfigureConnectedAnchor = false;
hinge.connectedAnchor = (parentJointPosition[link] - positions[parents[link]]);
*/
Debug.Log(parentJointPosition[link]);
Debug.Log(positions[parents[link]]);
Debug.Log(hinge.connectedAnchor);
//TODO: generalize
hinge.axis = Vector3.left;
hinge.breakForce = float.PositiveInfinity;
hinge.breakTorque = float.PositiveInfinity;
}
}
foreach (GameObject link in links)
{
link.transform.localScale = (Vector3.one + Vector3.up * (lengths[link] - 1.0f));
}
}
// Update is called once per frame
void Update () {
foreach (GameObject link in links)
{
Debug.Log(link.name + ": " + link.transform.localPosition);
}
}
}
