Skip to content

Commit

Permalink
feat. add IK for object orientation
Browse files Browse the repository at this point in the history
  • Loading branch information
@Chen-Yulin
Chen-Yulin committed Jul 20, 2024
1 parent 5b56f02 commit 21c25dc
Show file tree
Hide file tree
Showing 9 changed files with 190 additions and 42 deletions.
Binary file added Assets/2f85-open.fbx
View file
Binary file not shown.
107 changes: 107 additions & 0 deletions Assets/2f85-open.fbx.meta
View file

Some generated files are not rendered by default. Learn more about how customized files appear on GitHub.

30 changes: 15 additions & 15 deletions Assets/Scenes/SampleScene.unity
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1807,15 +1807,15 @@ Transform:
m_PrefabInstance: {fileID: 0}
m_PrefabAsset: {fileID: 0}
m_GameObject: {fileID: 91002362}
m_LocalRotation: {x: 0.22859704, y: -0.6457862, z: 0.19975486, w: 0.7005724}
m_LocalPosition: {x: 0.607, y: 1.86, z: 0.049}
m_LocalRotation: {x: 0.29883623, y: -0.64085644, z: 0.29883623, w: 0.64085644}
m_LocalPosition: {x: 0.607, y: 2.172, z: 0}
m_LocalScale: {x: 1, y: 1, z: 1}
m_ConstrainProportionsScale: 0
m_Children:
- {fileID: 637172883}
m_Father: {fileID: 0}
m_RootOrder: 9
m_LocalEulerAnglesHint: {x: 35.331, y: -85.683, z: -1.079}
m_LocalEulerAnglesHint: {x: 50, y: -90, z: 0}
--- !u!114 &91002364
MonoBehaviour:
m_ObjectHideFlags: 0
Expand Down Expand Up @@ -4755,7 +4755,7 @@ Transform:
m_PrefabAsset: {fileID: 0}
m_GameObject: {fileID: 223719657}
m_LocalRotation: {x: -0.0000001490116, y: -0.70710677, z: -0.000000029802319, w: -0.7071068}
m_LocalPosition: {x: 0.00045040395, y: -0.24459928, z: -0.0005501807}
m_LocalPosition: {x: 0.00045040395, y: -0.31, z: -0.0005501807}
m_LocalScale: {x: 0.04999998, y: 0.04999998, z: 0.049999975}
m_ConstrainProportionsScale: 0
m_Children: []
Expand Down Expand Up @@ -5692,7 +5692,7 @@ Transform:
m_PrefabAsset: {fileID: 0}
m_GameObject: {fileID: 287071670}
m_LocalRotation: {x: 0, y: 0, z: 0, w: 1}
m_LocalPosition: {x: 0, y: 1.6, z: 0}
m_LocalPosition: {x: 0, y: 1.532, z: -0.218}
m_LocalScale: {x: 1, y: 1, z: 1}
m_ConstrainProportionsScale: 0
m_Children:
Expand Down Expand Up @@ -9029,7 +9029,7 @@ Transform:
m_PrefabInstance: {fileID: 0}
m_PrefabAsset: {fileID: 0}
m_GameObject: {fileID: 389999216}
m_LocalRotation: {x: 0, y: 0, z: -1, w: 0}
m_LocalRotation: {x: -0.10452846, y: 0, z: -0.9945219, w: 0}
m_LocalPosition: {x: 0, y: -0.075499825, z: -0.00025028098}
m_LocalScale: {x: 1, y: 1, z: 1}
m_ConstrainProportionsScale: 0
Expand All @@ -9038,7 +9038,7 @@ Transform:
- {fileID: 2115809186}
m_Father: {fileID: 1196516649}
m_RootOrder: 2
m_LocalEulerAnglesHint: {x: 0, y: 0, z: -180}
m_LocalEulerAnglesHint: {x: 0, y: 12, z: -180}
--- !u!1 &397310429
GameObject:
m_ObjectHideFlags: 0
Expand Down Expand Up @@ -20814,7 +20814,7 @@ Transform:
m_PrefabInstance: {fileID: 0}
m_PrefabAsset: {fileID: 0}
m_GameObject: {fileID: 1014946303}
m_LocalRotation: {x: -0.7071069, y: 0.00000041723246, z: 0.7071067, w: 0.00000008940696}
m_LocalRotation: {x: 0.62932044, y: 0, z: -0.7771459, w: 0}
m_LocalPosition: {x: 0.0002501905, y: -0.07549977, z: 0}
m_LocalScale: {x: 1, y: 1, z: 1}
m_ConstrainProportionsScale: 0
Expand All @@ -20823,7 +20823,7 @@ Transform:
- {fileID: 729633860}
m_Father: {fileID: 1167032851}
m_RootOrder: 2
m_LocalEulerAnglesHint: {x: 0, y: 0, z: -180}
m_LocalEulerAnglesHint: {x: 0, y: -78, z: -180}
--- !u!1 &1035868678
GameObject:
m_ObjectHideFlags: 0
Expand Down Expand Up @@ -22616,7 +22616,7 @@ Transform:
m_PrefabAsset: {fileID: 0}
m_GameObject: {fileID: 1196516648}
m_LocalRotation: {x: -0.00000021634176, y: -0.0000000927179, z: 0.00000006181193, w: 1}
m_LocalPosition: {x: 0.00075006485, y: -0.106500104, z: 0}
m_LocalPosition: {x: 0.00075006485, y: -0.17, z: 0}
m_LocalScale: {x: 1, y: 1, z: 1}
m_ConstrainProportionsScale: 0
m_Children:
Expand Down Expand Up @@ -29358,7 +29358,7 @@ Transform:
m_GameObject: {fileID: 1656230296}
m_LocalRotation: {x: 3.330669e-16, y: 0.000000014901161, z: -0.000000022351742, w: 1}
m_LocalPosition: {x: 0.000000076293944, y: 0.000000038146972, z: 0.000000038146972}
m_LocalScale: {x: 1, y: 1, z: 1}
m_LocalScale: {x: 1, y: 3, z: 1}
m_ConstrainProportionsScale: 0
m_Children: []
m_Father: {fileID: 1196516649}
Expand Down Expand Up @@ -38066,7 +38066,7 @@ Transform:
m_PrefabInstance: {fileID: 0}
m_PrefabAsset: {fileID: 0}
m_GameObject: {fileID: 5126358137507062015}
m_LocalRotation: {x: 0, y: 0, z: -1, w: 0}
m_LocalRotation: {x: -0.10452846, y: 0, z: -0.9945219, w: 0}
m_LocalPosition: {x: 0, y: -0.075499825, z: -0.00025028098}
m_LocalScale: {x: 1, y: 1, z: 1}
m_ConstrainProportionsScale: 0
Expand All @@ -38075,7 +38075,7 @@ Transform:
- {fileID: 293615569989222206}
m_Father: {fileID: 7872603513119881426}
m_RootOrder: 2
m_LocalEulerAnglesHint: {x: 0, y: 0, z: -180}
m_LocalEulerAnglesHint: {x: 0, y: 12, z: -180}
--- !u!23 &5528787128564062439
MeshRenderer:
m_ObjectHideFlags: 0
Expand Down Expand Up @@ -39170,7 +39170,7 @@ Transform:
m_PrefabAsset: {fileID: 0}
m_GameObject: {fileID: 7527837571619291333}
m_LocalRotation: {x: -0.00000021634176, y: -0.0000000927179, z: 0.00000006181193, w: 1}
m_LocalPosition: {x: 0.00075006485, y: -0.106500104, z: 0}
m_LocalPosition: {x: 0.00075006485, y: -0.17, z: 0}
m_LocalScale: {x: 1, y: 1, z: 1}
m_ConstrainProportionsScale: 0
m_Children:
Expand Down Expand Up @@ -39349,7 +39349,7 @@ Transform:
m_GameObject: {fileID: 4975893042945254323}
m_LocalRotation: {x: 3.330669e-16, y: 0.000000014901161, z: -0.000000022351742, w: 1}
m_LocalPosition: {x: 0.000000076293944, y: 0.000000038146972, z: 0.000000038146972}
m_LocalScale: {x: 1, y: 1, z: 1}
m_LocalScale: {x: 1, y: 3, z: 1}
m_ConstrainProportionsScale: 0
m_Children: []
m_Father: {fileID: 7872603513119881426}
Expand Down
14 changes: 9 additions & 5 deletions Assets/Script/DT/RouteGenerator.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -13,9 +13,11 @@ public class KeyPoint
public Vector3 pos = Vector3.zero;
public bool grab = false;
public bool wait = false;
public KeyPoint(Vector3 pos, bool grab = false, bool wait = false)
public Vector3 right = default;
public KeyPoint(Vector3 pos, Vector3 right = default, bool grab = false, bool wait = false)
{
this.pos = pos;
this.right = right;
this.grab = grab;
this.wait = wait;
}
Expand All @@ -26,11 +28,13 @@ public class ArmAction
public float[] angles = new float[6];
public bool grab = false;
public bool wait = false;
public ArmAction(float[] a, bool grab = false, bool wait = false)
public Vector3 position = Vector3.zero;
public ArmAction(float[] a, bool grab = false, bool wait = false, Vector3 position = default)
{
angles = a;
this.grab = grab;
this.wait = wait;
this.position = position;
}
}

