Solve dragging (#47)

runPreDrag, runDragStep, runPostDrag, PosICDragNewtonRaphson

CMakeLists.txt

@@ -278,6 +278,7 @@ set(ONDSELSOLVER_SRC
         OndselSolver/PointInLineJoint.cpp
         OndselSolver/PointInPlaneJoint.cpp
         OndselSolver/Polynomial.cpp
+        OndselSolver/PosICDragNewtonRaphson.cpp
         OndselSolver/PosICKineNewtonRaphson.cpp
         OndselSolver/PosICNewtonRaphson.cpp
         OndselSolver/PosKineNewtonRaphson.cpp
@@ -596,6 +597,7 @@ set(ONDSELSOLVER_HEADERS
         OndselSolver/PlanarJoint.h
         OndselSolver/PointInLineJoint.h
         OndselSolver/PointInPlaneJoint.h
+        OndselSolver/PosICDragNewtonRaphson.h
         OndselSolver/PosICKineNewtonRaphson.h
         OndselSolver/PosICNewtonRaphson.h
         OndselSolver/PosKineNewtonRaphson.h

OndselSolver/ASMTAssembly.cpp

@@ -65,6 +65,12 @@ MbD::ASMTAssembly::ASMTAssembly() : ASMTSpatialContainer()
 	times = std::make_shared<FullRow<double>>();
 }
 
+std::shared_ptr<ASMTAssembly> MbD::ASMTAssembly::With()
+{
+	auto assembly = std::make_shared<ASMTAssembly>();
+	return assembly;
+}
+
 void MbD::ASMTAssembly::runSinglePendulumSuperSimplified()
 {
 	//In this version we skip declaration of variables that don't need as they use default values.
@@ -383,6 +389,28 @@ void MbD::ASMTAssembly::runSinglePendulum()
 	assembly->runKINEMATIC();
 }
 
+std::shared_ptr<ASMTAssembly> MbD::ASMTAssembly::assemblyFromFile(const char* fileName)
+{
+	std::ifstream stream(fileName);
+	if (stream.fail()) {
+		throw std::invalid_argument("File not found.");
+	}
+	std::string line;
+	std::vector<std::string> lines;
+	while (std::getline(stream, line)) {
+		lines.push_back(line);
+	}
+	auto assembly = ASMTAssembly::With();
+	auto str = assembly->popOffTop(lines);
+	bool bool1 = str == "freeCAD: 3D CAD with Motion Simulation  by  askoh.com";
+	bool bool2 = str == "OndselSolver";
+	assert(bool1 || bool2);
+	assert(assembly->readStringOffTop(lines) == "Assembly");
+	assembly->setFilename(fileName);
+	assembly->parseASMT(lines);
+	return assembly;
+}
+
 void MbD::ASMTAssembly::runFile(const char* fileName)
 {
 	std::ifstream stream(fileName);
@@ -408,6 +436,25 @@ void MbD::ASMTAssembly::runFile(const char* fileName)
 	}
 }
 
+void MbD::ASMTAssembly::runDraggingTest()
+{
+	auto assembly = ASMTAssembly::assemblyFromFile("../testapp/dragCrankSlider.asmt");
+	auto dragPart = assembly->parts->at(0);
+	auto dragParts = std::make_shared<std::vector<std::shared_ptr<ASMTPart>>>();
+	dragParts->push_back(dragPart);
+	assembly->runPreDrag();	//Do this before first drag
+	FColDsptr pos3D, delta;
+	pos3D = dragPart->position3D;
+	delta = std::make_shared<FullColumn<double>>(ListD{ 0.1, 0.2, 0.3 });
+	dragPart->updateMbDFromPosition3D(pos3D->plusFullColumn(delta));
+	assembly->runDragStep(dragParts);
+	pos3D = dragPart->position3D;
+	delta = std::make_shared<FullColumn<double>>(ListD{ 0.3, 0.2, 0.1 });
+	dragPart->updateMbDFromPosition3D(pos3D->plusFullColumn(delta));
+	assembly->runDragStep(dragParts);
+	assembly->runPostDrag();	//Do this after last drag
+}
+
 void MbD::ASMTAssembly::readWriteFile(const char* fileName)
 {
 	std::ifstream stream(fileName);
@@ -1046,10 +1093,36 @@ void MbD::ASMTAssembly::solve()
 	runKINEMATIC();
 }
 
+void MbD::ASMTAssembly::runPreDrag()
+{
+	mbdSystem = std::make_shared<System>();
+	mbdSystem->externalSystem->asmtAssembly = this;
+	try {
+		mbdSystem->runPreDrag(mbdSystem);
+	}
+	catch (SimulationStoppingError ex) {
+
+	}
+}
+
+void MbD::ASMTAssembly::runDragStep(std::shared_ptr<std::vector<std::shared_ptr<ASMTPart>>> dragParts)
+{
+	auto dragMbDParts = std::make_shared<std::vector<std::shared_ptr<Part>>>();
+	for (auto& dragPart : *dragParts) {
+		auto dragMbDPart = std::static_pointer_cast<Part>(dragPart->mbdObject);
+		dragMbDParts->push_back(dragMbDPart);
+	}
+	mbdSystem->runDragStep(dragMbDParts);
+}
+
+void MbD::ASMTAssembly::runPostDrag()
+{
+	runPreDrag();
+}
+
 void MbD::ASMTAssembly::runKINEMATIC()
 {
-	auto mbdSystem = std::make_shared<System>();
-	mbdObject = mbdSystem;
+	mbdSystem = std::make_shared<System>();
 	mbdSystem->externalSystem->asmtAssembly = this;
 	try {
 		mbdSystem->runKINEMATIC(mbdSystem);

OndselSolver/ASMTAssembly.h

@@ -33,11 +33,14 @@ namespace MbD {
 		//
 	public:
 		ASMTAssembly();
+		static std::shared_ptr<ASMTAssembly> With();
 		static void runSinglePendulumSuperSimplified();
 		static void runSinglePendulumSuperSimplified2();
 		static void runSinglePendulumSimplified();
 		static void runSinglePendulum();
+		static std::shared_ptr<ASMTAssembly> assemblyFromFile(const char* chars);
 		static void runFile(const char* chars);
+		static void runDraggingTest();
 		static void readWriteFile(const char* chars);
 		void initialize() override;
 		ASMTAssembly* root() override;
@@ -83,6 +86,9 @@ namespace MbD {
 		/* This function performs a one shot solve of the assembly.*/
 		void solve();
 
+		void runPreDrag();
+		void runDragStep(std::shared_ptr<std::vector<std::shared_ptr<ASMTPart>>> dragParts);
+		void runPostDrag();
 		void runKINEMATIC();
 		void initprincipalMassMarker();
 		std::shared_ptr<ASMTSpatialContainer> spatialContainerAt(std::shared_ptr<ASMTAssembly> self, std::string& longname);
@@ -128,6 +134,7 @@ namespace MbD {
 		std::shared_ptr<std::vector<double>> times = std::make_shared<std::vector<double>>();
 		std::shared_ptr<ASMTTime> asmtTime = std::make_shared<ASMTTime>();
 		std::shared_ptr<Units> mbdUnits = std::make_shared<Units>();
+		std::shared_ptr<System> mbdSystem;
 		MBDynSystem* mbdynItem = nullptr;
 	};
 }

OndselSolver/ASMTItem.cpp

@@ -56,6 +56,22 @@ void MbD::ASMTItem::parseASMT(std::vector<std::string>&)
 	assert(false);
 }
 
+std::string MbD::ASMTItem::popOffTop(std::vector<std::string>& args)
+{
+	auto str = args.at(0);	//Must copy string
+	args.erase(args.begin());
+	return str;
+}
+
+std::string MbD::ASMTItem::readStringOffTop(std::vector<std::string>& args)
+{
+	auto iss = std::istringstream(args.at(0));
+	args.erase(args.begin());
+	std::string str;
+	iss >> str;
+	return str;
+}
+
 FRowDsptr MbD::ASMTItem::readRowOfDoubles(std::string& line)
 {
 	std::istringstream iss(line);

OndselSolver/ASMTItem.h

@@ -19,6 +19,7 @@ namespace MbD {
 	{
 		//
 	public:
+		ASMTItem() {}
 		virtual ~ASMTItem() {}
 		virtual ASMTAssembly* root();
 		virtual ASMTSpatialContainer* partOrAssembly();
@@ -29,6 +30,8 @@ namespace MbD {
 		virtual std::string classname();
 		void setName(std::string str);
 		virtual void parseASMT(std::vector<std::string>& lines);
+		std::string popOffTop(std::vector<std::string>& args);
+		std::string readStringOffTop(std::vector<std::string>& args);
 		FRowDsptr readRowOfDoubles(std::string& line);
 		FColDsptr readColumnOfDoubles(std::string& line);
 		double readDouble(std::string& line);

OndselSolver/ASMTMarker.cpp

@@ -15,57 +15,57 @@
 #include "MarkerFrame.h"
 #include "ASMTPrincipalMassMarker.h"
 
-namespace MbD {
-    void ASMTMarker::parseASMT(std::vector<std::string>& lines)
-    {
-        readName(lines);
-        readPosition3D(lines);
-        readRotationMatrix(lines);
-    }
+using namespace MbD;
 
-    FColDsptr ASMTMarker::rpmp()
-    {
-        //p is cm
-        auto refItem = static_cast<ASMTRefItem*>(owner);
-        auto& rPrefP = refItem->position3D;
-        auto& aAPref = refItem->rotationMatrix;
-        auto& rrefmref = position3D;
-        auto rPmP = rPrefP->plusFullColumn(aAPref->timesFullColumn(rrefmref));
-        auto& principalMassMarker = static_cast<ASMTPart*>(refItem->owner)->principalMassMarker;
-        auto& rPcmP = principalMassMarker->position3D;
-        auto& aAPcm = principalMassMarker->rotationMatrix;
-        auto rpmp = aAPcm->transposeTimesFullColumn(rPmP->minusFullColumn(rPcmP));
-        return rpmp;
-    }
-
-    FMatDsptr ASMTMarker::aApm()
-    {
-        //p is cm
-        auto refItem = static_cast<ASMTRefItem*>(owner);
-        auto& aAPref = refItem->rotationMatrix;
-        auto& aArefm = rotationMatrix;
-        auto& principalMassMarker = static_cast<ASMTPart*>(refItem->owner)->principalMassMarker;
-        auto& aAPcm = principalMassMarker->rotationMatrix;
-        auto aApm = aAPcm->transposeTimesFullMatrix(aAPref->timesFullMatrix(aArefm));
-        return aApm;
-    }
-
-    void ASMTMarker::createMbD(std::shared_ptr<System>, std::shared_ptr<Units> mbdUnits)
-    {
-        auto mkr = CREATE<MarkerFrame>::With(name.c_str());
-        auto prt = std::static_pointer_cast<Part>(partOrAssembly()->mbdObject);
-        prt->partFrame->addMarkerFrame(mkr);
-
-        mkr->rpmp = rpmp()->times(1.0 / mbdUnits->length);
-        mkr->aApm = aApm();
-        mbdObject = mkr->endFrames->at(0);
-    }
-
-    void ASMTMarker::storeOnLevel(std::ofstream& os, int level)
-    {
-        storeOnLevelString(os, level, "Marker");
-        storeOnLevelString(os, level + 1, "Name");
-        storeOnLevelString(os, level + 2, name);
-        ASMTSpatialItem::storeOnLevel(os, level);
-    }
+void ASMTMarker::parseASMT(std::vector<std::string>& lines)
+{
+	readName(lines);
+	readPosition3D(lines);
+	readRotationMatrix(lines);
+}
+
+FColDsptr ASMTMarker::rpmp()
+{
+	//p is cm
+	auto refItem = static_cast<ASMTRefItem*>(owner);
+	auto& rPrefP = refItem->position3D;
+	auto& aAPref = refItem->rotationMatrix;
+	auto& rrefmref = position3D;
+	auto rPmP = rPrefP->plusFullColumn(aAPref->timesFullColumn(rrefmref));
+	auto& principalMassMarker = static_cast<ASMTPart*>(refItem->owner)->principalMassMarker;
+	auto& rPcmP = principalMassMarker->position3D;
+	auto& aAPcm = principalMassMarker->rotationMatrix;
+	auto rpmp = aAPcm->transposeTimesFullColumn(rPmP->minusFullColumn(rPcmP));
+	return rpmp;
+}
+
+FMatDsptr ASMTMarker::aApm()
+{
+	//p is cm
+	auto refItem = static_cast<ASMTRefItem*>(owner);
+	auto& aAPref = refItem->rotationMatrix;
+	auto& aArefm = rotationMatrix;
+	auto& principalMassMarker = static_cast<ASMTPart*>(refItem->owner)->principalMassMarker;
+	auto& aAPcm = principalMassMarker->rotationMatrix;
+	auto aApm = aAPcm->transposeTimesFullMatrix(aAPref->timesFullMatrix(aArefm));
+	return aApm;
+}
+
+void ASMTMarker::createMbD(std::shared_ptr<System>, std::shared_ptr<Units> mbdUnits)
+{
+	auto mkr = CREATE<MarkerFrame>::With(name.c_str());
+	auto prt = std::static_pointer_cast<Part>(partOrAssembly()->mbdObject);
+	prt->partFrame->addMarkerFrame(mkr);
+
+	mkr->rpmp = rpmp()->times(1.0 / mbdUnits->length);
+	mkr->aApm = aApm();
+	mbdObject = mkr->endFrames->at(0);
+}
+
+void ASMTMarker::storeOnLevel(std::ofstream& os, int level)
+{
+	storeOnLevelString(os, level, "Marker");
+	storeOnLevelString(os, level + 1, "Name");
+	storeOnLevelString(os, level + 2, name);
+	ASMTSpatialItem::storeOnLevel(os, level);
 }

OndselSolver/ASMTMotion.cpp

@@ -9,36 +9,36 @@
 
 #include "ASMTMotion.h"
 
-namespace MbD {
-    void ASMTMotion::readMotionSeries(std::vector<std::string>& lines)
-    {
-        std::string str = lines[0];
-        std::string substr = "MotionSeries";
-        auto pos = str.find(substr);
-        assert(pos != std::string::npos);
-        str.erase(0, pos + substr.length());
-        auto seriesName = readString(str);
-        assert(fullName("") == seriesName);
-        lines.erase(lines.begin());
-        readFXonIs(lines);
-        readFYonIs(lines);
-        readFZonIs(lines);
-        readTXonIs(lines);
-        readTYonIs(lines);
-        readTZonIs(lines);
-    }
+using namespace MbD;
 
-    void ASMTMotion::initMarkers()
-    {
-    }
-
-    void ASMTMotion::storeOnLevel(std::ofstream&, int)
-    {
-        assert(false);
-    }
-
-    void ASMTMotion::storeOnTimeSeries(std::ofstream&)
-    {
-        assert(false);
-    }
+void ASMTMotion::readMotionSeries(std::vector<std::string>& lines)
+{
+	std::string str = lines[0];
+	std::string substr = "MotionSeries";
+	auto pos = str.find(substr);
+	assert(pos != std::string::npos);
+	str.erase(0, pos + substr.length());
+	auto seriesName = readString(str);
+	assert(fullName("") == seriesName);
+	lines.erase(lines.begin());
+	readFXonIs(lines);
+	readFYonIs(lines);
+	readFZonIs(lines);
+	readTXonIs(lines);
+	readTYonIs(lines);
+	readTZonIs(lines);
+}
+
+void ASMTMotion::initMarkers()
+{
+}
+
+void ASMTMotion::storeOnLevel(std::ofstream&, int)
+{
+	assert(false);
+}
+
+void ASMTMotion::storeOnTimeSeries(std::ofstream&)
+{
+	assert(false);
 }

OndselSolver/ASMTRotationalMotion.cpp

@@ -73,7 +73,7 @@ void MbD::ASMTRotationalMotion::createMbD(std::shared_ptr<System> mbdSys, std::s
 	geoPhi->createMbD(mbdSys, mbdUnits);
 	//std::cout << *geoPhi << std::endl;
 	auto simple = geoPhi->simplified(geoPhi);
-	//std::cout << *simple << std::endl;
+	std::cout << *simple << std::endl;
 	std::static_pointer_cast<ZRotation>(mbdObject)->phiBlk = simple;
 }
 

OndselSolver/ASMTSpatialContainer.cpp

@@ -20,7 +20,7 @@
 
 using namespace MbD;
 
-MbD::ASMTSpatialContainer::ASMTSpatialContainer()
+MbD::ASMTSpatialContainer::ASMTSpatialContainer() : ASMTSpatialItem()
 {
 	refPoints = std::make_shared<std::vector<std::shared_ptr<ASMTRefPoint>>>();
 	refCurves = std::make_shared<std::vector<std::shared_ptr<ASMTRefCurve>>>();
@@ -293,6 +293,14 @@ void MbD::ASMTSpatialContainer::createMbD(std::shared_ptr<System> mbdSys, std::s
 	}
 }
 
+void MbD::ASMTSpatialContainer::updateMbDFromPosition3D(FColDsptr vec)
+{
+	position3D = vec;
+	auto mbdPart = std::static_pointer_cast<Part>(mbdObject);
+	auto mbdUnits = this->mbdUnits();
+	mbdPart->qX(rOcmO()->times(1.0 / mbdUnits->length));
+}
+
 FColDsptr MbD::ASMTSpatialContainer::rOcmO()
 {
 	auto& rOPO = position3D;

OndselSolver/ASMTSpatialContainer.h

@@ -61,6 +61,7 @@ namespace MbD {
         void readAlphaYs(std::vector<std::string>& lines);
         void readAlphaZs(std::vector<std::string>& lines);
         void createMbD(std::shared_ptr<System> mbdSys, std::shared_ptr<Units> mbdUnits) override;
+        void updateMbDFromPosition3D(FColDsptr position3D);
         FColDsptr rOcmO();
         std::shared_ptr<EulerParameters<double>> qEp();
         virtual FColDsptr vOcmO();

OndselSolver/ASMTSpatialItem.cpp

@@ -14,6 +14,10 @@
 
 using namespace MbD;
 
+MbD::ASMTSpatialItem::ASMTSpatialItem() : ASMTItem()
+{
+}
+
 void MbD::ASMTSpatialItem::setPosition3D(FColDsptr vec)
 {
 	position3D = vec;

OndselSolver/ASMTSpatialItem.h

@@ -15,6 +15,7 @@ namespace MbD {
 	{
 		//
 	public:
+		ASMTSpatialItem();
 		void setPosition3D(FColDsptr position3D);
 		void setRotationMatrix(FMatDsptr rotationMatrix);
 		void readPosition3D(std::vector<std::string>& lines);

OndselSolver/Array.h

@@ -116,7 +116,7 @@ namespace MbD {
 	inline double Array<T>::maxMagnitudeOfVector()
 	{
 		double answer = 0.0;
-		for (int i = 0; i < (int)this->size(); i++)
+		for (int i = 0; i < this->size(); i++)
 		{
 			double mag = std::abs(this->at(i));
 			if (answer < mag) answer = mag;

OndselSolver/DirectionCosineIecJec.cpp

@@ -5,32 +5,31 @@
  *                                                                         *
  *   See LICENSE file for details about copyright.                         *
  ***************************************************************************/
- 
+
 #include <memory>
 
 #include "DirectionCosineIecJec.h"
 #include "EndFramec.h"
 
-namespace MbD {
-    DirectionCosineIecJec::DirectionCosineIecJec()
-    = default;
+using namespace MbD;
 
-    DirectionCosineIecJec::DirectionCosineIecJec(EndFrmsptr frmi, EndFrmsptr frmj, int axisi, int axisj) :
-            KinematicIeJe(frmi, frmj), axisI(axisi), axisJ(axisj)
-    {
+DirectionCosineIecJec::DirectionCosineIecJec()
+= default;
 
-    }
+DirectionCosineIecJec::DirectionCosineIecJec(EndFrmsptr frmi, EndFrmsptr frmj, int axisi, int axisj) :
+	KinematicIeJe(frmi, frmj), axisI(axisi), axisJ(axisj)
+{
 
-    void DirectionCosineIecJec::calcPostDynCorrectorIteration()
-    {
-        aAjOIe = frmI->aAjOe(axisI);
-        aAjOJe = frmJ->aAjOe(axisJ);
-        aAijIeJe = aAjOIe->dot(aAjOJe);
-    }
-
-    double MbD::DirectionCosineIecJec::value()
-    {
-        return aAijIeJe;
-    }
 }
 
+void DirectionCosineIecJec::calcPostDynCorrectorIteration()
+{
+	aAjOIe = frmI->aAjOe(axisI);
+	aAjOJe = frmJ->aAjOe(axisJ);
+	aAijIeJe = aAjOIe->dot(aAjOJe);
+}
+
+double MbD::DirectionCosineIecJec::value()
+{
+	return aAijIeJe;
+}

OndselSolver/ExternalSystem.cpp

@@ -26,6 +26,19 @@ void MbD::ExternalSystem::preMbDrun(std::shared_ptr<System> mbdSys)
 	}
 }
 
+void MbD::ExternalSystem::updateFromMbD()
+{
+	if (cadSystem) {
+		cadSystem->updateFromMbD();
+	}
+	else if (asmtAssembly) {
+		asmtAssembly->updateFromMbD();
+	}
+	else {
+		assert(false);
+	}
+}
+
 void MbD::ExternalSystem::outputFor(AnalysisType type)
 {
 	if (cadSystem) {

OndselSolver/ExternalSystem.h

@@ -25,6 +25,7 @@ namespace MbD {
 		//
 	public:
 		void preMbDrun(std::shared_ptr<System> mbdSys);
+		void updateFromMbD();
 		void outputFor(AnalysisType type);
 		void logString(std::string& str);
 		void logString(double value);

OndselSolver/OndselSolver.vcxproj

@@ -272,6 +272,7 @@
     <ClCompile Include="Negative.cpp" />
     <ClCompile Include="PiecewiseFunction.cpp" />
     <ClCompile Include="Polynomial.cpp" />
+    <ClCompile Include="PosICDragNewtonRaphson.cpp" />
     <ClCompile Include="PosVelAccData.cpp" />
     <ClCompile Include="DiagonalMatrix.cpp" />
     <ClCompile Include="DifferenceOperator.cpp" />
@@ -596,6 +597,7 @@
     <ClInclude Include="Negative.h" />
     <ClInclude Include="PiecewiseFunction.h" />
     <ClInclude Include="Polynomial.h" />
+    <ClInclude Include="PosICDragNewtonRaphson.h" />
     <ClInclude Include="PosVelAccData.h" />
     <ClInclude Include="DiagonalMatrix.h" />
     <ClInclude Include="DifferenceOperator.h" />

OndselSolver/OndselSolver.vcxproj.filters

@@ -975,6 +975,15 @@
     <ClCompile Include="Arguments.cpp">
       <Filter>Source Files</Filter>
     </ClCompile>
+    <ClCompile Include="ASMTRevRevJoint.cpp">
+      <Filter>Source Files</Filter>
+    </ClCompile>
+    <ClCompile Include="RevRevJoint.cpp">
+      <Filter>Source Files</Filter>
+    </ClCompile>
+    <ClCompile Include="PosICDragNewtonRaphson.cpp">
+      <Filter>Source Files</Filter>
+    </ClCompile>
   </ItemGroup>
   <ItemGroup>
     <ClInclude Include="Array.h">
@@ -1943,6 +1952,15 @@
     <ClInclude Include="Arguments.h">
       <Filter>Header Files</Filter>
     </ClInclude>
+    <ClInclude Include="ASMTRevRevJoint.h">
+      <Filter>Header Files</Filter>
+    </ClInclude>
+    <ClInclude Include="RevRevJoint.h">
+      <Filter>Header Files</Filter>
+    </ClInclude>
+    <ClInclude Include="PosICDragNewtonRaphson.h">
+      <Filter>Header Files</Filter>
+    </ClInclude>
   </ItemGroup>
   <ItemGroup>
     <ResourceCompile Include="OndselSolver.rc">

OndselSolver/PosICDragNewtonRaphson.cpp

@@ -0,0 +1,81 @@
+/***************************************************************************
+ *   Copyright (c) 2023 Ondsel, Inc.                                       *
+ *                                                                         *
+ *   This file is part of OndselSolver.                                    *
+ *                                                                         *
+ *   See LICENSE file for details about copyright.                         *
+ ***************************************************************************/
+
+#include "PosICDragNewtonRaphson.h"
+#include "SystemSolver.h"
+#include "Part.h"
+#include "Constraint.h"
+
+using namespace MbD;
+
+std::shared_ptr<PosICDragNewtonRaphson> MbD::PosICDragNewtonRaphson::With()
+{
+	auto newtonRaphson = std::make_shared<PosICDragNewtonRaphson>();
+	newtonRaphson->initialize();
+	return newtonRaphson;
+}
+
+void MbD::PosICDragNewtonRaphson::initializeGlobally()
+{
+	AnyPosICNewtonRaphson::initializeGlobally();
+	iterMax = system->iterMaxPosKine;
+	dxTol = system->errorTolPosKine;
+	for (int i = 0; i < qsuWeights->size(); i++)
+	{
+		qsuWeights->at(i) = 1.0e3;	//minimum weight
+	}
+	for (auto& part : *dragParts) {
+		auto iqX = part->iqX();
+		for (int i = 0; i < 3; i++)
+		{
+			qsuWeights->at((size_t)iqX + i) = 1.0e6;	//maximum weight
+		}
+	}
+}
+
+void MbD::PosICDragNewtonRaphson::assignEquationNumbers()
+{
+	auto parts = system->parts();
+	//auto contactEndFrames = system->contactEndFrames();
+	//auto uHolders = system->uHolders();
+	auto constraints = system->allConstraints();
+	int eqnNo = 0;
+	for (auto& part : *parts) {
+		part->iqX(eqnNo);
+		eqnNo = eqnNo + 3;
+		part->iqE(eqnNo);
+		eqnNo = eqnNo + 4;
+	}
+	//for (auto& endFrm : *contactEndFrames) {
+	//	endFrm->is(eqnNo);
+	//	eqnNo = eqnNo + endFrm->sSize();
+	//}
+	//for (auto& uHolder : *uHolders) {
+	//	uHolder->iu(eqnNo);
+	//	eqnNo += 1;
+	//}
+	auto nEqns = eqnNo;	//C++ uses index 0.
+	nqsu = nEqns;
+	for (auto& con : *constraints) {
+		con->iG = eqnNo;
+		eqnNo += 1;
+	}
+	//auto lastEqnNo = eqnNo - 1;
+	nEqns = eqnNo;	//C++ uses index 0.
+	n = nEqns;
+}
+
+bool MbD::PosICDragNewtonRaphson::isConverged()
+{
+	return dxNorms->at(iterNo) < dxTol || isConvergedToNumericalLimit();
+}
+
+void MbD::PosICDragNewtonRaphson::setdragParts(std::shared_ptr<std::vector<std::shared_ptr<Part>>> _dragParts)
+{
+	dragParts = _dragParts;
+}

OndselSolver/PosICDragNewtonRaphson.h

@@ -0,0 +1,29 @@
+/***************************************************************************
+ *   Copyright (c) 2023 Ondsel, Inc.                                       *
+ *                                                                         *
+ *   This file is part of OndselSolver.                                    *
+ *                                                                         *
+ *   See LICENSE file for details about copyright.                         *
+ ***************************************************************************/
+
+#pragma once
+
+#include "AnyPosICNewtonRaphson.h"
+
+namespace MbD {
+    class Part;
+
+    class PosICDragNewtonRaphson : public AnyPosICNewtonRaphson
+    {
+        //Kinematics with under constrained system
+    public:
+        static std::shared_ptr<PosICDragNewtonRaphson> With();
+        void initializeGlobally() override;
+        void assignEquationNumbers() override;
+        bool isConverged() override;
+        void setdragParts(std::shared_ptr<std::vector<std::shared_ptr<Part>>> dragParts);
+
+        std::shared_ptr<std::vector<std::shared_ptr<Part>>> dragParts;
+    };
+}
+

OndselSolver/System.cpp

@@ -104,6 +104,27 @@ void System::clear()
 	forcesTorques->clear();
 }
 
+void MbD::System::runPreDrag(std::shared_ptr<System> self)
+{
+	externalSystem->preMbDrun(self);
+	while (true)
+	{
+		initializeLocally();
+		initializeGlobally();
+		if (!hasChanged) break;
+	}
+	partsJointsMotionsForcesTorquesDo([](std::shared_ptr<Item> item) { item->postInput(); });
+	systemSolver->runPreDrag();
+	externalSystem->updateFromMbD();
+}
+
+void MbD::System::runDragStep(std::shared_ptr<std::vector<std::shared_ptr<Part>>> dragParts)
+{
+	partsJointsMotionsForcesTorquesDo([](std::shared_ptr<Item> item) { item->postInput(); });
+	systemSolver->runDragStep(dragParts);
+	externalSystem->updateFromMbD();
+}
+
 std::shared_ptr<std::vector<std::string>> System::discontinuitiesAtIC()
 {
 	return std::make_shared<std::vector<std::string>>();

OndselSolver/System.h

@@ -43,6 +43,8 @@ namespace MbD {
 		void initializeLocally() override;
 		void initializeGlobally() override;
 		void clear();
+		void runPreDrag(std::shared_ptr<System> self);
+		void runDragStep(std::shared_ptr<std::vector<std::shared_ptr<Part>>> dragParts);
 		void runKINEMATIC(std::shared_ptr<System> self);
 		std::shared_ptr<std::vector<std::string>> discontinuitiesAtIC();
 		void jointsMotionsDo(const std::function <void(std::shared_ptr<Joint>)>& f);

OndselSolver/SystemSolver.cpp

@@ -28,6 +28,7 @@
 #include "AccKineNewtonRaphson.h"
 #include "VelICKineSolver.h"
 #include "AccICKineNewtonRaphson.h"
+#include "PosICDragNewtonRaphson.h"
 
 using namespace MbD;
 
@@ -164,6 +165,18 @@ void SystemSolver::runBasicKinematic()
 	}
 }
 
+void SystemSolver::runPreDrag()
+{
+	initializeLocally();
+	initializeGlobally();
+	runPosIC();
+}
+
+void MbD::SystemSolver::runDragStep(std::shared_ptr<std::vector<std::shared_ptr<Part>>> dragParts)
+{
+	runPosICDrag(dragParts);
+}
+
 void SystemSolver::runQuasiKinematic()
 {
 	try {
@@ -197,6 +210,15 @@ void SystemSolver::runAccKine()
 	icTypeSolver->run();
 }
 
+void MbD::SystemSolver::runPosICDrag(std::shared_ptr<std::vector<std::shared_ptr<Part>>> dragParts)
+{
+	auto newtonRaphson = PosICDragNewtonRaphson::With();
+	newtonRaphson->setdragParts(dragParts);
+	icTypeSolver = newtonRaphson;
+	icTypeSolver->setSystem(this);
+	icTypeSolver->run();
+}
+
 void SystemSolver::runPosICKine()
 {
 	icTypeSolver = CREATE<PosICKineNewtonRaphson>::With();

OndselSolver/SystemSolver.h

@@ -48,10 +48,13 @@ namespace MbD {
 		void runCollisionDerivativeIC();
 		void runBasicCollision();
 		void runBasicKinematic();
+		void runPreDrag();
+		void runDragStep(std::shared_ptr<std::vector<std::shared_ptr<Part>>> dragParts);
 		void runQuasiKinematic();
 		void runPosKine();
 		void runVelKine();
 		void runAccKine();
+		void runPosICDrag(std::shared_ptr<std::vector<std::shared_ptr<Part>>> dragParts);
 		void runPosICKine();
 		void runVelICKine();
 		void runAccICKine();

testapp/OndselSolver.cpp

@@ -26,6 +26,8 @@ void sharedptrTest();
 
 int main()
 {
+	ASMTAssembly::runDraggingTest();
+	ASMTAssembly::runFile("../testapp/Schmidt_Coupling_Ass_1-1.asmt");
 	ASMTAssembly::runFile("../testapp/RevRevJt.asmt");
 	ASMTAssembly::runFile("../testapp/RevCylJt.asmt");
 	ASMTAssembly::runFile("../testapp/CylSphJt.asmt");