Skip to content

Commit

Permalink
CLOiSim-2.7.4
Browse files Browse the repository at this point in the history 
[Major Changes]

* SDF.Implement: Set TargetVelocity=1 for prismatic which has dynamics parameter with limits
  - Set targetVelocity=0 only for not-prismatic joint type when reset() is called
* CLOiSimPlugin: Select parts name as a topic name when ros2/topic_name element is missing in Plugin parameters.


[Minor Changes]

* General: Change String.Empty -> string.Empty
* Core.Main: Modify making cloned model name method.
* Device.Lidar: Modfiy log print in Lidar Device
* Device.DepthCamera: Modify Thread and group size of shader computing for DepthBufferScaling


[Bug fix]

* SDF.Import: make it parenting correctly even if parent model is moved by another joint
  • Loading branch information
@hyunseok-yang
hyunseok-yang authored Jan 20, 2022
1 parent 64e0337 commit cdd400a
Show file tree
Hide file tree
Showing 13 changed files with 64 additions and 33 deletions.
4 changes: 2 additions & 2 deletions Assets/Resources/Shader/DepthBufferScaling.compute
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,8 +2,8 @@
#pragma kernel CSDepthBufferScaling

#define MaxValue16Bits 65535.0f;
#define THREADS 16
#define GROUPS 16
#define THREADS 8
#define GROUPS 4

RWByteAddressBuffer _Buffer;

Expand Down
11 changes: 5 additions & 6 deletions Assets/Scripts/CLOiSimPlugins/MicomPlugin.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6,7 +6,6 @@

using System.Collections.Generic;
using System.Text;
using System;
using Any = cloisim.msgs.Any;
using messages = cloisim.msgs;
using UnityEngine;
Expand Down Expand Up @@ -81,12 +80,12 @@ private void SetupMicom()
var motorFriction = GetPluginParameters().GetValue<float>("wheel/friction/motor", 0.1f);
var brakeFriction = GetPluginParameters().GetValue<float>("wheel/friction/brake", 0.1f);

var wheelLeftName = GetPluginParameters().GetValue<string>("wheel/location[@type='left']", String.Empty);
var wheelRightName = GetPluginParameters().GetValue<string>("wheel/location[@type='right']", String.Empty);
var rearWheelLeftName = GetPluginParameters().GetValue<string>("wheel/location[@type='rear_left']", String.Empty);
var rearWheelRightName = GetPluginParameters().GetValue<string>("wheel/location[@type='rear_right']", String.Empty);
var wheelLeftName = GetPluginParameters().GetValue<string>("wheel/location[@type='left']", string.Empty);
var wheelRightName = GetPluginParameters().GetValue<string>("wheel/location[@type='right']", string.Empty);
var rearWheelLeftName = GetPluginParameters().GetValue<string>("wheel/location[@type='rear_left']", string.Empty);
var rearWheelRightName = GetPluginParameters().GetValue<string>("wheel/location[@type='rear_right']", string.Empty);

if (!rearWheelLeftName.Equals(String.Empty) && !rearWheelRightName.Equals(String.Empty))
if (!rearWheelLeftName.Equals(string.Empty) && !rearWheelRightName.Equals(string.Empty))
{
micomSensor.SetWheel(wheelLeftName, wheelRightName, rearWheelLeftName, rearWheelRightName);
}
Expand Down
3 changes: 2 additions & 1 deletion Assets/Scripts/CLOiSimPlugins/Modules/Base/CLOiSimPlugin.CommonMethod.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -149,7 +149,8 @@ private void SetCustomHandleRequestMessage()
switch (requestType)
{
case "request_ros2":
var topic_name = GetPluginParameters().GetValue<string>("ros2/topic_name");

var topic_name = GetPluginParameters().GetValue<string>("ros2/topic_name", partsName);
GetPluginParameters().GetValues<string>("ros2/frame_id", out var frameIdList);
SetROS2CommonInfoResponse(ref response, topic_name, frameIdList);
break;
Expand Down
7 changes: 5 additions & 2 deletions Assets/Scripts/Devices/DepthCamera.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -119,6 +119,9 @@ protected override void SetupTexture()
cb.Release();
}

private int threadGroupsX = 8;
private int threadGroupsY = 4;