Expand All @@ -54,7 +58,7 @@ public Route(List<KeyPoint> turningPoints)
int sigment = (int)(dist * 20f);
for (int j = 0; j <= sigment; j++)
{
keypoints.Add(new KeyPoint(start.pos + ((float)j) / sigment * (end.pos - start.pos), start.grab));
keypoints.Add(new KeyPoint(start.pos + ((float)j) / sigment * (end.pos - start.pos), start.right, start.grab));
}
}
}
Expand Down Expand Up @@ -177,7 +181,7 @@ public bool CalculateSequence()
foreach (var kpt in route.keypoints)
{
Debug.Log("Calculate action for " + kpt.pos.ToString());
bool success = IK.InverseKinematics(IK.GetPositionForJ4(kpt.pos));
bool success = IK.InverseKinematics(IK.GetPositionForJ4(kpt.pos), kpt.right);
if (!success)
{
actionSequence.Clear();
Expand All @@ -186,7 +190,7 @@ public bool CalculateSequence()
}
float[] res = new float[IK.Angles.Length];
Array.Copy(IK.Angles, res, IK.Angles.Length);
actionSequence.Add(new ArmAction(res, kpt.grab, kpt.wait));
actionSequence.Add(new ArmAction(res, kpt.grab, kpt.wait, kpt.pos));
}
return true;
}
Expand Down
17 changes: 14 additions & 3 deletions Assets/Script/IK/IKController.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -14,7 +14,7 @@ public class IKController : MonoBehaviour {

public bool IK = false;

public int maxStep = 200;
public int maxStep = 1000;

public ArmDTController ArmDT; // for the initial state of arm (reset angle)

Expand Down Expand Up @@ -63,10 +63,10 @@ public Vector3 GetPositionForJ4(Vector3 pos)
left = left.normalized * 0.1148f;
Vector3 offset = forward - left;
offset = offset.normalized * 0.1175f;
return pos + Vector3.up * 0.2446f - offset;
return pos + Vector3.up * (0.31f) - offset;
}