protected override void PostProcessing(ref byte[] buffer)
{
if (readbackDstFormat.Equals(TextureFormat.R16) && computeShader != null)
Expand All @@ -127,8 +130,8 @@ protected override void PostProcessing(ref byte[] buffer)
computeShader.SetBuffer(kernelIndex, "_Buffer", computeBuffer);
computeBuffer.SetData(buffer);

var threadGroupX = camParameter.image_width / 16;
var threadGroupY = camParameter.image_height / 8;
var threadGroupX = camParameter.image_width / threadGroupsX;
var threadGroupY = camParameter.image_height / threadGroupsY;
computeShader.Dispatch(kernelIndex, threadGroupX, threadGroupY, 1);
computeBuffer.GetData(buffer);
computeBuffer.Release();
Expand Down
2 changes: 1 addition & 1 deletion Assets/Scripts/Devices/Lidar.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -485,7 +485,7 @@ protected override void GenerateMessage()
}
catch (Exception ex)
{
Debug.LogWarning("Error occured with Buffer.BlockCopy : " + ex.Message + ", " + srcBufferOffset + ", " + dstBufferOffset + ", " + copyLength);
Debug.LogWarning("Error occured with Buffer.BlockCopy : " + ex.Message + ", " + srcBufferOffset + ", " + dstBufferOffset + ", " + copyLength + ", Len: src "+ srcBuffer.Length + ", dst " + laserScan.Ranges.Length);
}
}
}
Expand Down
3 changes: 2 additions & 1 deletion Assets/Scripts/Main.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -304,6 +304,7 @@ private string GetClonedModelName(in string modelName)
if (childTransform.name.CompareTo(tmpModelName) == 0)
{
tmpModelName = modelName + "_clone_" + numbering++;
i = 0;
}
}
return tmpModelName;
Expand All @@ -323,7 +324,7 @@ private IEnumerator LoadModel(string modelPath, string modelFileName)
var addingModel = uiMainCanvasRoot.GetComponentInChildren<AddModel>();
addingModel.SetAddingModelForDeploy(targetObject);

yield return new WaitForSeconds(0.1f);
yield return new WaitForSeconds(0.01f);
yield return new WaitForEndOfFrame();

// for GUI
Expand Down
18 changes: 14 additions & 4 deletions Assets/Scripts/Tools/SDF/Helper/PoseControl.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -132,10 +132,15 @@ private void ResetArticulationBody(in int targetFrame = 0)
articulationBody.jointVelocity = zeroSpace;
articulationBody.jointAcceleration = zeroSpace;
articulationBody.jointForce = zeroSpace;
var isPrismatic = articulationBody.jointType.Equals(UE.ArticulationJointType.PrismaticJoint);

GetJointTarget(out var targetJoint1, out var targetJoint2, targetFrame);