public bool InverseKinematics(Vector3 pos)
public bool InverseKinematics(Vector3 pos, Vector3 right = default)
{
int step = 0;
while(step < maxStep && DistanceFromTarget(pos, Angles) >= DistanceThreshold)
Expand All @@ -76,6 +76,17 @@ public bool InverseKinematics(Vector3 pos)
}

Angles[3] = Angles[2]+Angles[1];

Angles[4] = 0;

if (right != default)
{
float offset = Vector2.SignedAngle(MathTool.Get2DCoordinate(Vector3.forward), MathTool.Get2DCoordinate(right));
offset -= 12;
offset -= Angles[0];
Angles[5] = offset + 90;
}

return step < 1000;
}

Expand Down
8 changes: 8 additions & 0 deletions Assets/Script/Jaka/JakaController.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -36,6 +36,14 @@ public void SetJointRot(float[] rot)
network.SendData(info);
}

public void SetGripper(bool open)
{
string info = "{Gripper}\n";
info += open ? "0\n" : "1\n";
Debug.Log(info);
network.SendData(info);
}

// Start is called before the first frame update
void Start()
{
Expand Down
16 changes: 8 additions & 8 deletions Assets/Script/Network/UdpSocket.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -97,28 +97,28 @@ private void ReceiveData()
string text = Encoding.UTF8.GetString(data);
ProcessInput(text);
string[] lines = text.Split(new[] { '\n', '\r' }, StringSplitOptions.RemoveEmptyEntries);
if (lines[0] == "{Object Detection}")
{
if (lines[0] == "{Object Detection}")
{
//print("Receive Object");
string cat = lines[1];
string cat = lines[1];
float[] transform = new float[10];

// 将剩下的十行转换为整数存入数组
for (int i = 0; i < 10; i++)
{
transform[i] = float.Parse(lines[i + 2]);
}
ObjectManager.AsyUpdateObject(cat, transform);
}
ObjectManager.AsyUpdateObject(cat, transform);
}else if (lines[0] == "{Current Joint}")
{
float[] angle = new float[6];
for (int i = 0; i < 6; i++)
{
angle[i] = float.Parse(lines[i + 1]);
}
if (ArmDT)
{
ArmDT.UpdateRealAngle(MathTool.Real_to_DT_angle(angle));
if (ArmDT)
{
ArmDT.UpdateRealAngle(MathTool.Real_to_DT_angle(angle));
}
}
}
Expand Down
Loading

0 comments on commit 21c25dc

Please sign in to comment.