var xDrive = articulationBody.xDrive;
if (!isPrismatic)
{
xDrive.targetVelocity = 0;
}
if (!articulationBody.linearLockX.Equals(UE.ArticulationDofLock.LockedMotion) ||
!articulationBody.twistLock.Equals(UE.ArticulationDofLock.LockedMotion))
{
Expand All @@ -153,10 +158,13 @@ private void ResetArticulationBody(in int targetFrame = 0)
{
xDrive.target = 0;
}
xDrive.targetVelocity = 0;
articulationBody.xDrive = xDrive;

var yDrive = articulationBody.yDrive;
if (!isPrismatic)
{
yDrive.targetVelocity = 0;
}
if (!articulationBody.linearLockY.Equals(UE.ArticulationDofLock.LockedMotion) ||
!articulationBody.swingYLock.Equals(UE.ArticulationDofLock.LockedMotion))
{
Expand All @@ -174,10 +182,13 @@ private void ResetArticulationBody(in int targetFrame = 0)
{
yDrive.target = 0;
}
yDrive.targetVelocity = 0;
articulationBody.yDrive = yDrive;

var zDrive = articulationBody.zDrive;
if (!isPrismatic)
{
zDrive.targetVelocity = 0;
}
if (!articulationBody.linearLockY.Equals(UE.ArticulationDofLock.LockedMotion) ||
!articulationBody.swingZLock.Equals(UE.ArticulationDofLock.LockedMotion))
{
Expand All @@ -193,9 +204,8 @@ private void ResetArticulationBody(in int targetFrame = 0)
}
else
{
yDrive.target = 0;
zDrive.target = 0;
}
zDrive.targetVelocity = 0;
articulationBody.zDrive = zDrive;
}
}
Expand Down
14 changes: 13 additions & 1 deletion Assets/Scripts/Tools/SDF/Implement/Implement.Joint.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -18,8 +18,13 @@ public static UE.Pose SetArticulationBodyRelationship(in SDF.Joint joint, UE.Tra
var modelTransformChild = linkChild.parent;
var modelHelperChild = modelTransformChild.GetComponent<SDF.Helper.Model>();

var linkHelperParent = linkParent.GetComponent<SDF.Helper.Link>();
var linkHelperChild = linkChild.GetComponent<SDF.Helper.Link>();

var anchorPose = new UE.Pose();
if (modelTransformChild.Equals(modelTransformParent) || modelHelperChild.IsFirstChild)
if (linkHelperChild.Model.Equals(linkHelperParent.Model) ||
modelTransformChild.Equals(modelTransformParent) ||
modelHelperChild.IsFirstChild)
{
linkChild.SetParent(linkParent);

Expand Down Expand Up @@ -205,6 +210,13 @@ public static void MakePrismatic(in UE.ArticulationBody body, in SDF.Axis axis,
drive.stiffness = (float)axis.dynamics.spring_stiffness;
drive.target = (float)axis.dynamics.spring_reference;
drive.damping = (float)axis.dynamics.damping;

// Check if spring
if (axis.limit.Use())
{
drive.targetVelocity = 1;
}

body.jointFriction = (float)axis.dynamics.friction;
}
else
Expand Down
13 changes: 9 additions & 4 deletions Assets/Scripts/Tools/SDF/Import/Import.Joint.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -24,7 +24,8 @@ private static UE.Transform FindTransformByName(in string name, UE.Transform tar
rootTransform = rootTransform.parent;
}

(var modelName, var linkName) = SDF2Unity.GetModelLinkName(name);
(var modelName, var linkName) = SDF2Unity.GetModelLinkName(name, targetTransform.name);
// UE.Debug.Log("GetModelLinkName => " + modelName + ", " + linkName);

if (string.IsNullOrEmpty(modelName))
{
Expand Down Expand Up @@ -53,10 +54,14 @@ private static UE.Transform FindTransformByName(in string name, UE.Transform tar

if (modelTransform != null)
{
var linkTransform = modelTransform.Find(linkName);
if (linkTransform != null)
foreach (var linkObject in modelTransform.GetComponentsInChildren<SDF.Helper.Link>())
{
foundLinkObject = linkTransform;
var linkTransform = linkObject.transform;
if (linkTransform.name.Equals(linkName))
{
foundLinkObject = linkTransform;
break;
}
}
}
}
Expand Down
6 changes: 3 additions & 3 deletions Assets/Scripts/Tools/SDF/Import/Import.Link.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -95,9 +95,9 @@ private static UE.ArticulationBody CreateArticulationBody(in UE.GameObject linkO
articulationBody.angularVelocity = UE.Vector3.zero;
articulationBody.useGravity = (linkHelper == null) ? false : linkHelper.useGravity;
articulationBody.jointType = UE.ArticulationJointType.FixedJoint;
articulationBody.mass = (inertial == null) ? 1e-07f : (float)inertial.mass;
articulationBody.linearDamping = 0.05f;
articulationBody.angularDamping = 0.05f;
articulationBody.mass = (inertial == null) ? 1e-06f : (float)inertial.mass;
articulationBody.linearDamping = 0.01f;
articulationBody.angularDamping = 0.01f;
articulationBody.jointFriction = 0f;
articulationBody.velocity = UE.Vector3.zero;
articulationBody.angularVelocity = UE.Vector3.zero;
Expand Down
10 changes: 5 additions & 5 deletions Assets/Scripts/Tools/SDF/Parser/Pose.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -51,8 +51,8 @@ public void Set(string x, string y)
var code = Type.GetTypeCode(typeof(T));
if (code != TypeCode.Empty)
{
x = regex_num.Replace(x, String.Empty);
y = regex_num.Replace(y, String.Empty);
x = regex_num.Replace(x, string.Empty);
y = regex_num.Replace(y, string.Empty);
Set((T)Convert.ChangeType(x, code), (T)Convert.ChangeType(y, code));
}
}
Expand Down Expand Up @@ -116,9 +116,9 @@ public void Set(string x, string y, string z)
var code = Type.GetTypeCode(typeof(T));
if (code != TypeCode.Empty)
{
x = Vector2<T>.regex_num.Replace(x, String.Empty);
y = Vector2<T>.regex_num.Replace(y, String.Empty);
z = Vector2<T>.regex_num.Replace(z, String.Empty);
x = Vector2<T>.regex_num.Replace(x, string.Empty);
y = Vector2<T>.regex_num.Replace(y, string.Empty);
z = Vector2<T>.regex_num.Replace(z, string.Empty);
Set((T)Convert.ChangeType(x, code), (T)Convert.ChangeType(y, code), (T)Convert.ChangeType(z, code));
}
}
Expand Down
4 changes: 2 additions & 2 deletions Assets/Scripts/Tools/SDF/Util/SDF2Unity.Model.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -8,9 +8,9 @@

public partial class SDF2Unity
{
public static (string, string) GetModelLinkName(in string value)
public static (string, string) GetModelLinkName(in string value, in string defaultModelName = "")
{
var modelName = string.Empty;
var modelName = defaultModelName;
var linkName = value;

if (value.Contains("::"))
Expand Down
2 changes: 1 addition & 1 deletion ProjectSettings/ProjectSettings.asset
View file
Original file line number Diff line number Diff line change
Expand Up @@ -134,7 +134,7 @@ PlayerSettings:
16:10: 1
16:9: 1
Others: 1
bundleVersion: 2.7.3
bundleVersion: 2.7.4
preloadedAssets: []
metroInputSource: 0
wsaTransparentSwapchain: 0
Expand Down

0 comments on commit cdd400a

Please sign in to comment.